Cleaned up files
This commit is contained in:
parent
e7ab5b52d1
commit
496d92fe84
Binary file not shown.
4
teensylc/.gitignore
vendored
4
teensylc/.gitignore
vendored
@ -1,4 +0,0 @@
|
|||||||
.pio
|
|
||||||
.vscode
|
|
||||||
.cache
|
|
||||||
compile_commands.json
|
|
||||||
@ -1,13 +0,0 @@
|
|||||||
# Uncomment lines below if you have problems with $PATH
|
|
||||||
#SHELL := /bin/bash
|
|
||||||
#PATH := /usr/local/bin:$(PATH)
|
|
||||||
|
|
||||||
all:
|
|
||||||
pio run && pio run -t compiledb
|
|
||||||
|
|
||||||
clean:
|
|
||||||
pio run -t clean
|
|
||||||
|
|
||||||
upload:
|
|
||||||
pio run -t upload
|
|
||||||
|
|
||||||
@ -1,5 +0,0 @@
|
|||||||
import os
|
|
||||||
Import("env")
|
|
||||||
|
|
||||||
# include toolchain paths
|
|
||||||
env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True)
|
|
||||||
@ -1 +0,0 @@
|
|||||||
Subproject commit 88cb7c10ca12ea33fb96aa10ae97a14d087da852
|
|
||||||
@ -1,18 +0,0 @@
|
|||||||
; PlatformIO Project Configuration File
|
|
||||||
;
|
|
||||||
; Build options: build flags, source filter
|
|
||||||
; Upload options: custom upload port, speed and extra flags
|
|
||||||
; Library options: dependencies, extra library storages
|
|
||||||
; Advanced options: extra scripting
|
|
||||||
;
|
|
||||||
; Please visit documentation for the other options and examples
|
|
||||||
; http://docs.platformio.org/page/projectconf.html
|
|
||||||
|
|
||||||
[env:teensylc]
|
|
||||||
platform = teensy
|
|
||||||
board = teensylc
|
|
||||||
framework = arduino
|
|
||||||
build_flags = -Isrc -Ilib -D USB_HID -D LAYOUT_SWEDISH -w
|
|
||||||
board_build.f_cpu = 48000000L
|
|
||||||
extra_scripts = pre:extra_script.py
|
|
||||||
upload_protocol = teensy-cli
|
|
||||||
@ -1,496 +0,0 @@
|
|||||||
/*
|
|
||||||
* Project: CMtec CMDR Joystick RC
|
|
||||||
* Date: 2023-09-02
|
|
||||||
* Author: Christoffer Martinsson
|
|
||||||
* Email: cm@cmtec.se
|
|
||||||
* License: Please refer to LICENSE file in root folder
|
|
||||||
*/
|
|
||||||
|
|
||||||
#include <Arduino.h>
|
|
||||||
#include <EEPROM.h>
|
|
||||||
#include <ElrsTx.h>
|
|
||||||
#include <IndicatorLed.h>
|
|
||||||
#include <ResponsiveAnalogRead.h>
|
|
||||||
|
|
||||||
IndicatorLed status_led(13);
|
|
||||||
IndicatorLed button_led_1(22);
|
|
||||||
IndicatorLed button_led_2(23);
|
|
||||||
|
|
||||||
const int TIME_US_1ms = 1000;
|
|
||||||
const int TIME_US_5ms = 5000;
|
|
||||||
const uint64_t TIME_US_200ms = 200000;
|
|
||||||
uint64_t current_timestamp_micros = 0;
|
|
||||||
uint64_t process_data_timestamp_micros = 0;
|
|
||||||
uint64_t indicator_timestamp_micros = 0;
|
|
||||||
uint64_t send_usb_timestamp_micros = 0;
|
|
||||||
uint64_t send_elrs_timestamp_micros = 0;
|
|
||||||
|
|
||||||
const int BUTTON_PRESSED = LOW;
|
|
||||||
const int BUTTON_NOT_PRESSED = HIGH;
|
|
||||||
const int BUTTON_FRONT_LEFT_UPPER_PIN = 4;
|
|
||||||
const int BUTTON_FRONT_LEFT_LOWER_PIN = 5;
|
|
||||||
const int BUTTON_FRONT_RIGHT_UPPER_PIN = 6;
|
|
||||||
const int BUTTON_FRONT_RIGHT_LOWER_PIN = 7;
|
|
||||||
const int BUTTON_TOP_LEFT_UPPER_PIN = 9;
|
|
||||||
const int BUTTON_TOP_RIGHT_UPPER_PIN = 10;
|
|
||||||
const int BUTTON_TOP_LEFT_LOWER_PIN = 8;
|
|
||||||
const int BUTTON_TOP_RIGHT_LOWER_PIN = 11;
|
|
||||||
|
|
||||||
const int GIMBAL_MODE_FRSKY_M7 = 0;
|
|
||||||
const int GIMBAL_MODE_FRSKY_M10 = 1;
|
|
||||||
const int GIMBAL_MODE_PIN = 21;
|
|
||||||
int gimbal_mode = GIMBAL_MODE_FRSKY_M7;
|
|
||||||
|
|
||||||
bool toggle_button_arm = false;
|
|
||||||
bool toggle_button_mode = false;
|
|
||||||
|
|
||||||
const int AXIS_10BIT_MAX = 1023;
|
|
||||||
const int AXIS_10BIT_MIN = 0;
|
|
||||||
const int AXIS_10BIT_CENTER = 512;
|
|
||||||
const int AXIS_12BIT_MAX = 4096;
|
|
||||||
const int AXIS_12BIT_MIN = 0;
|
|
||||||
const int AXIS_12BIT_CENTER = 2048;
|
|
||||||
const int DEADZONE_X = 50;
|
|
||||||
const int DEADZONE_Y = 50;
|
|
||||||
const int JOYSTICK_HAT_CENTER = -1;
|
|
||||||
|
|
||||||
enum EEPROM_ADR {
|
|
||||||
MAX_X1_ADR_HIGH,
|
|
||||||
MAX_X1_ADR_LOW,
|
|
||||||
MIN_X1_ADR_HIGH,
|
|
||||||
MIN_X1_ADR_LOW,
|
|
||||||
CNT_X1_ADR_HIGH,
|
|
||||||
CNT_X1_ADR_LOW,
|
|
||||||
MAX_Y1_ADR_HIGH,
|
|
||||||
MAX_Y1_ADR_LOW,
|
|
||||||
MIN_Y1_ADR_HIGH,
|
|
||||||
MIN_Y1_ADR_LOW,
|
|
||||||
CNT_Y1_ADR_HIGH,
|
|
||||||
CNT_Y1_ADR_LOW,
|
|
||||||
MAX_X2_ADR_HIGH,
|
|
||||||
MAX_X2_ADR_LOW,
|
|
||||||
MIN_X2_ADR_HIGH,
|
|
||||||
MIN_X2_ADR_LOW,
|
|
||||||
CNT_X2_ADR_HIGH,
|
|
||||||
CNT_X2_ADR_LOW,
|
|
||||||
MAX_Y2_ADR_HIGH,
|
|
||||||
MAX_Y2_ADR_LOW,
|
|
||||||
MIN_Y2_ADR_HIGH,
|
|
||||||
MIN_Y2_ADR_LOW,
|
|
||||||
CNT_Y2_ADR_HIGH,
|
|
||||||
CNT_Y2_ADR_LOW,
|
|
||||||
EEPROM_ADR_NBR_OF_BYTES
|
|
||||||
};
|
|
||||||
|
|
||||||
int joystick_counter = 0;
|
|
||||||
int joystick_x1_12bit = 0;
|
|
||||||
int joystick_x1_12bit_max = 4096;
|
|
||||||
int joystick_x1_12bit_min = 0;
|
|
||||||
int joystick_x1_12bit_center = joystick_x1_12bit_max / 2;
|
|
||||||
int joystick_y1_12bit = 0;
|
|
||||||
int joystick_y1_12bit_max = 4096;
|
|
||||||
int joystick_y1_12bit_min = 0;
|
|
||||||
int joystick_y1_12bit_center = joystick_y1_12bit_max / 2;
|
|
||||||
int joystick_x2_12bit = 0;
|
|
||||||
int joystick_x2_12bit_max = 4096;
|
|
||||||
int joystick_x2_12bit_min = 0;
|
|
||||||
int joystick_x2_12bit_center = joystick_x2_12bit_max / 2;
|
|
||||||
int joystick_y2_12bit = 0;
|
|
||||||
int joystick_y2_12bit_max = 4096;
|
|
||||||
int joystick_y2_12bit_min = 0;
|
|
||||||
int joystick_y2_12bit_center = joystick_y2_12bit_max / 2;
|
|
||||||
|
|
||||||
float exp_constant = 0.2;
|
|
||||||
|
|
||||||
int fn_mode = 0;
|
|
||||||
|
|
||||||
ResponsiveAnalogRead analog_x1(A0, true);
|
|
||||||
ResponsiveAnalogRead analog_y1(A1, true);
|
|
||||||
ResponsiveAnalogRead analog_x2(A3, true);
|
|
||||||
ResponsiveAnalogRead analog_y2(A2, true);
|
|
||||||
|
|
||||||
ElrsTx elrs(Serial1);
|
|
||||||
|
|
||||||
const int CALIBRATION_OFF = 0;
|
|
||||||
const int CALIBRATION_INIT = 1;
|
|
||||||
const int CALIBRATION_CENTER = 2;
|
|
||||||
const int CALIBRATION_MINMAX = 3;
|
|
||||||
int joystick_calibration_mode = CALIBRATION_OFF;
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Save calibration data to EEPROM
|
|
||||||
*/
|
|
||||||
void save_to_eeprom() {
|
|
||||||
EEPROM.write(MAX_X1_ADR_LOW, joystick_x1_12bit_max);
|
|
||||||
EEPROM.write(MAX_X1_ADR_HIGH, joystick_x1_12bit_max >> 8);
|
|
||||||
EEPROM.write(MIN_X1_ADR_LOW, joystick_x1_12bit_min);
|
|
||||||
EEPROM.write(MIN_X1_ADR_HIGH, joystick_x1_12bit_min >> 8);
|
|
||||||
EEPROM.write(CNT_X1_ADR_LOW, joystick_x1_12bit_center);
|
|
||||||
EEPROM.write(CNT_X1_ADR_HIGH, joystick_x1_12bit_center >> 8);
|
|
||||||
|
|
||||||
EEPROM.write(MAX_Y1_ADR_LOW, joystick_y1_12bit_max);
|
|
||||||
EEPROM.write(MAX_Y1_ADR_HIGH, joystick_y1_12bit_max >> 8);
|
|
||||||
EEPROM.write(MIN_Y1_ADR_LOW, joystick_y1_12bit_min);
|
|
||||||
EEPROM.write(MIN_Y1_ADR_HIGH, joystick_y1_12bit_min >> 8);
|
|
||||||
EEPROM.write(CNT_Y1_ADR_LOW, joystick_y1_12bit_center);
|
|
||||||
EEPROM.write(CNT_Y1_ADR_HIGH, joystick_y1_12bit_center >> 8);
|
|
||||||
|
|
||||||
EEPROM.write(MAX_X2_ADR_LOW, joystick_x2_12bit_max);
|
|
||||||
EEPROM.write(MAX_X2_ADR_HIGH, joystick_x2_12bit_max >> 8);
|
|
||||||
EEPROM.write(MIN_X2_ADR_LOW, joystick_x2_12bit_min);
|
|
||||||
EEPROM.write(MIN_X2_ADR_HIGH, joystick_x2_12bit_min >> 8);
|
|
||||||
EEPROM.write(CNT_X2_ADR_LOW, joystick_x2_12bit_center);
|
|
||||||
EEPROM.write(CNT_X2_ADR_HIGH, joystick_x2_12bit_center >> 8);
|
|
||||||
|
|
||||||
EEPROM.write(MAX_Y2_ADR_LOW, joystick_y2_12bit_max);
|
|
||||||
EEPROM.write(MAX_Y2_ADR_HIGH, joystick_y2_12bit_max >> 8);
|
|
||||||
EEPROM.write(MIN_Y2_ADR_LOW, joystick_y2_12bit_min);
|
|
||||||
EEPROM.write(MIN_Y2_ADR_HIGH, joystick_y2_12bit_min >> 8);
|
|
||||||
EEPROM.write(CNT_Y2_ADR_LOW, joystick_y2_12bit_center);
|
|
||||||
EEPROM.write(CNT_Y2_ADR_HIGH, joystick_y2_12bit_center >> 8);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Load calibration data from EEPROM
|
|
||||||
*/
|
|
||||||
void load_from_eeprom() {
|
|
||||||
joystick_x1_12bit_max = (EEPROM.read(MAX_X1_ADR_HIGH) << 8);
|
|
||||||
joystick_x1_12bit_max |= EEPROM.read(MAX_X1_ADR_LOW);
|
|
||||||
joystick_x1_12bit_min = (EEPROM.read(MIN_X1_ADR_HIGH) << 8);
|
|
||||||
joystick_x1_12bit_min |= EEPROM.read(MIN_X1_ADR_LOW);
|
|
||||||
joystick_x1_12bit_center = (EEPROM.read(CNT_X1_ADR_HIGH) << 8);
|
|
||||||
joystick_x1_12bit_center |= EEPROM.read(CNT_X1_ADR_LOW);
|
|
||||||
|
|
||||||
joystick_y1_12bit_max = (EEPROM.read(MAX_Y1_ADR_HIGH) << 8);
|
|
||||||
joystick_y1_12bit_max |= EEPROM.read(MAX_Y1_ADR_LOW);
|
|
||||||
joystick_y1_12bit_min = (EEPROM.read(MIN_Y1_ADR_HIGH) << 8);
|
|
||||||
joystick_y1_12bit_min |= EEPROM.read(MIN_Y1_ADR_LOW);
|
|
||||||
joystick_y1_12bit_center = (EEPROM.read(CNT_Y1_ADR_HIGH) << 8);
|
|
||||||
joystick_y1_12bit_center |= EEPROM.read(CNT_Y1_ADR_LOW);
|
|
||||||
|
|
||||||
joystick_x2_12bit_max = (EEPROM.read(MAX_X2_ADR_HIGH) << 8);
|
|
||||||
joystick_x2_12bit_max |= EEPROM.read(MAX_X2_ADR_LOW);
|
|
||||||
joystick_x2_12bit_min = (EEPROM.read(MIN_X2_ADR_HIGH) << 8);
|
|
||||||
joystick_x2_12bit_min |= EEPROM.read(MIN_X2_ADR_LOW);
|
|
||||||
joystick_x2_12bit_center = (EEPROM.read(CNT_X2_ADR_HIGH) << 8);
|
|
||||||
joystick_x2_12bit_center |= EEPROM.read(CNT_X2_ADR_LOW);
|
|
||||||
|
|
||||||
joystick_y2_12bit_max = (EEPROM.read(MAX_Y2_ADR_HIGH) << 8);
|
|
||||||
joystick_y2_12bit_max |= EEPROM.read(MAX_Y2_ADR_LOW);
|
|
||||||
joystick_y2_12bit_min = (EEPROM.read(MIN_Y2_ADR_HIGH) << 8);
|
|
||||||
joystick_y2_12bit_min |= EEPROM.read(MIN_Y2_ADR_LOW);
|
|
||||||
joystick_y2_12bit_center = (EEPROM.read(CNT_Y2_ADR_HIGH) << 8);
|
|
||||||
joystick_y2_12bit_center |= EEPROM.read(CNT_Y2_ADR_LOW);
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Calibrate all joystick axis
|
|
||||||
*
|
|
||||||
* @param analog_x1
|
|
||||||
* @param analog_y1
|
|
||||||
* @param analog_x2
|
|
||||||
* @param analog_y2
|
|
||||||
*/
|
|
||||||
void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) {
|
|
||||||
joystick_x1_12bit = AXIS_12BIT_CENTER;
|
|
||||||
joystick_y1_12bit = AXIS_12BIT_CENTER;
|
|
||||||
joystick_x2_12bit = AXIS_12BIT_CENTER;
|
|
||||||
joystick_y2_12bit = AXIS_12BIT_CENTER;
|
|
||||||
|
|
||||||
// Check for calibration mode
|
|
||||||
if (joystick_calibration_mode == CALIBRATION_INIT && digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_NOT_PRESSED) {
|
|
||||||
joystick_calibration_mode = CALIBRATION_CENTER;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Calibrate joystick center values
|
|
||||||
else if (joystick_calibration_mode == CALIBRATION_CENTER) {
|
|
||||||
joystick_x1_12bit_center = analog_x1;
|
|
||||||
joystick_y1_12bit_center = analog_y1;
|
|
||||||
joystick_x1_12bit_max = joystick_x1_12bit_center;
|
|
||||||
joystick_x1_12bit_min = joystick_x1_12bit_center;
|
|
||||||
joystick_y1_12bit_max = joystick_y1_12bit_center;
|
|
||||||
joystick_y1_12bit_min = joystick_y1_12bit_center;
|
|
||||||
|
|
||||||
joystick_x2_12bit_center = analog_x2;
|
|
||||||
joystick_y2_12bit_center = analog_y2;
|
|
||||||
joystick_x2_12bit_max = joystick_x2_12bit_center;
|
|
||||||
joystick_x2_12bit_min = joystick_x2_12bit_center;
|
|
||||||
joystick_y2_12bit_max = joystick_y2_12bit_center;
|
|
||||||
joystick_y2_12bit_min = joystick_y2_12bit_center;
|
|
||||||
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
joystick_calibration_mode = CALIBRATION_MINMAX;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
// Calibrate joystick min/max values
|
|
||||||
else if (joystick_calibration_mode == CALIBRATION_MINMAX) {
|
|
||||||
if (analog_x1 > joystick_x1_12bit_max) joystick_x1_12bit_max = analog_x1;
|
|
||||||
if (analog_x1 < joystick_x1_12bit_min) joystick_x1_12bit_min = analog_x1;
|
|
||||||
if (analog_y1 > joystick_y1_12bit_max) joystick_y1_12bit_max = analog_y1;
|
|
||||||
if (analog_y1 < joystick_y1_12bit_min) joystick_y1_12bit_min = analog_y1;
|
|
||||||
if (analog_x2 > joystick_x2_12bit_max) joystick_x2_12bit_max = analog_x2;
|
|
||||||
if (analog_x2 < joystick_x2_12bit_min) joystick_x2_12bit_min = analog_x2;
|
|
||||||
if (analog_y2 > joystick_y2_12bit_max) joystick_y2_12bit_max = analog_y2;
|
|
||||||
if (analog_y2 < joystick_y2_12bit_min) joystick_y2_12bit_min = analog_y2;
|
|
||||||
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
joystick_calibration_mode = CALIBRATION_OFF;
|
|
||||||
save_to_eeprom();
|
|
||||||
}
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Save joystick calibration values to EEPROM
|
|
||||||
*/
|
|
||||||
void send_elrs_data() {
|
|
||||||
// Set ELRS analog channels
|
|
||||||
elrs.set_data(map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 0);
|
|
||||||
elrs.set_data(map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 1);
|
|
||||||
elrs.set_data(map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 2);
|
|
||||||
elrs.set_data(map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX), 3);
|
|
||||||
|
|
||||||
// Set ELRS digital channels
|
|
||||||
for (int i = 4; i < CRSF_MAX_CHANNEL; i++) {
|
|
||||||
elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (toggle_button_arm) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 4);
|
|
||||||
if (toggle_button_mode) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 5);
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 6);
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 7);
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8);
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 9);
|
|
||||||
|
|
||||||
// Send ELRS data
|
|
||||||
elrs.send_data();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* Send USB data to PC
|
|
||||||
*/
|
|
||||||
void send_usb_data() {
|
|
||||||
// Set USB analog channels
|
|
||||||
int joystick_x1_10bit = map(joystick_x1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
|
|
||||||
int joystick_y1_10bit = map(joystick_y1_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
|
|
||||||
int joystick_x2_10bit = map(joystick_x2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
|
|
||||||
int joystick_y2_10bit = map(joystick_y2_12bit, AXIS_12BIT_MIN, AXIS_12BIT_MAX, AXIS_10BIT_MIN, AXIS_10BIT_MAX);
|
|
||||||
|
|
||||||
Joystick.Zrotate(joystick_x1_10bit);
|
|
||||||
Joystick.Z(joystick_y1_10bit);
|
|
||||||
Joystick.X(joystick_x2_10bit);
|
|
||||||
Joystick.Y(joystick_y2_10bit);
|
|
||||||
Joystick.sliderRight(AXIS_10BIT_CENTER);
|
|
||||||
Joystick.sliderLeft(AXIS_10BIT_CENTER);
|
|
||||||
|
|
||||||
// Set USB digital channels
|
|
||||||
for (int i = 1; i < 32; i++) {
|
|
||||||
Joystick.button(i, 0);
|
|
||||||
}
|
|
||||||
|
|
||||||
Joystick.hat(JOYSTICK_HAT_CENTER);
|
|
||||||
|
|
||||||
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 1);
|
|
||||||
if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1);
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(4, 1);
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(5, 1);
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(6, 1);
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(7, 1);
|
|
||||||
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(1, 1);
|
|
||||||
if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(2, 1);
|
|
||||||
|
|
||||||
Joystick.send_now();
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Apply calibration to 12-bit gimbal value
|
|
||||||
*
|
|
||||||
* @param gimbal_value
|
|
||||||
* @param min_value
|
|
||||||
* @param max_value
|
|
||||||
* @param center_value
|
|
||||||
* @param deadband_value
|
|
||||||
* @param expo_value
|
|
||||||
*/
|
|
||||||
int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int center_value, int deadband_value, int expo_value) {
|
|
||||||
int calibrated_value = AXIS_12BIT_CENTER;
|
|
||||||
|
|
||||||
if (gimbal_value > (center_value + deadband_value)) {
|
|
||||||
calibrated_value = constrain(map(gimbal_value, (center_value + deadband_value), max_value, AXIS_12BIT_CENTER, AXIS_12BIT_MAX),
|
|
||||||
AXIS_12BIT_CENTER, AXIS_12BIT_MAX);
|
|
||||||
} else if (gimbal_value < (center_value - deadband_value)) {
|
|
||||||
calibrated_value = constrain(map(gimbal_value, min_value, (center_value - deadband_value), AXIS_12BIT_MIN, AXIS_12BIT_CENTER),
|
|
||||||
AXIS_12BIT_MIN, AXIS_12BIT_CENTER);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (expo_value != 0) {
|
|
||||||
float joystick_x_float = calibrated_value / float(AXIS_12BIT_MAX);
|
|
||||||
/* Calculate expo using 9th order polynomial function with 0.5 as center point */
|
|
||||||
float joystick_x_exp = expo_value * (0.5 + 256 * pow((joystick_x_float - 0.5), 9)) + (1 - expo_value) * joystick_x_float;
|
|
||||||
calibrated_value = constrain(int(joystick_x_exp * float(AXIS_12BIT_MAX)), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
}
|
|
||||||
|
|
||||||
return calibrated_value;
|
|
||||||
}
|
|
||||||
|
|
||||||
/**
|
|
||||||
* @brief Process input data from gimbal and buttons
|
|
||||||
*/
|
|
||||||
void process_input_data() {
|
|
||||||
int analog_x1_gimbal_value = 0;
|
|
||||||
int analog_y1_gimbal_value = 0;
|
|
||||||
int analog_x2_gimbal_value = 0;
|
|
||||||
int analog_y2_gimbal_value = 0;
|
|
||||||
|
|
||||||
if (gimbal_mode == GIMBAL_MODE_FRSKY_M10) {
|
|
||||||
analog_x1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_y1_gimbal_value = constrain(analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_x2_gimbal_value = constrain(analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_y2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
} else if (gimbal_mode == GIMBAL_MODE_FRSKY_M7) {
|
|
||||||
analog_x1_gimbal_value = constrain(analog_x1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_y1_gimbal_value = constrain(AXIS_12BIT_MAX - analog_y1.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_x2_gimbal_value = constrain(AXIS_12BIT_MAX - analog_x2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
analog_y2_gimbal_value = constrain(analog_y2.getValue(), AXIS_12BIT_MIN, AXIS_12BIT_MAX);
|
|
||||||
}
|
|
||||||
|
|
||||||
if (joystick_calibration_mode != CALIBRATION_OFF) {
|
|
||||||
calibrate_axis(analog_x1_gimbal_value, analog_y1_gimbal_value, analog_x2_gimbal_value, analog_y2_gimbal_value);
|
|
||||||
return;
|
|
||||||
}
|
|
||||||
|
|
||||||
joystick_x1_12bit = apply_calibration_12bit(analog_x1_gimbal_value, joystick_x1_12bit_min, joystick_x1_12bit_max,
|
|
||||||
joystick_x1_12bit_center, DEADZONE_X, exp_constant);
|
|
||||||
joystick_y1_12bit = apply_calibration_12bit(analog_y1_gimbal_value, joystick_y1_12bit_min, joystick_y1_12bit_max,
|
|
||||||
joystick_y1_12bit_center, DEADZONE_Y, 0);
|
|
||||||
joystick_x2_12bit = apply_calibration_12bit(analog_x2_gimbal_value, joystick_x2_12bit_min, joystick_x2_12bit_max,
|
|
||||||
joystick_x2_12bit_center, DEADZONE_X, exp_constant);
|
|
||||||
joystick_y2_12bit = apply_calibration_12bit(analog_y2_gimbal_value, joystick_y2_12bit_min, joystick_y2_12bit_max,
|
|
||||||
joystick_y2_12bit_center, DEADZONE_Y, exp_constant);
|
|
||||||
|
|
||||||
// Check toggle mode buttons
|
|
||||||
if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) {
|
|
||||||
toggle_button_mode = true;
|
|
||||||
} else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
toggle_button_mode = false;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check toggle arm button
|
|
||||||
if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) {
|
|
||||||
toggle_button_arm = true;
|
|
||||||
} else if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
toggle_button_arm = false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
void setup() {
|
|
||||||
/* Init HW */
|
|
||||||
status_led.begin();
|
|
||||||
status_led.blink();
|
|
||||||
button_led_1.begin();
|
|
||||||
button_led_2.begin();
|
|
||||||
|
|
||||||
pinMode(BUTTON_FRONT_LEFT_LOWER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_FRONT_LEFT_UPPER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_FRONT_RIGHT_LOWER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_FRONT_RIGHT_UPPER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_TOP_LEFT_LOWER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_TOP_RIGHT_LOWER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_TOP_LEFT_UPPER_PIN, INPUT_PULLUP);
|
|
||||||
pinMode(BUTTON_TOP_RIGHT_UPPER_PIN, INPUT_PULLUP);
|
|
||||||
|
|
||||||
pinMode(GIMBAL_MODE_PIN, INPUT_PULLUP);
|
|
||||||
|
|
||||||
// Set ADC resolution to 12bit
|
|
||||||
analogReadResolution(12);
|
|
||||||
analogReadAveraging(32);
|
|
||||||
delay(500);
|
|
||||||
|
|
||||||
// Set analog (lib) resolution to 12bit
|
|
||||||
analog_x1.setAnalogResolution(4096);
|
|
||||||
analog_y1.setAnalogResolution(4096);
|
|
||||||
analog_x2.setAnalogResolution(4096);
|
|
||||||
analog_y2.setAnalogResolution(4096);
|
|
||||||
|
|
||||||
// Init EEPROM
|
|
||||||
load_from_eeprom();
|
|
||||||
|
|
||||||
// Init Joystick
|
|
||||||
Joystick.useManualSend(true);
|
|
||||||
|
|
||||||
// Check if calibration mode is enabled
|
|
||||||
if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
joystick_calibration_mode = CALIBRATION_INIT;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check if bootloader mode is enabled
|
|
||||||
if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) {
|
|
||||||
button_led_2.on();
|
|
||||||
button_led_2.update();
|
|
||||||
delay(200);
|
|
||||||
_reboot_Teensyduino_();
|
|
||||||
}
|
|
||||||
|
|
||||||
// Check what gimbal mode is selected
|
|
||||||
if (digitalRead(GIMBAL_MODE_PIN) == LOW) {
|
|
||||||
gimbal_mode = GIMBAL_MODE_FRSKY_M10;
|
|
||||||
}
|
|
||||||
|
|
||||||
// Init ELRS
|
|
||||||
elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW);
|
|
||||||
}
|
|
||||||
|
|
||||||
void loop() {
|
|
||||||
current_timestamp_micros = micros();
|
|
||||||
|
|
||||||
analog_x1.update();
|
|
||||||
analog_y1.update();
|
|
||||||
analog_x2.update();
|
|
||||||
analog_y2.update();
|
|
||||||
|
|
||||||
/* Process data with 1ms interval*/
|
|
||||||
if (current_timestamp_micros >= process_data_timestamp_micros) {
|
|
||||||
process_input_data();
|
|
||||||
process_data_timestamp_micros = current_timestamp_micros + TIME_US_1ms;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Update/Send USB data with 5ms interval*/
|
|
||||||
if (current_timestamp_micros >= send_usb_timestamp_micros) {
|
|
||||||
send_usb_data();
|
|
||||||
send_usb_timestamp_micros = current_timestamp_micros + TIME_US_5ms;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Update/Send ERLS data with about 1,6ms interval */
|
|
||||||
if (current_timestamp_micros >= send_elrs_timestamp_micros) {
|
|
||||||
send_elrs_data();
|
|
||||||
send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US;
|
|
||||||
}
|
|
||||||
|
|
||||||
/* Update indicator with 200ms interval */
|
|
||||||
if (current_timestamp_micros >= indicator_timestamp_micros) {
|
|
||||||
button_led_1.off();
|
|
||||||
button_led_2.off();
|
|
||||||
|
|
||||||
if (joystick_calibration_mode == CALIBRATION_INIT) {
|
|
||||||
button_led_1.blink();
|
|
||||||
button_led_2.blink();
|
|
||||||
} else if (joystick_calibration_mode == CALIBRATION_CENTER) {
|
|
||||||
button_led_1.blink();
|
|
||||||
button_led_2.off();
|
|
||||||
} else if (joystick_calibration_mode == CALIBRATION_MINMAX) {
|
|
||||||
button_led_1.off();
|
|
||||||
button_led_2.blink();
|
|
||||||
} else if ((joystick_x1_12bit != AXIS_12BIT_CENTER) || (joystick_y1_12bit != AXIS_12BIT_MIN) ||
|
|
||||||
(joystick_x2_12bit != AXIS_12BIT_CENTER) || (joystick_y2_12bit != AXIS_12BIT_CENTER)) {
|
|
||||||
button_led_1.on();
|
|
||||||
button_led_2.on();
|
|
||||||
}
|
|
||||||
|
|
||||||
status_led.update();
|
|
||||||
button_led_1.update();
|
|
||||||
button_led_2.update();
|
|
||||||
|
|
||||||
indicator_timestamp_micros = current_timestamp_micros + TIME_US_200ms;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
Loading…
x
Reference in New Issue
Block a user