diff --git a/mCAD/cmdr-joystick-simon.FCStd b/mCAD/cmdr-joystick-simon.FCStd deleted file mode 100644 index f6b3e35..0000000 Binary files a/mCAD/cmdr-joystick-simon.FCStd and /dev/null differ diff --git a/teensylc/.gitignore b/teensylc/.gitignore deleted file mode 100644 index 050b399..0000000 --- a/teensylc/.gitignore +++ /dev/null @@ -1,4 +0,0 @@ -.pio -.vscode -.cache -compile_commands.json diff --git a/teensylc/.gitkeep b/teensylc/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/teensylc/Makefile b/teensylc/Makefile deleted file mode 100644 index 5121a38..0000000 --- a/teensylc/Makefile +++ /dev/null @@ -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 - diff --git a/teensylc/extra_script.py b/teensylc/extra_script.py deleted file mode 100644 index b09c47d..0000000 --- a/teensylc/extra_script.py +++ /dev/null @@ -1,5 +0,0 @@ -import os -Import("env") - -# include toolchain paths -env.Replace(COMPILATIONDB_INCLUDE_TOOLCHAIN=True) diff --git a/teensylc/include/.gitkeep b/teensylc/include/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/teensylc/lib/.gitkeep b/teensylc/lib/.gitkeep deleted file mode 100644 index e69de29..0000000 diff --git a/teensylc/lib/teensy-libc b/teensylc/lib/teensy-libc deleted file mode 160000 index 88cb7c1..0000000 --- a/teensylc/lib/teensy-libc +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 88cb7c10ca12ea33fb96aa10ae97a14d087da852 diff --git a/teensylc/platformio.ini b/teensylc/platformio.ini deleted file mode 100644 index b86a206..0000000 --- a/teensylc/platformio.ini +++ /dev/null @@ -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 diff --git a/teensylc/src/cmdr_joystick.cpp b/teensylc/src/cmdr_joystick.cpp deleted file mode 100644 index 16de500..0000000 --- a/teensylc/src/cmdr_joystick.cpp +++ /dev/null @@ -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 -#include -#include -#include -#include - -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; - } -}