/* * 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; int toggle_button_arm_previous_value = HIGH; 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_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 6); if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 7); if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) elrs.set_data(CRSF_DIGITAL_CHANNEL_MAX, 8); // 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); if (fn_mode == 2) { Joystick.X(AXIS_10BIT_CENTER); Joystick.Y(AXIS_10BIT_CENTER); Joystick.sliderRight(joystick_x2_10bit); Joystick.sliderLeft(joystick_y2_10bit); } else if (fn_mode == 1) { Joystick.X(AXIS_10BIT_CENTER); Joystick.Y(joystick_y2_10bit); Joystick.sliderRight(joystick_x2_10bit); Joystick.sliderLeft(AXIS_10BIT_CENTER); } else { 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 (fn_mode == 2) { if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(12, 1); if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(13, 1); if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(14, 1); if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(15, 1); if (digitalRead(BUTTON_FRONT_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(16, 1); if (digitalRead(BUTTON_FRONT_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(17, 1); } else if (fn_mode == 1) { if (digitalRead(BUTTON_TOP_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(8, 1); if (digitalRead(BUTTON_TOP_RIGHT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(9, 1); if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(10, 1); if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) Joystick.button(11, 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); } else { if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) Joystick.button(3, 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 fn mode fn_mode = 0; if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { fn_mode = 1; if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { fn_mode = 2; } } // 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 ((fn_mode == 1) && (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) != toggle_button_arm_previous_value)) { if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == false)) { toggle_button_arm = true; } else if ((digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) && (toggle_button_arm == true)) { toggle_button_arm = false; } toggle_button_arm_previous_value = digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN); } } 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; } }