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 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<T: PartialOrd>(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
}