From 6d22e5dc54d03021355e6e803396c38a867a7777 Mon Sep 17 00:00:00 2001 From: Christoffer Martinsson Date: Sun, 11 May 2025 18:06:00 +0200 Subject: [PATCH] Code cleanup --- rp2040/src/main.rs | 52 +++++++++++++++++++++++----------------------- 1 file changed, 26 insertions(+), 26 deletions(-) diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index 8aff440..0c9d2ad 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -53,9 +53,9 @@ pub const BUTTON_ROWS: usize = 5; pub const BUTTON_COLS: usize = 5; pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS; -pub const AXIS_MIN: u16 = 0; -pub const AXIS_MAX: u16 = 4095; -pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2; +pub const ADC_MIN: u16 = 0; +pub const ADC_MAX: u16 = 4095; +pub const AXIS_CENTER: u16 = (ADC_MIN + ADC_MAX) / 2; pub const NBR_OF_GIMBAL_AXIS: usize = 4; pub const GIMBAL_AXIS_LEFT_X: usize = 0; @@ -111,8 +111,8 @@ impl Default for GimbalAxis { value: AXIS_CENTER, previous_value: AXIS_CENTER, idle_value: AXIS_CENTER, - max: AXIS_MAX, - min: AXIS_MIN, + max: ADC_MAX, + min: ADC_MIN, center: AXIS_CENTER, deadzone: (100, 50, 100), expo: true, @@ -401,7 +401,7 @@ fn main() -> ! { buttons[26].usb_button = 8; // Table for gimbal expo curve lookup insded of doing floating point math for every analog read - let expo_lut: [u16; AXIS_MAX as usize + 1] = generate_expo_lut(0.3); + let expo_lut: [u16; ADC_MAX as usize + 1] = generate_expo_lut(0.3); // Create dynamic smoother array for gimbal axis let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ @@ -463,12 +463,12 @@ fn main() -> ! { if gimbal_mode == GIMBAL_MODE_M10 { // Invert X1 and Y2 axis (M10 gimbals) - left_x = AXIS_MAX - left_x; - right_y = AXIS_MAX - right_y; + left_x = ADC_MAX - left_x; + right_y = ADC_MAX - right_y; } else if gimbal_mode == GIMBAL_MODE_M7 { // Invert Y1 and X2 axis (M7 gimbals) - left_y = AXIS_MAX - left_y; - right_x = AXIS_MAX - right_x; + left_y = ADC_MAX - left_y; + right_x = ADC_MAX - right_x; } // Process anlog filter smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32); @@ -584,9 +584,9 @@ fn main() -> ! { { axis[GIMBAL_AXIS_LEFT_Y].value = remap( axis[GIMBAL_AXIS_LEFT_Y].value, - AXIS_MIN, + ADC_MIN, AXIS_CENTER, - AXIS_MIN, + ADC_MIN, axis[GIMBAL_AXIS_LEFT_Y].hold, ); } else if throttle_hold_enable @@ -596,9 +596,9 @@ fn main() -> ! { axis[GIMBAL_AXIS_LEFT_Y].value = remap( axis[GIMBAL_AXIS_LEFT_Y].value, AXIS_CENTER, - AXIS_MAX, + ADC_MAX, axis[GIMBAL_AXIS_LEFT_Y].hold, - AXIS_MAX, + ADC_MAX, ); } else if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value == AXIS_CENTER { axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold; @@ -727,9 +727,9 @@ fn get_joystick_report( axis: &mut [GimbalAxis; 4], ) -> JoystickReport { let x: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_LEFT_X].value); - let y: i16 = axis_12bit_to_i16(AXIS_MAX - axis[GIMBAL_AXIS_LEFT_Y].value); + let y: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_LEFT_Y].value); let z: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_RIGHT_X].value); - let rx: i16 = axis_12bit_to_i16(AXIS_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value); + let rx: i16 = axis_12bit_to_i16(ADC_MAX - axis[GIMBAL_AXIS_RIGHT_Y].value); let mut hat1: u8 = 0xf; let mut hat2: u8 = 0xf; let mut hat3: u8 = 0xf; @@ -929,13 +929,13 @@ fn calculate_axis_value( center: u16, deadzone: (u16, u16, u16), expo: bool, - expo_lut: &[u16; AXIS_MAX as usize + 1], + expo_lut: &[u16; ADC_MAX as usize + 1], ) -> u16 { if value <= min { - return AXIS_MIN; + return ADC_MIN; } if value >= max { - return AXIS_MAX; + return ADC_MAX; } let mut calibrated_value = AXIS_CENTER; @@ -946,14 +946,14 @@ fn calculate_axis_value( center + deadzone.1, max - deadzone.2, AXIS_CENTER, - AXIS_MAX, + ADC_MAX, ); } else if value < (center - deadzone.1) { calibrated_value = remap( value, min + deadzone.0, center - deadzone.1, - AXIS_MIN, + ADC_MIN, AXIS_CENTER, ); } @@ -1003,14 +1003,14 @@ fn constrain(value: T, out_min: T, out_max: T) -> T { /// /// # Arguments /// * `expo` - Exponential curve factor (range 0.0 - 1.0) -fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { - let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1]; - for i in 0..AXIS_MAX + 1 { - let value_float = i as f32 / AXIS_MAX as f32; +fn generate_expo_lut(expo: f32) -> [u16; ADC_MAX as usize + 1] { + let mut lut: [u16; ADC_MAX as usize + 1] = [0; ADC_MAX as usize + 1]; + for i in 0..ADC_MAX + 1 { + let value_float = i as f32 / ADC_MAX as f32; // Calculate expo using 9th order polynomial function with 0.5 as center point let value_exp: f32 = expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float; - lut[i as usize] = constrain((value_exp * AXIS_MAX as f32) as u16, AXIS_MIN, AXIS_MAX); + lut[i as usize] = constrain((value_exp * ADC_MAX as f32) as u16, ADC_MIN, ADC_MAX); } lut }