cm/cmdr-joystick
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2025-05-11 18:06:00 +02:00
parent 30a39c22a6
commit 6d22e5dc54
1 changed files with 26 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

52
rp2040/src/main.rs
View File

@ -53,9 +53,9 @@ pub const BUTTON_ROWS: usize = 5;
pub const BUTTON_COLS: usize = 5; pub const BUTTON_COLS: usize = 5;
pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS; pub const NUMBER_OF_BUTTONS: usize = BUTTON_ROWS * BUTTON_COLS;
pub const AXIS_MIN: u16 = 0; pub const ADC_MIN: u16 = 0;
pub const AXIS_MAX: u16 = 4095; pub const ADC_MAX: u16 = 4095;
pub const AXIS_CENTER: u16 = (AXIS_MIN + AXIS_MAX) / 2; pub const AXIS_CENTER: u16 = (ADC_MIN + ADC_MAX) / 2;
pub const NBR_OF_GIMBAL_AXIS: usize = 4; pub const NBR_OF_GIMBAL_AXIS: usize = 4;
pub const GIMBAL_AXIS_LEFT_X: usize = 0; pub const GIMBAL_AXIS_LEFT_X: usize = 0;
@ -111,8 +111,8 @@ impl Default for GimbalAxis {
value: AXIS_CENTER, value: AXIS_CENTER,
previous_value: AXIS_CENTER, previous_value: AXIS_CENTER,
idle_value: AXIS_CENTER, idle_value: AXIS_CENTER,
max: AXIS_MAX, max: ADC_MAX,
min: AXIS_MIN, min: ADC_MIN,
center: AXIS_CENTER, center: AXIS_CENTER,
deadzone: (100, 50, 100), deadzone: (100, 50, 100),
expo: true, expo: true,
@ -401,7 +401,7 @@ fn main() -> ! {
buttons[26].usb_button = 8; buttons[26].usb_button = 8;
// Table for gimbal expo curve lookup insded of doing floating point math for every analog read // 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 // Create dynamic smoother array for gimbal axis
let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [ let mut smoother: [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] = [
@ -463,12 +463,12 @@ fn main() -> ! {
if gimbal_mode == GIMBAL_MODE_M10 { if gimbal_mode == GIMBAL_MODE_M10 {
// Invert X1 and Y2 axis (M10 gimbals) // Invert X1 and Y2 axis (M10 gimbals)
left_x = AXIS_MAX - left_x; left_x = ADC_MAX - left_x;
right_y = AXIS_MAX - right_y; right_y = ADC_MAX - right_y;
} else if gimbal_mode == GIMBAL_MODE_M7 { } else if gimbal_mode == GIMBAL_MODE_M7 {
// Invert Y1 and X2 axis (M7 gimbals) // Invert Y1 and X2 axis (M7 gimbals)
left_y = AXIS_MAX - left_y; left_y = ADC_MAX - left_y;
right_x = AXIS_MAX - right_x; right_x = ADC_MAX - right_x;
} }
// Process anlog filter // Process anlog filter
smoother[GIMBAL_AXIS_LEFT_X].tick(left_x as i32); 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 = remap(
axis[GIMBAL_AXIS_LEFT_Y].value, axis[GIMBAL_AXIS_LEFT_Y].value,
AXIS_MIN, ADC_MIN,
AXIS_CENTER, AXIS_CENTER,
AXIS_MIN, ADC_MIN,
axis[GIMBAL_AXIS_LEFT_Y].hold, axis[GIMBAL_AXIS_LEFT_Y].hold,
); );
} else if throttle_hold_enable } else if throttle_hold_enable
@ -596,9 +596,9 @@ fn main() -> ! {
axis[GIMBAL_AXIS_LEFT_Y].value = remap( axis[GIMBAL_AXIS_LEFT_Y].value = remap(
axis[GIMBAL_AXIS_LEFT_Y].value, axis[GIMBAL_AXIS_LEFT_Y].value,
AXIS_CENTER, AXIS_CENTER,
AXIS_MAX, ADC_MAX,
axis[GIMBAL_AXIS_LEFT_Y].hold, axis[GIMBAL_AXIS_LEFT_Y].hold,
AXIS_MAX, ADC_MAX,
); );
} else if throttle_hold_enable && axis[GIMBAL_AXIS_LEFT_Y].value == AXIS_CENTER { } 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; axis[GIMBAL_AXIS_LEFT_Y].value = axis[GIMBAL_AXIS_LEFT_Y].hold;
@ -727,9 +727,9 @@ fn get_joystick_report(
axis: &mut [GimbalAxis; 4], axis: &mut [GimbalAxis; 4],
) -> JoystickReport { ) -> JoystickReport {
let x: i16 = axis_12bit_to_i16(axis[GIMBAL_AXIS_LEFT_X].value); 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 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 hat1: u8 = 0xf;
let mut hat2: u8 = 0xf; let mut hat2: u8 = 0xf;
let mut hat3: u8 = 0xf; let mut hat3: u8 = 0xf;
@ -929,13 +929,13 @@ fn calculate_axis_value(
center: u16, center: u16,
deadzone: (u16, u16, u16), deadzone: (u16, u16, u16),
expo: bool, expo: bool,
expo_lut: &[u16; AXIS_MAX as usize + 1], expo_lut: &[u16; ADC_MAX as usize + 1],
) -> u16 { ) -> u16 {
if value <= min { if value <= min {
return AXIS_MIN; return ADC_MIN;
} }
if value >= max { if value >= max {
return AXIS_MAX; return ADC_MAX;
} }
let mut calibrated_value = AXIS_CENTER; let mut calibrated_value = AXIS_CENTER;
@ -946,14 +946,14 @@ fn calculate_axis_value(
center + deadzone.1, center + deadzone.1,
max - deadzone.2, max - deadzone.2,
AXIS_CENTER, AXIS_CENTER,
AXIS_MAX, ADC_MAX,
); );
} else if value < (center - deadzone.1) { } else if value < (center - deadzone.1) {
calibrated_value = remap( calibrated_value = remap(
value, value,
min + deadzone.0, min + deadzone.0,
center - deadzone.1, center - deadzone.1,
AXIS_MIN, ADC_MIN,
AXIS_CENTER, AXIS_CENTER,
); );
} }
@ -1003,14 +1003,14 @@ fn constrain<T: PartialOrd>(value: T, out_min: T, out_max: T) -> T {
/// ///
/// # Arguments /// # Arguments
/// * `expo` - Exponential curve factor (range 0.0 - 1.0) /// * `expo` - Exponential curve factor (range 0.0 - 1.0)
fn generate_expo_lut(expo: f32) -> [u16; AXIS_MAX as usize + 1] { fn generate_expo_lut(expo: f32) -> [u16; ADC_MAX as usize + 1] {
let mut lut: [u16; AXIS_MAX as usize + 1] = [0; AXIS_MAX as usize + 1]; let mut lut: [u16; ADC_MAX as usize + 1] = [0; ADC_MAX as usize + 1];
for i in 0..AXIS_MAX + 1 { for i in 0..ADC_MAX + 1 {
let value_float = i as f32 / AXIS_MAX as f32; let value_float = i as f32 / ADC_MAX as f32;
// Calculate expo using 9th order polynomial function with 0.5 as center point // Calculate expo using 9th order polynomial function with 0.5 as center point
let value_exp: f32 = let value_exp: f32 =
expo * (0.5 + 256.0 * powf(value_float - 0.5, 9.0)) + (1.0 - expo) * value_float; 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 lut
} }