From 727d76a25c018189651641cbeaf7d3f89321fbd2 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Tue, 6 Jun 2023 17:27:44 +0200 Subject: [PATCH] Changed to new clang format --- firmware/lib/teensy-libc | 2 +- firmware/src/cmdr_joystick.cpp | 154 ++++++++++----------------------- 2 files changed, 49 insertions(+), 107 deletions(-) diff --git a/firmware/lib/teensy-libc b/firmware/lib/teensy-libc index 6a2b179..88cb7c1 160000 --- a/firmware/lib/teensy-libc +++ b/firmware/lib/teensy-libc @@ -1 +1 @@ -Subproject commit 6a2b179d8f624d69b41a5d30df7b46080a1deec6 +Subproject commit 88cb7c10ca12ea33fb96aa10ae97a14d087da852 diff --git a/firmware/src/cmdr_joystick.cpp b/firmware/src/cmdr_joystick.cpp index 8deeb22..d3018da 100644 --- a/firmware/src/cmdr_joystick.cpp +++ b/firmware/src/cmdr_joystick.cpp @@ -55,8 +55,7 @@ const int DEADZONE_X = 50; const int DEADZONE_Y = 50; const int JOYSTICK_HAT_CENTER = -1; -enum EEPROM_ADR -{ +enum EEPROM_ADR { MAX_X1_ADR_HIGH, MAX_X1_ADR_LOW, MIN_X1_ADR_HIGH, @@ -122,8 +121,7 @@ int joystick_calibration_mode = CALIBRATION_OFF; /** * @brief Save calibration data to EEPROM */ -void save_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); @@ -156,8 +154,7 @@ void save_to_eeprom() /** * @brief Load calibration data from EEPROM */ -void load_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); @@ -195,22 +192,19 @@ void load_from_eeprom() * @param analog_x2 * @param analog_y2 */ -void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int 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) - { + 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) - { + 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; @@ -225,15 +219,13 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) 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) - { + 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) - { + 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; @@ -243,8 +235,7 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) 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) - { + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { joystick_calibration_mode = CALIBRATION_OFF; save_to_eeprom(); } @@ -254,8 +245,7 @@ void calibrate_axis(int analog_x1, int analog_y1, int analog_x2, int analog_y2) /** * @brief Save joystick calibration values to EEPROM */ -void send_elrs_data() -{ +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); @@ -263,8 +253,7 @@ void send_elrs_data() 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++) - { + for (int i = 4; i < CRSF_MAX_CHANNEL; i++) { elrs.set_data(CRSF_DIGITAL_CHANNEL_MIN, i); } @@ -281,8 +270,7 @@ void send_elrs_data() /** * Send USB data to PC */ -void send_usb_data() -{ +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); @@ -292,22 +280,17 @@ void send_usb_data() Joystick.Zrotate(joystick_x1_10bit); Joystick.Z(joystick_y1_10bit); - if (fn_mode == 2) - { + 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) - { + } 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 - { + } else { Joystick.X(joystick_x2_10bit); Joystick.Y(joystick_y2_10bit); Joystick.sliderRight(AXIS_10BIT_CENTER); @@ -315,33 +298,27 @@ void send_usb_data() } // Set USB digital channels - for (int i = 1; i < 32; i++) - { + for (int i = 1; i < 32; i++) { Joystick.button(i, 0); } Joystick.hat(JOYSTICK_HAT_CENTER); - if (fn_mode == 2) - { + 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) - { + } 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 - { + } 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); @@ -364,23 +341,18 @@ void send_usb_data() * @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 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)) - { + 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)) - { + } 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) - { + 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; @@ -393,30 +365,25 @@ int apply_calibration_12bit(int gimbal_value, int min_value, int max_value, int /** * @brief Process input data from gimbal and buttons */ -void process_input_data() -{ +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) - { + 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) - { + } 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) - { + 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; } @@ -432,42 +399,32 @@ void process_input_data() // Check fn mode fn_mode = 0; - if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { + if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) { fn_mode = 1; - if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) - { + 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) - { + if (digitalRead(BUTTON_FRONT_LEFT_UPPER_PIN) == BUTTON_PRESSED) { toggle_button_mode = true; - } - else if (digitalRead(BUTTON_FRONT_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { + } 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)) - { + 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)) - { + } 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() -{ +void setup() { /* Init HW */ status_led.begin(); status_led.blink(); @@ -503,14 +460,12 @@ void setup() Joystick.useManualSend(true); // Check if calibration mode is enabled - if (digitalRead(BUTTON_TOP_LEFT_LOWER_PIN) == BUTTON_PRESSED) - { + 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) - { + if (digitalRead(BUTTON_TOP_RIGHT_LOWER_PIN) == BUTTON_PRESSED) { button_led_2.on(); button_led_2.update(); delay(200); @@ -518,8 +473,7 @@ void setup() } // Check what gimbal mode is selected - if (digitalRead(GIMBAL_MODE_PIN) == LOW) - { + if (digitalRead(GIMBAL_MODE_PIN) == LOW) { gimbal_mode = GIMBAL_MODE_FRSKY_M10; } @@ -527,8 +481,7 @@ void setup() elrs.begin(ELRS_PACKET_RATE_500Hz, ELRS_TX_POWER_25mW); } -void loop() -{ +void loop() { current_timestamp_micros = micros(); analog_x1.update(); @@ -537,50 +490,39 @@ void loop() analog_y2.update(); /* Process data with 1ms interval*/ - if (current_timestamp_micros >= process_data_timestamp_micros) - { + 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) - { + 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) - { + 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) - { + if (current_timestamp_micros >= indicator_timestamp_micros) { button_led_1.off(); button_led_2.off(); - if (joystick_calibration_mode == CALIBRATION_INIT) - { + if (joystick_calibration_mode == CALIBRATION_INIT) { button_led_1.blink(); button_led_2.blink(); - } - else if (joystick_calibration_mode == CALIBRATION_CENTER) - { + } else if (joystick_calibration_mode == CALIBRATION_CENTER) { button_led_1.blink(); button_led_2.off(); - } - else if (joystick_calibration_mode == CALIBRATION_MINMAX) - { + } 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)) - { + } 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(); }