From 0ba7584d567a4c67f8dd4f4d7fe9300ba14f7097 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 16 May 2023 20:53:44 +0000 Subject: [PATCH] Code cleanup. Added support for two different gimbal (M10 and M7) --- .../{src => lib}/ResponsiveAnalogRead.cpp | 0 firmware/{src => lib}/ResponsiveAnalogRead.h | 0 firmware/src/{main.cpp => cmdr_joystick.cpp} | 918 +++++++++--------- 3 files changed, 453 insertions(+), 465 deletions(-) rename firmware/{src => lib}/ResponsiveAnalogRead.cpp (100%) rename firmware/{src => lib}/ResponsiveAnalogRead.h (100%) rename firmware/src/{main.cpp => cmdr_joystick.cpp} (52%) diff --git a/firmware/src/ResponsiveAnalogRead.cpp b/firmware/lib/ResponsiveAnalogRead.cpp similarity index 100% rename from firmware/src/ResponsiveAnalogRead.cpp rename to firmware/lib/ResponsiveAnalogRead.cpp diff --git a/firmware/src/ResponsiveAnalogRead.h b/firmware/lib/ResponsiveAnalogRead.h similarity index 100% rename from firmware/src/ResponsiveAnalogRead.h rename to firmware/lib/ResponsiveAnalogRead.h diff --git a/firmware/src/main.cpp b/firmware/src/cmdr_joystick.cpp similarity index 52% rename from firmware/src/main.cpp rename to firmware/src/cmdr_joystick.cpp index d23a1d0..17d3089 100644 --- a/firmware/src/main.cpp +++ b/firmware/src/cmdr_joystick.cpp @@ -138,39 +138,40 @@ int16_t elrs_channels[CRSF_MAX_CHANNEL]; bool elrs_init_done = false; int elrs_init_counter = 0; -const int STATUS_LED = 13; +const int STATUS_LED_PIN = 13; bool status_led_on = false; int status_led_mode = 2; -const int BUTTON_LED_1 = 22; +const int BUTTON_LED_1_PIN = 22; bool button_led_1_on = false; int button_led_1_mode = 0; -const int BUTTON_LED_2 = 23; +const int BUTTON_LED_2_PIN = 23; bool button_led_2_on = false; int button_led_2_mode = 0; unsigned long current_timestamp = 0; -unsigned long led_timestamp = 0; +unsigned long process_data_timestamp = 0; unsigned long indicator_timestamp = 0; unsigned long send_usb_timestamp = 0; unsigned long current_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 -const int BUTTON_FRU = 6; // Front right upper button -const int BUTTON_FRD = 7; // Front right lower button -const int BUTTON_TLU = 9; // top upper left button -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 +const int BUTTON_FLU_PIN = 4; // Front left upper button +const int BUTTON_FLD_PIN = 5; // Front left lower button +const int BUTTON_FRU_PIN = 6; // Front right upper button +const int BUTTON_FRD_PIN = 7; // Front right lower button +const int BUTTON_TLU_PIN = 9; // top upper left button +const int BUTTON_TRU_PIN = 10; // Top upper right button +const int BUTTON_TLD_PIN = 8; // Top left lower button +const int BUTTON_TRD_PIN = 11; // Top right lower button + +const int GIMBAL_MODE_PIN = 21; // Open = M7, Closed (GND) = M10 bool toggle_button_arm = false; bool toggle_button_mode = false; -int button_arm_previous_value = HIGH; -int button_11_previous_value = HIGH; +int toggle_button_arm_previous_value = HIGH; #define HID_AXIS_MAX 1023 #define HID_AXIS_MIN 0 @@ -213,7 +214,6 @@ 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; @@ -221,19 +221,16 @@ 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; int joystick_y2_center = joystick_y2_max/2; @@ -241,11 +238,12 @@ int joystick_y2_center = joystick_y2_max/2; float exp_constant = 0.2; int fn_mode = 0; +int gimbal_mode = 0; ResponsiveAnalogRead analog_x1(A0, true); ResponsiveAnalogRead analog_y1(A1, true); -ResponsiveAnalogRead analog_x2(A2, true); -ResponsiveAnalogRead analog_y2(A3, true); +ResponsiveAnalogRead analog_x2(A3, true); +ResponsiveAnalogRead analog_y2(A2, true); #define CALIBRATION_OFF 0 #define CALIBRATION_INIT 1 @@ -315,211 +313,8 @@ void load_from_eeprom(){ joystick_y2_center |= EEPROM.read(CNT_Y2_ADR_LOW); } -void read_io_data(){ - - if(joystick_counter == 0){ - joystick_x1_raw = analog_x1.getValue(); - joystick_counter++; - } - else if(joystick_counter == 1){ - joystick_y1_raw = analog_y1.getValue(); - joystick_counter++; - } - else if(joystick_counter == 2){ - joystick_x2_raw = analog_x2.getValue(); - joystick_counter++; - } - else if(joystick_counter == 3){ - joystick_y2_raw = analog_y2.getValue(); - joystick_counter = 0; - - if (joystick_calibration_mode == CALIBRATION_OFF){ - - // Calculate X1 joystick values - if(joystick_x1_raw > (joystick_x1_center + DEADZONE_X)){ - joystick_x1 = constrain(map(joystick_x1_raw, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); - } - else if(joystick_x1_raw < (joystick_x1_center - DEADZONE_X)){ - joystick_x1 = constrain(map(joystick_x1_raw, joystick_x1_min, (joystick_x1_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); - } - else{ - joystick_x1 = AXIS_CENTER; - } - - // Calculate Y1 joystick values - 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_MIN; - } - - // Calculate X2 joystick values - if(joystick_x2_raw > (joystick_x2_center + DEADZONE_X)){ - joystick_x2 = constrain(map(joystick_x2_raw, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); - } - else if(joystick_x2_raw < (joystick_x2_center - DEADZONE_X)){ - joystick_x2 = constrain(map(joystick_x2_raw, joystick_x2_min, (joystick_x2_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); - } - else{ - joystick_x2 = AXIS_CENTER; - } - - // Calculate Y2 joystick values - if(joystick_y2_raw > (joystick_y2_center + DEADZONE_Y)){ - joystick_y2 = constrain(map(joystick_y2_raw, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); - } - else if(joystick_y2_raw < (joystick_y2_center - DEADZONE_Y)){ - joystick_y2 = constrain(map(joystick_y2_raw, joystick_y2_min, (joystick_y2_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); - } - else{ - joystick_y2 = AXIS_CENTER; - } - - // Calculate new X1 values after applying exp curve - 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_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 = HID_AXIS_CENTER; - } - - // Calculate new Y1 values - joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_MIN, HID_AXIS_MAX); - - // 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_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 = 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_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 = 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 == 1){ - 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 = 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 - 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; - joystick_x1_min = joystick_x1_center; - joystick_y1_max = joystick_y1_center; - joystick_y1_min = joystick_y1_center; - - joystick_x2_center = joystick_x2_raw; - joystick_y2_center = joystick_y2_raw; - joystick_x2_max = joystick_x2_center; - 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){ - if(joystick_x1_raw > joystick_x1_max){ - joystick_x1_max = joystick_x1_raw; - } - if(joystick_x1_raw < joystick_x1_min){ - joystick_x1_min = joystick_x1_raw; - } - if(joystick_y1_raw > joystick_y1_max){ - joystick_y1_max = joystick_y1_raw; - } - if(joystick_y1_raw < joystick_y1_min){ - joystick_y1_min = joystick_y1_raw; - } - - if(joystick_x2_raw > joystick_x2_max){ - joystick_x2_max = joystick_x2_raw; - } - if(joystick_x2_raw < joystick_x2_min){ - joystick_x2_min = joystick_x2_raw; - } - if(joystick_y2_raw > joystick_y2_max){ - joystick_y2_max = joystick_y2_raw; - } - 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(); - } - } - } - } -} - // CRSF CRC calculation via lookup table -uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) { +uint8_t calculate_crsf_crc8(const uint8_t *ptr, uint8_t len) { uint8_t crc = 0; for (uint8_t i = 0; i < len; i++){ crc = crsf_crc8tab[crc ^ *ptr++]; @@ -528,7 +323,7 @@ uint8_t crsf_crc8(const uint8_t *ptr, uint8_t len) { } // Prepare CRSF data packet -void crsf_prepare_data_packet(uint8_t packet[], int16_t channels[]) { +void prepare_crsf_data_packet(uint8_t packet[], int16_t channels[]) { packet[0] = ELRS_ADDRESS; // Header packet[1] = 24; // length packet[2] = CRSF_TYPE_CHANNELS; @@ -554,11 +349,11 @@ void crsf_prepare_data_packet(uint8_t packet[], int16_t channels[]) { packet[22] = (uint8_t)((channels[13] & 0x07FF) >> 9 | (channels[14] & 0x07FF) << 2); // Channel not used by ELRS packet[23] = (uint8_t)((channels[14] & 0x07FF) >> 6 | (channels[15] & 0x07FF) << 5); // Channel not used by ELRS packet[24] = (uint8_t)((channels[15] & 0x07FF) >> 3); // Channel not used by ELRS - packet[25] = crsf_crc8(&packet[2], packet[1] - 1); + packet[25] = calculate_crsf_crc8(&packet[2], packet[1] - 1); } // Prepare CRSF setup packet -void crsf_prepare_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) { +void prepare_crsf_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t value) { packet_cmd[0] = ELRS_ADDRESS; // Header packet_cmd[1] = 6; // length packet_cmd[2] = ELRS_TYPE_SETTINGS_WRITE; @@ -566,35 +361,424 @@ void crsf_prepare_cmd_packet(uint8_t packet_cmd[], uint8_t command, uint8_t valu packet_cmd[4] = ELRS_ADDR_RADIO; packet_cmd[5] = command; packet_cmd[6] = value; - packet_cmd[7] = crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); + packet_cmd[7] = calculate_crsf_crc8(&packet_cmd[2], packet_cmd[1] - 1); +} + +void send_elrs_data(){ + + // 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_PIN) == LOW){ + elrs_channels[6] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (digitalRead(BUTTON_FRD_PIN) == LOW){ + elrs_channels[7] = CRSF_DIGITAL_CHANNEL_MAX; + } + if (digitalRead(BUTTON_TRU_PIN) == LOW){ + elrs_channels[8] = CRSF_DIGITAL_CHANNEL_MAX; + } + + // Send ELRS data + if (elrs_init_done == true){ + prepare_crsf_data_packet(crsf_packet, elrs_channels); + 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){ + prepare_crsf_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){ + prepare_crsf_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){ + prepare_crsf_cmd_packet(crsf_cmd_packet, ELRS_POWER_COMMAND, ELRS_POWER); + ELRS_SERIAL_PORT.write(crsf_cmd_packet, CRSF_CMD_PACKET_SIZE); + elrs_init_counter++; + } + else{ + elrs_init_done = true; + } + } +} + +void send_usb_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_PIN) == LOW){ + Joystick.button(12, 1); + } + if (digitalRead(BUTTON_FRD_PIN) == LOW){ + Joystick.button(13, 1); + } + if (digitalRead(BUTTON_TLD_PIN) == LOW){ + Joystick.button(14, 1); + } + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + Joystick.button(15, 1); + } + } + else if (fn_mode == 1){ + + if (digitalRead(BUTTON_FLD_PIN) == LOW){ + Joystick.button(8, 1); + } + if (digitalRead(BUTTON_FRD_PIN) == LOW){ + Joystick.button(9, 1); + } + if (digitalRead(BUTTON_TLD_PIN) == LOW){ + Joystick.button(10, 1); + } + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + Joystick.button(11, 1); + } + } + else{ + + if (digitalRead(BUTTON_FLD_PIN) == LOW){ + Joystick.button(2, 1); + } + if (digitalRead(BUTTON_FRD_PIN) == LOW){ + Joystick.button(4, 1); + } + if (digitalRead(BUTTON_TRU_PIN) == LOW){ + Joystick.button(5, 1); + } + if (digitalRead(BUTTON_TLD_PIN) == LOW){ + Joystick.button(6, 1); + } + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + Joystick.button(7, 1); + } + } + + if (digitalRead(BUTTON_FLU_PIN) == LOW){ + Joystick.button(1, 1); + } + if (digitalRead(BUTTON_FRU_PIN) == LOW){ + Joystick.button(3, 1); + } + + // Set axis values + Joystick.Zrotate(joystick_x1_hid); + Joystick.Z(joystick_y1_hid); + + if (fn_mode == 2){ + Joystick.X(HID_AXIS_CENTER); + Joystick.Y(HID_AXIS_CENTER); + Joystick.sliderRight(joystick_x2_hid); + Joystick.sliderLeft(joystick_y2_hid); + } + else if (fn_mode == 1){ + Joystick.X(joystick_x2_hid); + Joystick.Y(HID_AXIS_CENTER); + Joystick.sliderRight(HID_AXIS_CENTER); + Joystick.sliderLeft(joystick_y2_hid); + } + else{ + Joystick.X(joystick_x2_hid); + Joystick.Y(joystick_y2_hid); + Joystick.sliderRight(HID_AXIS_CENTER); + Joystick.sliderLeft(HID_AXIS_CENTER); + } + + // Send USB Joystick data + Joystick.send_now(); +} + +void process_input_data(){ + + int analog_x1_inv = 0; + int analog_y1_inv = 0; + int analog_x2_inv = 0; + int analog_y2_inv = 0; + + if (gimbal_mode == 1){ + // FrSky M10 Gimbal + analog_x1_inv = constrain(AXIS_MAX - analog_x1.getValue(), AXIS_MIN, AXIS_MAX); + analog_y1_inv = constrain(analog_y1.getValue(), AXIS_MIN, AXIS_MAX); + analog_x2_inv = constrain(analog_x2.getValue(), AXIS_MIN, AXIS_MAX); + analog_y2_inv = constrain(AXIS_MAX - analog_y2.getValue(), AXIS_MIN, AXIS_MAX); + } + else{ + // FrSky M7 Gimbal + analog_x1_inv = constrain(analog_x1.getValue(), AXIS_MIN, AXIS_MAX); + analog_y1_inv = constrain(AXIS_MAX - analog_y1.getValue(), AXIS_MIN, AXIS_MAX); + analog_x2_inv = constrain(AXIS_MAX - analog_x2.getValue(), AXIS_MIN, AXIS_MAX); + analog_y2_inv = constrain(analog_y2.getValue(), AXIS_MIN, AXIS_MAX); + } + + if (joystick_calibration_mode == CALIBRATION_OFF){ + + // Calculate X1 joystick values + if(analog_x1_inv > (joystick_x1_center + DEADZONE_X)){ + joystick_x1 = constrain(map(analog_x1_inv, (joystick_x1_center + DEADZONE_X), joystick_x1_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); + } + else if(analog_x1_inv < (joystick_x1_center - DEADZONE_X)){ + joystick_x1 = constrain(map(analog_x1_inv, joystick_x1_min, (joystick_x1_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); + } + else{ + joystick_x1 = AXIS_CENTER; + } + + // Calculate Y1 joystick values + if(analog_y1_inv > (joystick_y1_min + DEADZONE_Y)){ + joystick_y1 = constrain(map(analog_y1_inv, (joystick_y1_min + DEADZONE_Y), joystick_y1_max, AXIS_MIN, AXIS_MAX), AXIS_MIN, AXIS_MAX); + } + else{ + joystick_y1 = AXIS_MIN; + } + + // Calculate X2 joystick values + if(analog_x2_inv > (joystick_x2_center + DEADZONE_X)){ + joystick_x2 = constrain(map(analog_x2_inv, (joystick_x2_center + DEADZONE_X), joystick_x2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); + } + else if(analog_x2_inv < (joystick_x2_center - DEADZONE_X)){ + joystick_x2 = constrain(map(analog_x2_inv, joystick_x2_min, (joystick_x2_center - DEADZONE_X), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); + } + else{ + joystick_x2 = AXIS_CENTER; + } + + // Calculate Y2 joystick values + if(analog_y2_inv > (joystick_y2_center + DEADZONE_Y)){ + joystick_y2 = constrain(map(analog_y2_inv, (joystick_y2_center + DEADZONE_Y), joystick_y2_max, AXIS_CENTER, AXIS_MAX), AXIS_CENTER, AXIS_MAX); + } + else if(analog_y2_inv < (joystick_y2_center - DEADZONE_Y)){ + joystick_y2 = constrain(map(analog_y2_inv, joystick_y2_min, (joystick_y2_center - DEADZONE_Y), AXIS_MIN, AXIS_CENTER), AXIS_MIN, AXIS_CENTER); + } + else{ + joystick_y2 = AXIS_CENTER; + } + + // Calculate new X1 values after applying exp curve + 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_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 = HID_AXIS_CENTER; + } + + // Calculate new Y1 values + joystick_y1_hid = map(joystick_y1, AXIS_MIN, AXIS_MAX, HID_AXIS_MIN, HID_AXIS_MAX); + + // 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_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 = 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_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 = HID_AXIS_CENTER; + } + + // Check fn mode + fn_mode = 0; + if (digitalRead(BUTTON_TLU_PIN) == LOW){ + fn_mode = 1; + if (digitalRead(BUTTON_TRU_PIN) == LOW){ + fn_mode = 2; + } + } + + // Check toggle mode buttons + if (digitalRead(BUTTON_FLU_PIN) == LOW){ + toggle_button_mode = true; + } + else if (digitalRead(BUTTON_FLD_PIN) == LOW){ + toggle_button_mode = false; + } + + // Check toggle arm button + if (fn_mode == 1){ + if (digitalRead(BUTTON_TRD_PIN) != toggle_button_arm_previous_value){ + toggle_button_arm_previous_value = digitalRead(BUTTON_TRD_PIN); + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + if (toggle_button_arm == false){ + toggle_button_arm = true; + } + else{ + toggle_button_arm = false; + } + } + } + } + } + else{ + + // Calibration mode. + 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_PIN) == HIGH){ + joystick_calibration_mode = CALIBRATION_CENTER; + } + } + + // Calibrate joystick center values + else if (joystick_calibration_mode == CALIBRATION_CENTER){ + joystick_x1_center = analog_x1_inv; + joystick_y1_center = analog_y1_inv; + joystick_x1_max = joystick_x1_center; + joystick_x1_min = joystick_x1_center; + joystick_y1_max = joystick_y1_center; + joystick_y1_min = joystick_y1_center; + + joystick_x2_center = analog_x2_inv; + joystick_y2_center = analog_y2_inv; + joystick_x2_max = joystick_x2_center; + joystick_x2_min = joystick_x2_center; + joystick_y2_max = joystick_y2_center; + joystick_y2_min = joystick_y2_center; + + if (digitalRead(BUTTON_TLD_PIN) == LOW){ + joystick_calibration_mode = CALIBRATION_MINMAX; + } + } + + // Calibrate joystick min/max values + else if (joystick_calibration_mode == CALIBRATION_MINMAX){ + if(analog_x1_inv > joystick_x1_max){ + joystick_x1_max = analog_x1_inv; + } + if(analog_x1_inv < joystick_x1_min){ + joystick_x1_min = analog_x1_inv; + } + if(analog_y1_inv > joystick_y1_max){ + joystick_y1_max = analog_y1_inv; + } + if(analog_y1_inv < joystick_y1_min){ + joystick_y1_min = analog_y1_inv; + } + + if(analog_x2_inv > joystick_x2_max){ + joystick_x2_max = analog_x2_inv; + } + if(analog_x2_inv < joystick_x2_min){ + joystick_x2_min = analog_x2_inv; + } + if(analog_y2_inv > joystick_y2_max){ + joystick_y2_max = analog_y2_inv; + } + if(analog_y2_inv < joystick_y2_min){ + joystick_y2_min = analog_y2_inv; + } + + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + joystick_calibration_mode = CALIBRATION_OFF; + save_to_eeprom(); + } + } + } +} + +/* Update indicator 0=Off, 1=On, 2=Blink */ +void update_indicator(int mode, bool ¤t_state, int pin){ + + if (mode == 2 && current_state == false) + { + digitalWrite(pin, HIGH); + current_state = true; + } + else if (mode == 2 && current_state == true) + { + digitalWrite(pin, LOW); + current_state = false; + } + else if (mode == 1) + { + digitalWrite(pin, HIGH); + current_state = true; + } + else + { + digitalWrite(pin, LOW); + current_state = false; + } } void setup() { /* Init HW */ - pinMode(STATUS_LED, OUTPUT); - digitalWrite(STATUS_LED, LOW); + pinMode(STATUS_LED_PIN, OUTPUT); + digitalWrite(STATUS_LED_PIN, LOW); - pinMode(BUTTON_LED_1, OUTPUT); - digitalWrite(BUTTON_LED_1, LOW); + pinMode(BUTTON_LED_1_PIN, OUTPUT); + digitalWrite(BUTTON_LED_1_PIN, LOW); - pinMode(BUTTON_LED_2, OUTPUT); - digitalWrite(BUTTON_LED_2, LOW); + pinMode(BUTTON_LED_2_PIN, OUTPUT); + digitalWrite(BUTTON_LED_2_PIN, LOW); - pinMode(BUTTON_FLD, INPUT_PULLUP); - pinMode(BUTTON_FLU, INPUT_PULLUP); - pinMode(BUTTON_FRD, INPUT_PULLUP); - pinMode(BUTTON_FRU, INPUT_PULLUP); - pinMode(BUTTON_TLD, INPUT_PULLUP); - pinMode(BUTTON_TRD, INPUT_PULLUP); - pinMode(BUTTON_TLU, INPUT_PULLUP); - pinMode(BUTTON_TRU, INPUT_PULLUP); + pinMode(BUTTON_FLD_PIN, INPUT_PULLUP); + pinMode(BUTTON_FLU_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRD_PIN, INPUT_PULLUP); + pinMode(BUTTON_FRU_PIN, INPUT_PULLUP); + pinMode(BUTTON_TLD_PIN, INPUT_PULLUP); + pinMode(BUTTON_TRD_PIN, INPUT_PULLUP); + pinMode(BUTTON_TLU_PIN, INPUT_PULLUP); + pinMode(BUTTON_TRU_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); @@ -607,17 +791,22 @@ void setup() Joystick.useManualSend(true); // Check if calibration mode is enabled - if (digitalRead(BUTTON_TLD) == LOW){ + if (digitalRead(BUTTON_TLD_PIN) == LOW){ joystick_calibration_mode = CALIBRATION_INIT; } // Check if bootloader mode is enabled - if (digitalRead(BUTTON_TRD) == LOW){ - digitalWrite(BUTTON_LED_2, HIGH); + if (digitalRead(BUTTON_TRD_PIN) == LOW){ + digitalWrite(BUTTON_LED_2_PIN, HIGH); delay(200); _reboot_Teensyduino_(); } + // Check what gimbal mode is selected + if (digitalRead(GIMBAL_MODE_PIN) == LOW){ + gimbal_mode = 1; + } + // Init ELRS ELRS_SERIAL_PORT.begin(ELRS_SERIAL_BAUDRATE); } @@ -629,179 +818,43 @@ void loop() /* Update current time (us) */ current_timestamp_micros = micros(); - /* Read io value and process as fast as possible */ + /* Read io value as fast as possible */ analog_x1.update(); analog_y1.update(); analog_x2.update(); analog_y2.update(); - read_io_data(); + /* Process data with 1ms interval*/ + if (current_timestamp >= process_data_timestamp) + { + process_input_data(); + process_data_timestamp = current_timestamp + 1; + } + /* Update/Send USB data with 5ms interval*/ if (current_timestamp >= send_usb_timestamp) { - // 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); - } - 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); - } - - // Set axis values - Joystick.Y(joystick_y1_hid); - 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_data(); send_usb_timestamp = current_timestamp + 5; } - /* Send data every CRSF_TIME_BETWEEN_FRAMES_US */ + /* Update/Send ERLS data with 1,6ms interval */ 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){ - crsf_prepare_data_packet(crsf_packet, elrs_channels); - 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_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); - elrs_init_counter++; - } - else{ - elrs_init_done = true; - } - } + send_elrs_data(); send_elrs_timestamp_micros = current_timestamp_micros + CRSF_TIME_BETWEEN_FRAMES_US; } - /* Update leds 200ms */ - if (current_timestamp >= led_timestamp) + /* Update indicator 200ms */ + if (current_timestamp >= indicator_timestamp) { // 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; + button_led_1_mode = 2; + button_led_2_mode = 2; } else if (joystick_calibration_mode == CALIBRATION_CENTER){ button_led_1_mode = 2; @@ -818,77 +871,12 @@ void loop() } /* Updated button led 1 */ - if (button_led_1_mode == 2 && button_led_1_on == false) - { - digitalWrite(BUTTON_LED_1, HIGH); - button_led_1_on = true; - } - else if (button_led_1_mode == 2 && button_led_1_on == true) - { - digitalWrite(BUTTON_LED_1, LOW); - button_led_1_on = false; - } - else if (button_led_1_mode == 1) - { - digitalWrite(BUTTON_LED_1, HIGH); - button_led_1_on = true; - } - else - { - digitalWrite(BUTTON_LED_1, LOW); - button_led_1_on = false; - } - + update_indicator(button_led_1_mode, button_led_1_on, BUTTON_LED_1_PIN); /* Updated button led 2 */ - if (button_led_2_mode == 2 && button_led_2_on == false) - { - digitalWrite(BUTTON_LED_2, HIGH); - button_led_2_on = true; - } - else if (button_led_2_mode == 2 && button_led_2_on == true) - { - digitalWrite(BUTTON_LED_2, LOW); - button_led_2_on = false; - } - else if (button_led_2_mode == 1) - { - digitalWrite(BUTTON_LED_2, HIGH); - button_led_2_on = true; - } - else - { - digitalWrite(BUTTON_LED_2, LOW); - button_led_2_on = false; - } - - led_timestamp = current_timestamp + 200; - } - - /* Update indicator 200ms */ - if (current_timestamp >= indicator_timestamp) - { + update_indicator(button_led_2_mode, button_led_2_on, BUTTON_LED_2_PIN); /* 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; - } - + update_indicator(status_led_mode, status_led_on, STATUS_LED_PIN); + indicator_timestamp = current_timestamp + 200; } }