From 3f0f19cfde605d517e7bb34aac7bf9d2fa18948c Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Mon, 15 May 2023 15:33:37 +0000 Subject: [PATCH] Code cleanup --- firmware/src/main.cpp | 535 ++++++++++++++++++++++-------------------- 1 file changed, 276 insertions(+), 259 deletions(-) diff --git a/firmware/src/main.cpp b/firmware/src/main.cpp index 123f8f0..ee07593 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/main.cpp @@ -150,12 +150,12 @@ bool button_led_2_on = false; int button_led_2_mode = 0; unsigned long current_timestamp = 0; -unsigned long button_timestamp = 0; -unsigned long joystick_timestamp = 0; +unsigned long led_timestamp = 0; unsigned long indicator_timestamp = 0; +unsigned long send_usb_timestamp = 0; unsigned long current_timestamp_micros = 0; -unsigned long elrs_timestamp_micros = 0; +unsigned long send_elrs_timestamp_micros = 0; const int BUTTON_FLU = 4; // Front left upper button const int BUTTON_FLD = 5; // Front left lower button @@ -166,9 +166,9 @@ const int BUTTON_TRU = 10; // Top upper right button const int BUTTON_TLD = 8; // Top left lower button const int BUTTON_TRD = 11; // Top right lower button -bool toggle_button_10 = false; -bool toggle_button_11 = false; -int button_10_previous_value = HIGH; +bool toggle_button_arm = false; +bool toggle_button_mode = false; +int button_arm_previous_value = HIGH; int button_11_previous_value = HIGH; #define HID_AXIS_MAX 1023 @@ -180,14 +180,6 @@ int button_11_previous_value = HIGH; #define DEADZONE_X 50 #define DEADZONE_Y 50 -// USB HID data variables -int xy_x1 = HID_AXIS_CENTER; -int xy_y1 = HID_AXIS_CENTER; -int xy_x2 = HID_AXIS_CENTER; -int xy_y2 = HID_AXIS_CENTER; -int xy_x3 = HID_AXIS_CENTER; -int xy_y3 = HID_AXIS_CENTER; - enum EEPROM_ADR { MAX_X1_ADR_HIGH, @@ -219,21 +211,27 @@ enum EEPROM_ADR int joystick_counter = 0; int joystick_x1 = 0; +int joystick_x1_hid = 0; int joystick_x1_raw = 0; int joystick_x1_max = 4096; int joystick_x1_min = 0; int joystick_x1_center = joystick_x1_max/2; int joystick_y1 = 0; +int joystick_y1_hid = 0; +int joystick_y1_hid_rev = 0; +int joystick_y1_hid_rev_mode = 0; int joystick_y1_raw = 0; int joystick_y1_max = 4096; int joystick_y1_min = 0; int joystick_y1_center = joystick_y1_max/2; int joystick_x2 = 0; +int joystick_x2_hid = 0; int joystick_x2_raw = 0; int joystick_x2_max = 4096; int joystick_x2_min = 0; int joystick_x2_center = joystick_x2_max/2; int joystick_y2 = 0; +int joystick_y2_hid = 0; int joystick_y2_raw = 0; int joystick_y2_max = 4096; int joystick_y2_min = 0; @@ -241,6 +239,8 @@ int joystick_y2_center = joystick_y2_max/2; float exp_constant = 0.2; +int fn_mode = 0; + #define CALIBRATION_OFF 0 #define CALIBRATION_INIT 1 #define CALIBRATION_CENTER 2 @@ -309,194 +309,6 @@ void load_from_eeprom(){ joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW); } -void process_io_data(){ - - // Check for calibration mode - if (joystick_calibration_mode == CALIBRATION_INIT){ - if (digitalRead(BUTTON_TLD) == HIGH){ - joystick_calibration_mode = CALIBRATION_CENTER; - } - return; - } - if (joystick_calibration_mode == CALIBRATION_CENTER){ - button_led_1_mode = 2; - button_led_2_mode = 0; - if (digitalRead(BUTTON_TLD) == LOW){ - joystick_calibration_mode = CALIBRATION_MINMAX; - button_led_1_mode = 0; - } - return; - } - if (joystick_calibration_mode == CALIBRATION_MINMAX){ - button_led_1_mode = 0; - button_led_2_mode = 2; - if (digitalRead(BUTTON_TRD) == LOW){ - joystick_calibration_mode = CALIBRATION_OFF; - save_to_eeprom(); - button_led_2_mode = 0; - } - return; - } - - // Check fn mode - int fn_mode = 0; - if (digitalRead(BUTTON_TLU) == LOW){ - fn_mode = 1; - if (digitalRead(BUTTON_TRU) == LOW){ - fn_mode = 2; - } - } - - // Check toggle buttons - if (fn_mode == 1){ - if (digitalRead(BUTTON_TLD) != button_10_previous_value){ - button_10_previous_value = digitalRead(BUTTON_TLD); - if (digitalRead(BUTTON_TLD) == LOW){ - if (toggle_button_10 == false){ - toggle_button_10 = true; - button_led_1_mode = 1; - } - else{ - toggle_button_10 = false; - button_led_1_mode = 0; - } - } - } - if (digitalRead(BUTTON_TRD) != button_11_previous_value){ - button_11_previous_value = digitalRead(BUTTON_TRD); - if (digitalRead(BUTTON_TRD) == LOW){ - if (toggle_button_11 == false){ - toggle_button_11 = true; - button_led_2_mode = 1; - } - else{ - toggle_button_11 = false; - button_led_2_mode = 0; - } - } - } - } - - // Set joystick buttons to released - for (int i = 1; i < 32; i++){ - Joystick.button(i, 0); - } - - // Set joystick hat to center position - Joystick.hat(-1); - - // Set joystick buttons to pressed - if (fn_mode == 2){ - - if (digitalRead(BUTTON_FLD) == LOW){ - Joystick.button(12, 1); - } - if (digitalRead(BUTTON_FRD) == LOW){ - Joystick.button(13, 1); - } - if (digitalRead(BUTTON_TLD) == LOW){ - Joystick.button(14, 1); - } - if (digitalRead(BUTTON_TRD) == LOW){ - Joystick.button(15, 1); - } - } - else if (fn_mode == 1){ - - if (digitalRead(BUTTON_FLD) == LOW){ - Joystick.button(8, 1); - } - if (digitalRead(BUTTON_FRD) == LOW){ - Joystick.button(9, 1); - } - } - else{ - if (digitalRead(BUTTON_FLD) == LOW){ - Joystick.button(2, 1); - } - if (digitalRead(BUTTON_FRD) == LOW){ - Joystick.button(4, 1); - } - if (digitalRead(BUTTON_TRU) == LOW){ - Joystick.button(5, 1); - } - if (digitalRead(BUTTON_TLD) == LOW){ - Joystick.button(6, 1); - } - if (digitalRead(BUTTON_TRD) == LOW){ - Joystick.button(7, 1); - } - } - - if (digitalRead(BUTTON_FLU) == LOW){ - Joystick.button(1, 1); - } - if (digitalRead(BUTTON_FRU) == LOW){ - Joystick.button(3, 1); - } - if (toggle_button_10){ - Joystick.button(10, 1); - } - if (toggle_button_11){ - Joystick.button(11, 1); - } - - // Set axis values - Joystick.X(joystick_x1); - Joystick.Y(joystick_y1); - - if (fn_mode == 2){ - Joystick.Z(HID_AXIS_CENTER); - Joystick.Zrotate(HID_AXIS_CENTER); - Joystick.sliderLeft(joystick_x2); - Joystick.sliderRight(joystick_y2); - } - else if (fn_mode == 1){ - Joystick.Z(HID_AXIS_CENTER); - Joystick.Zrotate(joystick_y2); - Joystick.sliderLeft(joystick_x2); - Joystick.sliderRight(HID_AXIS_CENTER); - } - else{ - Joystick.Z(joystick_x2); - Joystick.Zrotate(joystick_y2); - Joystick.sliderLeft(HID_AXIS_CENTER); - Joystick.sliderRight(HID_AXIS_CENTER); - } - - // Set ELRS analog channels - elrs_channels[0] = map(joystick_x1, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); - elrs_channels[1] = map(joystick_y1, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); - elrs_channels[2] = map(joystick_x2, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); - elrs_channels[3] = map(joystick_y2, HID_AXIS_MIN, HID_AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); - - // Set ELRS digital channels - for (int i = 4; i < CRSF_MAX_CHANNEL; i++){ - elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN; - } - if (toggle_button_10){ - elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (toggle_button_11){ - elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (digitalRead(BUTTON_FLU) == LOW){ - elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (digitalRead(BUTTON_FLD) == LOW){ - elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (digitalRead(BUTTON_FRU) == LOW){ - elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (digitalRead(BUTTON_FRD) == LOW){ - elrs_channels[9] = CRSF_DIGITAL_CHANNEL_MAX; - } - if (digitalRead(BUTTON_TRU) == LOW){ - elrs_channels[10] = CRSF_DIGITAL_CHANNEL_MAX; - } -} - void read_io_data(){ if(joystick_counter == 0){ @@ -529,14 +341,11 @@ void read_io_data(){ } // Calculate Y1 joystick values - if(joystick_y1_raw > (joystick_y1_center + DEADZONE_Y)){ - joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_center + DEADZONE_Y), joystick_y1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); - } - else if(joystick_y1_raw < (joystick_y1_center - DEADZONE_Y)){ - joystick_y1 = constrain(map(joystick_y1_raw, joystick_y1_min, (joystick_y1_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); + if(joystick_y1_raw > (joystick_y1_min + DEADZONE_Y)){ + joystick_y1 = constrain(map(joystick_y1_raw, (joystick_y1_min + DEADZONE_Y), joystick_y1_max, AXIS_MIN, AXIS_MAX), AXIS_MIN, AXIS_MAX); } else{ - joystick_y1 = AXIS_CENTER; + joystick_y1 = AXIS_MIN; } // Calculate X2 joystick values @@ -565,56 +374,90 @@ void read_io_data(){ if (joystick_x1 != AXIS_CENTER){ float joystick_x_float = joystick_x1 / float(AXIS_MAX); float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float; - joystick_x1 = int(joystick_x_exp * float(HID_AXIS_MAX)); - joystick_x1 = constrain(joystick_x1, HID_AXIS_MIN, HID_AXIS_MAX); + joystick_x1_hid = int(joystick_x_exp * float(HID_AXIS_MAX)); + joystick_x1_hid = constrain(joystick_x1_hid, HID_AXIS_MIN, HID_AXIS_MAX); } else{ - joystick_x1 = HID_AXIS_CENTER; + joystick_x1_hid = HID_AXIS_CENTER; } - // Calculate new Y1 values after applying exp curve - if (joystick_y1 != AXIS_CENTER){ - float joystick_y_float = joystick_y1 / float(AXIS_MAX); - float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float; - joystick_y1 = int(joystick_y_exp * float(HID_AXIS_MAX)); - joystick_y1 = constrain(joystick_y1, HID_AXIS_MIN, HID_AXIS_MAX); - } - else{ - joystick_y1 = HID_AXIS_CENTER; - } + // Calculate new Y1 values + joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_CENTER, HID_AXIS_MAX); + joystick_y1_hid_rev = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_CENTER, HID_AXIS_MIN); // Calculate new X2 values after applying exp curve if (joystick_x2 != AXIS_CENTER){ float joystick_x_float = joystick_x2 / float(AXIS_MAX); float joystick_x_exp = exp_constant * (0.5 + 256 * pow((joystick_x_float - 0.5),9)) + (1 - exp_constant) * joystick_x_float; - joystick_x2 = int(joystick_x_exp * float(HID_AXIS_MAX)); - joystick_x2 = constrain(joystick_x2, HID_AXIS_MIN, HID_AXIS_MAX); + joystick_x2_hid = int(joystick_x_exp * float(HID_AXIS_MAX)); + joystick_x2_hid = constrain(joystick_x2_hid, HID_AXIS_MIN, HID_AXIS_MAX); } else{ - joystick_x2 = HID_AXIS_CENTER; + joystick_x2_hid = HID_AXIS_CENTER; } // Calculate new Y2 values after applying exp curve if (joystick_y2 != AXIS_CENTER){ float joystick_y_float = joystick_y2 / float(AXIS_MAX); float joystick_y_exp = exp_constant * (0.5 + 256 * pow((joystick_y_float - 0.5),9)) + (1 - exp_constant) * joystick_y_float; - joystick_y2 = int(joystick_y_exp * float(HID_AXIS_MAX)); - joystick_y2 = constrain(joystick_y2, HID_AXIS_MIN, HID_AXIS_MAX); + joystick_y2_hid = int(joystick_y_exp * float(HID_AXIS_MAX)); + joystick_y2_hid = constrain(joystick_y2_hid, HID_AXIS_MIN, HID_AXIS_MAX); } else{ - joystick_y2 = HID_AXIS_CENTER; + joystick_y2_hid = HID_AXIS_CENTER; + } + + // Check fn mode + fn_mode = 0; + if (digitalRead(BUTTON_TLU) == LOW){ + fn_mode = 1; + if (digitalRead(BUTTON_TRU) == LOW){ + fn_mode = 2; + } + } + + // Check toggle buttons + if (digitalRead(BUTTON_FLU) == LOW){ + toggle_button_mode = true; + } + else if (digitalRead(BUTTON_FLD) == LOW){ + toggle_button_mode = false; + } + + if (fn_mode == 2){ + if (digitalRead(BUTTON_TRD) != button_arm_previous_value){ + button_arm_previous_value = digitalRead(BUTTON_TRD); + if (digitalRead(BUTTON_TRD) == LOW){ + if (toggle_button_arm == false){ + toggle_button_arm = true; + } + else{ + toggle_button_arm = false; + } + } + } } } else{ // Calibration mode. - joystick_x1 = HID_AXIS_CENTER; - joystick_y1 = HID_AXIS_CENTER; - joystick_x2 = HID_AXIS_CENTER; - joystick_y2 = HID_AXIS_CENTER; + joystick_x1 = AXIS_CENTER; + joystick_y1 = AXIS_CENTER; + joystick_x2 = AXIS_CENTER; + joystick_y2 = AXIS_CENTER; + joystick_x1_hid = HID_AXIS_CENTER; + joystick_y1_hid = HID_AXIS_CENTER; + joystick_x2_hid = HID_AXIS_CENTER; + joystick_y2_hid = HID_AXIS_CENTER; + // Check for calibration mode + if (joystick_calibration_mode == CALIBRATION_INIT){ + if (digitalRead(BUTTON_TLD) == HIGH){ + joystick_calibration_mode = CALIBRATION_CENTER; + } + } // Calibrate joystick center values - if (joystick_calibration_mode == CALIBRATION_CENTER){ + else if (joystick_calibration_mode == CALIBRATION_CENTER){ joystick_x1_center = joystick_x1_raw; joystick_y1_center = joystick_y1_raw; joystick_x1_max = joystick_x1_center; @@ -628,6 +471,10 @@ void read_io_data(){ joystick_x2_min = joystick_x2_center; joystick_y2_max = joystick_y2_center; joystick_y2_min = joystick_y2_center; + + if (digitalRead(BUTTON_TLD) == LOW){ + joystick_calibration_mode = CALIBRATION_MINMAX; + } } // Calibrate joystick min/max values else if (joystick_calibration_mode == CALIBRATION_MINMAX){ @@ -656,6 +503,11 @@ void read_io_data(){ if(joystick_y2_raw < joystick_y2_min){ joystick_y2_min = joystick_y2_raw; } + + if (digitalRead(BUTTON_TRD) == LOW){ + joystick_calibration_mode = CALIBRATION_OFF; + save_to_eeprom(); + } } } } @@ -751,6 +603,8 @@ void setup() // Check if bootloader mode is enabled if (digitalRead(BUTTON_TRD) == LOW){ + digitalWrite(BUTTON_LED_2, HIGH); + delay(200); _reboot_Teensyduino_(); } @@ -768,13 +622,146 @@ void loop() /* Read io value and process as fast as possible */ read_io_data(); - /* Send data every 1500us */ - if (current_timestamp_micros >= elrs_timestamp_micros) + if (current_timestamp >= send_usb_timestamp) { - process_io_data(); + // Set joystick buttons to released + for (int i = 1; i < 32; i++){ + Joystick.button(i, 0); + } + // Set joystick hat to center position + Joystick.hat(-1); + + // Set joystick buttons to pressed + if (fn_mode == 2){ + + if (digitalRead(BUTTON_FLD) == LOW){ + Joystick.button(12, 1); + } + if (digitalRead(BUTTON_FRD) == LOW){ + Joystick.button(13, 1); + } + if (digitalRead(BUTTON_TLD) == LOW){ + Joystick.button(14, 1); + } + } + else if (fn_mode == 1){ + + if (digitalRead(BUTTON_FLD) == LOW){ + Joystick.button(8, 1); + } + if (digitalRead(BUTTON_FRD) == LOW){ + Joystick.button(9, 1); + } + if (digitalRead(BUTTON_TLD) == LOW){ + Joystick.button(10, 1); + } + if (digitalRead(BUTTON_TRD) == LOW){ + Joystick.button(11, 1); + } + } + else{ + + if (digitalRead(BUTTON_FLD) == LOW){ + Joystick.button(2, 1); + } + if (digitalRead(BUTTON_FRD) == LOW){ + Joystick.button(4, 1); + } + if (digitalRead(BUTTON_TRU) == LOW){ + Joystick.button(5, 1); + } + if (digitalRead(BUTTON_TLD) == LOW){ + Joystick.button(6, 1); + } + if (digitalRead(BUTTON_TRD) == LOW){ + Joystick.button(7, 1); + } + } + + if (digitalRead(BUTTON_FLU) == LOW){ + Joystick.button(1, 1); + } + if (digitalRead(BUTTON_FRU) == LOW){ + Joystick.button(3, 1); + } + if (toggle_button_arm){ + Joystick.button(15, 1); + } + + // Check rev mode (reverse stick for Y1 axix) + if (joystick_y1 == AXIS_MIN){ + joystick_y1_hid_rev_mode = 1; + } + else if ((joystick_y1 > AXIS_MIN) && (fn_mode <= 1) && (joystick_y1_hid_rev_mode == 1)){ + joystick_y1_hid_rev_mode = 0; + } + else if ((joystick_y1 > AXIS_MIN) && (fn_mode > 1) && (joystick_y1_hid_rev_mode == 1)){ + joystick_y1_hid_rev_mode = 2; + } + + // Set axis values + if (joystick_y1_hid_rev_mode == 0){ + Joystick.Y(joystick_y1_hid); + } + else if (joystick_y1_hid_rev_mode == 2){ + Joystick.Y(joystick_y1_hid_rev); + } + + Joystick.X(joystick_x1_hid); + + if (fn_mode == 2){ + Joystick.Z(HID_AXIS_CENTER); + Joystick.Zrotate(HID_AXIS_CENTER); + Joystick.sliderLeft(joystick_x2_hid); + Joystick.sliderRight(joystick_y2_hid); + } + else if (fn_mode == 1){ + Joystick.Z(HID_AXIS_CENTER); + Joystick.Zrotate(joystick_y2_hid); + Joystick.sliderLeft(joystick_x2_hid); + Joystick.sliderRight(HID_AXIS_CENTER); + } + else{ + Joystick.Z(joystick_x2_hid); + Joystick.Zrotate(joystick_y2_hid); + Joystick.sliderLeft(HID_AXIS_CENTER); + Joystick.sliderRight(HID_AXIS_CENTER); + } + // Send USB Joystick data Joystick.send_now(); + send_usb_timestamp = current_timestamp + 5; + } + + /* Send data every CRSF_TIME_BETWEEN_FRAMES_US */ + if (current_timestamp_micros >= send_elrs_timestamp_micros) + { + // Set ELRS analog channels + elrs_channels[0] = map(joystick_x1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); + elrs_channels[1] = map(joystick_y1, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); + elrs_channels[2] = map(joystick_x2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); + elrs_channels[3] = map(joystick_y2, AXIS_MIN, AXIS_MAX, CRSF_DIGITAL_CHANNEL_MIN, CRSF_DIGITAL_CHANNEL_MAX); + + // Set ELRS digital channels + for (int i = 4; i < CRSF_MAX_CHANNEL; i++){ + elrs_channels[i] = CRSF_DIGITAL_CHANNEL_MIN; + } + if (toggle_button_arm){ + elrs_channels[4] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (toggle_button_mode){ + elrs_channels[5] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (digitalRead(BUTTON_FRU) == LOW){ + elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (digitalRead(BUTTON_FRD) == LOW){ + elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (digitalRead(BUTTON_TRU) == LOW){ + elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX; + } // Send ELRS data if (elrs_init_done == true){ @@ -782,16 +769,19 @@ void loop() ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE); } else{ + // Setting up initial communication link between ERLS TX and Teensy (give ERLS TX time to auto detect Teensy) if (elrs_init_counter < 500){ - crsf_prepare_data_packet(crsf_cmd_packet, elrs_channels); - ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + crsf_prepare_data_packet(crsf_packet, elrs_channels); + ELRS_SERIAL_PORT.write(crsf_packet, CRSF_PACKET_SIZE); elrs_init_counter++; } + // Send command to update TX packet rate to 500Hz else if (elrs_init_counter < 505){ crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_PKT_RATE_COMMAND, ELRS_PKT_RATE); ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); elrs_init_counter++; } + // Send command to update TX power to 100mW else if (elrs_init_counter < 510){ crsf_prepare_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER); ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); @@ -801,33 +791,32 @@ void loop() elrs_init_done = true; } } - elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; + send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; } - - /* Update indicator 200ms */ - if (current_timestamp >= indicator_timestamp) + + /* Update leds 200ms */ + if (current_timestamp >= led_timestamp) { - /* Update status led */ - if (status_led_mode == 2 && status_led_on == false) - { - digitalWrite(STATUS_LED, HIGH); - status_led_on = true; + // Set led mode + button_led_1_mode = 0; + button_led_2_mode = 0; + + if (joystick_calibration_mode == CALIBRATION_INIT){ + button_led_1_mode = 1; + button_led_2_mode = 1; } - else if (status_led_mode == 2 && status_led_on == true) - { - digitalWrite(STATUS_LED, LOW); - status_led_on = false; + else if (joystick_calibration_mode == CALIBRATION_CENTER){ + button_led_1_mode = 2; + button_led_2_mode = 0; } - else if (status_led_mode == 1) - { - digitalWrite(STATUS_LED, HIGH); - status_led_on = true; - } - else - { - digitalWrite(STATUS_LED, LOW); - status_led_on = false; + else if (joystick_calibration_mode == CALIBRATION_MINMAX){ + button_led_1_mode = 0; + button_led_2_mode = 2; } + else if (joystick_y1_hid_rev_mode == 2){ + button_led_1_mode = 2; + button_led_2_mode = 2; + } /* Updated button led 1 */ if (button_led_1_mode == 2 && button_led_1_on == false) @@ -873,6 +862,34 @@ void loop() button_led_2_on = false; } + led_timestamp = current_timestamp + 200; + } + + /* Update indicator 200ms */ + if (current_timestamp >= indicator_timestamp) + { + /* Update status led */ + if (status_led_mode == 2 && status_led_on == false) + { + digitalWrite(STATUS_LED, HIGH); + status_led_on = true; + } + else if (status_led_mode == 2 && status_led_on == true) + { + digitalWrite(STATUS_LED, LOW); + status_led_on = false; + } + else if (status_led_mode == 1) + { + digitalWrite(STATUS_LED, HIGH); + status_led_on = true; + } + else + { + digitalWrite(STATUS_LED, LOW); + status_led_on = false; + } + indicator_timestamp = current_timestamp + 200; } }