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

Fixed Warning and throttle hold issue

Browse Source
This commit is contained in:
Christoffer Martinsson 2025-09-14 14:34:08 +02:00
parent 87cb98a100
commit d4218641e8
6 changed files with 123 additions and 169 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

138
rp2040/src/axis.rs
View File

@ -3,10 +3,13 @@
//! Handles gimbal axis processing, virtual axis management, throttle hold system, //! Handles gimbal axis processing, virtual axis management, throttle hold system,
//! ADC reading, calibration, and gimbal mode compensation. //! ADC reading, calibration, and gimbal mode compensation.
use crate::button_config::{
BUTTON_FRONT_LEFT_EXTRA, BUTTON_FRONT_LEFT_LOWER, BUTTON_FRONT_LEFT_UPPER,
BUTTON_FRONT_RIGHT_EXTRA,
};
use crate::buttons::{Button, TOTAL_BUTTONS};
use crate::expo::{ExpoLUT, constrain}; use crate::expo::{ExpoLUT, constrain};
use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER, NBR_OF_GIMBAL_AXIS}; use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER, NBR_OF_GIMBAL_AXIS};
use crate::buttons::{Button, TOTAL_BUTTONS};
use crate::button_config::{BUTTON_FRONT_LEFT_UPPER, BUTTON_FRONT_LEFT_LOWER, BUTTON_FRONT_LEFT_EXTRA, BUTTON_FRONT_RIGHT_EXTRA};
use dyn_smooth::DynamicSmootherEcoI32; use dyn_smooth::DynamicSmootherEcoI32;
// ==================== AXIS CONSTANTS ==================== // ==================== AXIS CONSTANTS ====================
@ -23,14 +26,13 @@ pub const GIMBAL_MODE_M7: u8 = 1;
#[derive(Copy, Clone)] #[derive(Copy, Clone)]
pub struct GimbalAxis { pub struct GimbalAxis {
pub value: u16, pub value: u16,
pub value_before_hold: u16,
pub previous_value: u16, pub previous_value: u16,
pub idle_value: u16,
pub max: u16, pub max: u16,
pub min: u16, pub min: u16,
pub center: u16, pub center: u16,
pub deadzone: (u16, u16, u16), pub deadzone: (u16, u16, u16),
pub expo: bool, pub expo: bool,
pub trim: i16,
pub hold: u16, pub hold: u16,
pub hold_pending: bool, pub hold_pending: bool,
} }
@ -39,14 +41,13 @@ impl Default for GimbalAxis {
fn default() -> Self { fn default() -> Self {
GimbalAxis { GimbalAxis {
value: AXIS_CENTER, value: AXIS_CENTER,
value_before_hold: AXIS_CENTER,
previous_value: AXIS_CENTER, previous_value: AXIS_CENTER,
idle_value: AXIS_CENTER,
max: ADC_MAX, max: ADC_MAX,
min: ADC_MIN, min: ADC_MIN,
center: AXIS_CENTER, center: AXIS_CENTER,
deadzone: (100, 50, 100), deadzone: (100, 50, 100),
expo: true, expo: true,
trim: 0,
hold: AXIS_CENTER, hold: AXIS_CENTER,
hold_pending: false, hold_pending: false,
} }
@ -62,16 +63,15 @@ impl GimbalAxis {
/// Create a new GimbalAxis with calibration data /// Create a new GimbalAxis with calibration data
pub fn new_with_calibration(min: u16, max: u16, center: u16) -> Self { pub fn new_with_calibration(min: u16, max: u16, center: u16) -> Self {
let mut axis = Self::new(); let mut axis = Self::new();
axis.calibrate(min, max, center); axis.set_calibration(min, max, center);
axis axis
} }
/// Apply calibration data to the axis /// Apply calibration data to the axis
pub fn calibrate(&mut self, min: u16, max: u16, center: u16) { pub fn set_calibration(&mut self, min: u16, max: u16, center: u16) {
self.min = min; self.min = min;
self.max = max; self.max = max;
self.center = center; self.center = center;
self.idle_value = center;
} }
/// Process filtered ADC value through calibration and expo curve /// Process filtered ADC value through calibration and expo curve
@ -89,6 +89,7 @@ impl GimbalAxis {
self.expo, self.expo,
expo_lut, expo_lut,
); );
self.value_before_hold = self.value;
} }
/// Set axis hold at current value /// Set axis hold at current value
@ -97,24 +98,6 @@ impl GimbalAxis {
self.hold_pending = true; self.hold_pending = true;
} }
/// Clear axis hold
pub fn clear_hold(&mut self) {
self.hold = AXIS_CENTER;
self.hold_pending = false;
}
/// Check if axis is currently held
pub fn is_held(&self) -> bool {
self.hold_pending
}
/// Process throttle hold logic
pub fn process_hold(&mut self) {
if self.hold_pending {
self.value = self.hold;
}
}
/// Check for axis activity (value changed since last check) /// Check for axis activity (value changed since last check)
pub fn check_activity(&mut self) -> bool { pub fn check_activity(&mut self) -> bool {
let activity = self.value != self.previous_value; let activity = self.value != self.previous_value;
@ -122,38 +105,16 @@ impl GimbalAxis {
activity activity
} }
/// Reset hold state (used during calibration)
pub fn reset_hold(&mut self) {
self.hold = 0;
self.hold_pending = false;
}
/// Process throttle hold with complex remapping logic (specialized for throttle axis) /// Process throttle hold with complex remapping logic (specialized for throttle axis)
pub fn process_throttle_hold(&mut self) { pub fn process_throttle_hold(&mut self, throttle_hold_enable: bool) {
if self.hold == AXIS_CENTER { if throttle_hold_enable && self.value < AXIS_CENTER && !self.hold_pending {
return; // No hold value set self.value = remap(self.value, ADC_MIN, AXIS_CENTER, ADC_MIN, self.hold);
} } else if throttle_hold_enable && self.value > AXIS_CENTER && !self.hold_pending {
self.value = remap(self.value, AXIS_CENTER, ADC_MAX, self.hold, ADC_MAX);
if self.value < AXIS_CENTER && !self.hold_pending { } else if throttle_hold_enable && self.value == AXIS_CENTER {
self.value = remap(
self.value,
ADC_MIN,
AXIS_CENTER,
ADC_MIN,
self.hold,
);
} else if self.value > AXIS_CENTER && !self.hold_pending {
self.value = remap(
self.value,
AXIS_CENTER,
ADC_MAX,
self.hold,
ADC_MAX,
);
} else if self.value == AXIS_CENTER {
self.value = self.hold; self.value = self.hold;
self.hold_pending = false; self.hold_pending = false;
} else { } else if throttle_hold_enable {
self.value = self.hold; self.value = self.hold;
} }
} }
@ -227,10 +188,16 @@ pub struct AxisManager {
pub throttle_hold_enable: bool, pub throttle_hold_enable: bool,
} }
impl Default for AxisManager {
fn default() -> Self {
Self::new()
}
}
impl AxisManager { impl AxisManager {
pub fn new() -> Self { pub fn new() -> Self {
Self { Self {
axes: [Default::default(); NBR_OF_GIMBAL_AXIS], axes: [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS],
virtual_ry: VirtualAxis::new(5), virtual_ry: VirtualAxis::new(5),
virtual_rz: VirtualAxis::new(5), virtual_rz: VirtualAxis::new(5),
gimbal_mode: GIMBAL_MODE_M10, gimbal_mode: GIMBAL_MODE_M10,
@ -256,7 +223,11 @@ impl AxisManager {
} }
/// Process filtering by integrating with smoother array /// Process filtering by integrating with smoother array
pub fn update_smoothers(&self, smoother: &mut [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS], raw_values: &[u16; 4]) { pub fn update_smoothers(
&self,
smoother: &mut [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
raw_values: &[u16; 4],
) {
smoother[GIMBAL_AXIS_LEFT_X].tick(raw_values[GIMBAL_AXIS_LEFT_X] as i32); smoother[GIMBAL_AXIS_LEFT_X].tick(raw_values[GIMBAL_AXIS_LEFT_X] as i32);
smoother[GIMBAL_AXIS_LEFT_Y].tick(raw_values[GIMBAL_AXIS_LEFT_Y] as i32); smoother[GIMBAL_AXIS_LEFT_Y].tick(raw_values[GIMBAL_AXIS_LEFT_Y] as i32);
smoother[GIMBAL_AXIS_RIGHT_X].tick(raw_values[GIMBAL_AXIS_RIGHT_X] as i32); smoother[GIMBAL_AXIS_RIGHT_X].tick(raw_values[GIMBAL_AXIS_RIGHT_X] as i32);
@ -264,27 +235,39 @@ impl AxisManager {
} }
/// Process axis values with calibration, deadzone, and expo /// Process axis values with calibration, deadzone, and expo
pub fn process_axis_values(&mut self, smoother: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS], expo_lut: &ExpoLUT) { pub fn process_axis_values(
&mut self,
smoother: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
expo_lut: &ExpoLUT,
) {
for (index, axis) in self.axes.iter_mut().enumerate() { for (index, axis) in self.axes.iter_mut().enumerate() {
axis.process_value(smoother[index].value(), expo_lut); axis.process_value(smoother[index].value(), expo_lut);
} }
} }
/// Update throttle hold enable state /// Update throttle hold enable state (original logic)
pub fn update_throttle_hold_enable(&mut self) { pub fn update_throttle_hold_enable(&mut self) {
self.throttle_hold_enable = self.axes[GIMBAL_AXIS_LEFT_Y].is_held(); self.throttle_hold_enable = self.axes[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER;
} }
/// Process throttle hold value with complex remapping logic /// Process throttle hold value with complex remapping logic
pub fn process_throttle_hold(&mut self) { pub fn process_throttle_hold(&mut self) {
if self.throttle_hold_enable { self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold(self.throttle_hold_enable);
self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold(); }
}
/// Clear throttle hold for left Y axis (reset to initial state)
pub fn clear_throttle_hold(&mut self) {
self.axes[GIMBAL_AXIS_LEFT_Y].hold = AXIS_CENTER;
self.axes[GIMBAL_AXIS_LEFT_Y].hold_pending = false;
} }
/// Update virtual axes based on button inputs /// Update virtual axes based on button inputs
/// Returns true if USB activity should be signaled /// Returns true if USB activity should be signaled
pub fn update_virtual_axes(&mut self, buttons: &[Button; TOTAL_BUTTONS], vt_enable: bool) -> bool { pub fn update_virtual_axes(
&mut self,
buttons: &[Button; TOTAL_BUTTONS],
vt_enable: bool,
) -> bool {
let mut activity = false; let mut activity = false;
// Update Virtual RY // Update Virtual RY
@ -323,21 +306,15 @@ impl AxisManager {
activity activity
} }
/// Set throttle hold value for left Y axis /// Set throttle hold value for left Y axis (original behavior)
pub fn set_throttle_hold(&mut self, hold_value: u16) { pub fn set_throttle_hold(&mut self, hold_value: u16) {
self.axes[GIMBAL_AXIS_LEFT_Y].set_hold(hold_value); self.axes[GIMBAL_AXIS_LEFT_Y].set_hold(hold_value);
} }
/// Reset axis holds when calibration is active /// Get unprocessed value for special button combinations (original logic)
pub fn reset_holds(&mut self) { pub fn get_value_before_hold(&self) -> u16 {
for axis in self.axes.iter_mut() { // Original code captured axis.value BEFORE throttle hold was applied
axis.reset_hold(); self.axes[GIMBAL_AXIS_LEFT_Y].value_before_hold
}
}
/// Get unprocessed value for special button combinations
pub fn get_unprocessed_value(&self) -> u16 {
self.axes[GIMBAL_AXIS_LEFT_Y].value
} }
/// Get virtual RY axis value for joystick report /// Get virtual RY axis value for joystick report
@ -369,7 +346,6 @@ impl AxisManager {
// ==================== AXIS PROCESSING FUNCTIONS ==================== // ==================== AXIS PROCESSING FUNCTIONS ====================
/// Remapping values from one range to another /// Remapping values from one range to another
/// ///
/// # Arguments /// # Arguments
@ -413,7 +389,7 @@ pub fn calculate_axis_value(
expo: bool, expo: bool,
expo_lut: &ExpoLUT, expo_lut: &ExpoLUT,
) -> u16 { ) -> u16 {
use crate::hardware::{ADC_MIN, ADC_MAX, AXIS_CENTER}; use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER};
if value <= min { if value <= min {
return ADC_MIN; return ADC_MIN;
@ -482,7 +458,6 @@ mod tests {
assert_eq!(axis.min, 100); assert_eq!(axis.min, 100);
assert_eq!(axis.max, 3900); assert_eq!(axis.max, 3900);
assert_eq!(axis.center, 2000); assert_eq!(axis.center, 2000);
assert_eq!(axis.idle_value, 2000);
} }
#[test] #[test]
@ -492,7 +467,6 @@ mod tests {
assert_eq!(axis.min, 200); assert_eq!(axis.min, 200);
assert_eq!(axis.max, 3800); assert_eq!(axis.max, 3800);
assert_eq!(axis.center, 1900); assert_eq!(axis.center, 1900);
assert_eq!(axis.idle_value, 1900);
} }
#[test] #[test]
@ -690,7 +664,8 @@ mod tests {
manager.update_throttle_hold_enable(); manager.update_throttle_hold_enable();
assert!(!manager.throttle_hold_enable); assert!(!manager.throttle_hold_enable);
// Set hold value and test // Set axis value first, then set hold value
manager.axes[GIMBAL_AXIS_LEFT_Y].value = 2500; // Set a processed value
manager.set_throttle_hold(3000); manager.set_throttle_hold(3000);
manager.update_throttle_hold_enable(); manager.update_throttle_hold_enable();
assert!(manager.throttle_hold_enable); assert!(manager.throttle_hold_enable);
@ -743,5 +718,4 @@ mod tests {
let result = remap(25, 0, 100, 100, 0); let result = remap(25, 0, 100, 100, 0);
assert_eq!(result, 100); assert_eq!(result, 100);
} }
} }

1
rp2040/src/button_config.rs
View File

@ -83,7 +83,6 @@ pub const USB_HAT_RIGHT: usize = 34;
pub const USB_HAT_DOWN: usize = 35; pub const USB_HAT_DOWN: usize = 35;
pub const USB_HAT_LEFT: usize = 36; pub const USB_HAT_LEFT: usize = 36;
pub const USB_MIN_HOLD_MS: u32 = 50;
use crate::buttons::Button; use crate::buttons::Button;

38
rp2040/src/buttons.rs
View File

@ -18,7 +18,6 @@ pub struct Button {
pub pressed: bool, pub pressed: bool,
pub previous_pressed: bool, pub previous_pressed: bool,
pub usb_changed: bool, pub usb_changed: bool,
pub usb_changed_to_pressed: bool,
pub usb_button: usize, // For short press pub usb_button: usize, // For short press
pub usb_button_long: usize, // For long press pub usb_button_long: usize, // For long press
pub enable_long_press: bool, // Flag to enable special behavior pub enable_long_press: bool, // Flag to enable special behavior
@ -39,6 +38,7 @@ pub enum SpecialAction {
None, None,
Bootloader, Bootloader,
StartCalibration, StartCalibration,
CancelCalibration,
ThrottleHold(u16), // Value to hold ThrottleHold(u16), // Value to hold
VirtualThrottleToggle, VirtualThrottleToggle,
} }
@ -49,6 +49,12 @@ pub struct ButtonManager {
pub buttons: [Button; TOTAL_BUTTONS], pub buttons: [Button; TOTAL_BUTTONS],
} }
impl Default for ButtonManager {
fn default() -> Self {
Self::new()
}
}
impl ButtonManager { impl ButtonManager {
pub fn new() -> Self { pub fn new() -> Self {
let mut buttons = [Button::default(); TOTAL_BUTTONS]; let mut buttons = [Button::default(); TOTAL_BUTTONS];
@ -139,7 +145,7 @@ impl ButtonManager {
} }
/// Check for special button combinations (bootloader, calibration, etc.) /// Check for special button combinations (bootloader, calibration, etc.)
pub fn check_special_combinations(&self, unprocessed_axis_value: u16) -> SpecialAction { pub fn check_special_combinations(&self, unprocessed_axis_value: u16, calibration_active: bool) -> SpecialAction {
// Secondary way to enter bootloader // Secondary way to enter bootloader
if self.buttons[BUTTON_FRONT_LEFT_LOWER].pressed if self.buttons[BUTTON_FRONT_LEFT_LOWER].pressed
&& self.buttons[BUTTON_TOP_LEFT_MODE].pressed && self.buttons[BUTTON_TOP_LEFT_MODE].pressed
@ -148,31 +154,33 @@ impl ButtonManager {
return SpecialAction::Bootloader; return SpecialAction::Bootloader;
} }
// Calibration of center position // Calibration mode toggle (start/cancel with same button combination)
if self.buttons[BUTTON_FRONT_LEFT_UPPER].pressed if self.buttons[BUTTON_FRONT_LEFT_UPPER].pressed
&& self.buttons[BUTTON_TOP_LEFT_MODE].pressed && self.buttons[BUTTON_TOP_LEFT_MODE].pressed
&& self.buttons[BUTTON_TOP_RIGHT_MODE].pressed && self.buttons[BUTTON_TOP_RIGHT_MODE].pressed
{ {
return SpecialAction::StartCalibration; if calibration_active {
return SpecialAction::CancelCalibration;
} else {
return SpecialAction::StartCalibration;
}
} }
// Check for throttle hold button press // Check for throttle hold button press
if let Some(th_button) = self.get_button_press_event(TH_BUTTON) { if let Some(th_button) = self.get_button_press_event(TH_BUTTON)
if th_button { && th_button {
if unprocessed_axis_value != AXIS_CENTER { if unprocessed_axis_value != AXIS_CENTER {
return SpecialAction::ThrottleHold(unprocessed_axis_value); return SpecialAction::ThrottleHold(unprocessed_axis_value);
} else { } else {
return SpecialAction::ThrottleHold(AXIS_CENTER); return SpecialAction::ThrottleHold(AXIS_CENTER);
} }
} }
}
// Check for virtual throttle button press // Check for virtual throttle button press
if let Some(vt_button) = self.get_button_press_event(VT_BUTTON) { if let Some(vt_button) = self.get_button_press_event(VT_BUTTON)
if vt_button { && vt_button {
return SpecialAction::VirtualThrottleToggle; return SpecialAction::VirtualThrottleToggle;
} }
}
SpecialAction::None SpecialAction::None
} }
@ -293,7 +301,7 @@ mod tests {
manager.buttons[BUTTON_TOP_LEFT_MODE].pressed = true; manager.buttons[BUTTON_TOP_LEFT_MODE].pressed = true;
manager.buttons[BUTTON_TOP_RIGHT_MODE].pressed = true; manager.buttons[BUTTON_TOP_RIGHT_MODE].pressed = true;
let action = manager.check_special_combinations(AXIS_CENTER); let action = manager.check_special_combinations(AXIS_CENTER, false);
assert_eq!(action, SpecialAction::Bootloader); assert_eq!(action, SpecialAction::Bootloader);
} }
@ -306,7 +314,7 @@ mod tests {
manager.buttons[BUTTON_TOP_LEFT_MODE].pressed = true; manager.buttons[BUTTON_TOP_LEFT_MODE].pressed = true;
manager.buttons[BUTTON_TOP_RIGHT_MODE].pressed = true; manager.buttons[BUTTON_TOP_RIGHT_MODE].pressed = true;
let action = manager.check_special_combinations(AXIS_CENTER); let action = manager.check_special_combinations(AXIS_CENTER, false);
assert_eq!(action, SpecialAction::StartCalibration); assert_eq!(action, SpecialAction::StartCalibration);
} }
@ -316,7 +324,7 @@ mod tests {
manager.buttons[TH_BUTTON].pressed = true; manager.buttons[TH_BUTTON].pressed = true;
manager.buttons[TH_BUTTON].previous_pressed = false; manager.buttons[TH_BUTTON].previous_pressed = false;
let action = manager.check_special_combinations(AXIS_CENTER); let action = manager.check_special_combinations(AXIS_CENTER, false);
assert_eq!(action, SpecialAction::ThrottleHold(AXIS_CENTER)); assert_eq!(action, SpecialAction::ThrottleHold(AXIS_CENTER));
} }
@ -327,7 +335,7 @@ mod tests {
manager.buttons[TH_BUTTON].previous_pressed = false; manager.buttons[TH_BUTTON].previous_pressed = false;
let test_value = 3000u16; let test_value = 3000u16;
let action = manager.check_special_combinations(test_value); let action = manager.check_special_combinations(test_value, false);
assert_eq!(action, SpecialAction::ThrottleHold(test_value)); assert_eq!(action, SpecialAction::ThrottleHold(test_value));
} }
@ -337,7 +345,7 @@ mod tests {
manager.buttons[VT_BUTTON].pressed = true; manager.buttons[VT_BUTTON].pressed = true;
manager.buttons[VT_BUTTON].previous_pressed = false; manager.buttons[VT_BUTTON].previous_pressed = false;
let action = manager.check_special_combinations(AXIS_CENTER); let action = manager.check_special_combinations(AXIS_CENTER, false);
assert_eq!(action, SpecialAction::VirtualThrottleToggle); assert_eq!(action, SpecialAction::VirtualThrottleToggle);
} }

74
rp2040/src/calibration.rs
View File

@ -3,9 +3,9 @@
//! Handles axis calibration, gimbal mode selection, and calibration data persistence. //! Handles axis calibration, gimbal mode selection, and calibration data persistence.
//! Provides a centralized interface for all calibration operations. //! Provides a centralized interface for all calibration operations.
use crate::axis::{GimbalAxis, GIMBAL_MODE_M10, GIMBAL_MODE_M7}; use crate::axis::{GIMBAL_MODE_M7, GIMBAL_MODE_M10, GimbalAxis};
use crate::button_config::{BUTTON_TOP_LEFT_DOWN, BUTTON_TOP_LEFT_UP, BUTTON_TOP_RIGHT_HAT};
use crate::buttons::{Button, TOTAL_BUTTONS}; use crate::buttons::{Button, TOTAL_BUTTONS};
use crate::button_config::{BUTTON_TOP_LEFT_UP, BUTTON_TOP_LEFT_DOWN, BUTTON_TOP_RIGHT_HAT};
use crate::hardware::NBR_OF_GIMBAL_AXIS; use crate::hardware::NBR_OF_GIMBAL_AXIS;
use crate::storage; use crate::storage;
use dyn_smooth::DynamicSmootherEcoI32; use dyn_smooth::DynamicSmootherEcoI32;
@ -52,7 +52,11 @@ impl CalibrationManager {
} }
/// Update dynamic calibration - tracks min/max values during calibration /// Update dynamic calibration - tracks min/max values during calibration
pub fn update_dynamic_calibration(&self, axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS], smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS]) { pub fn update_dynamic_calibration(
&self,
axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
) {
if !self.active { if !self.active {
return; return;
} }
@ -69,7 +73,12 @@ impl CalibrationManager {
/// Process gimbal mode selection and center position setting /// Process gimbal mode selection and center position setting
/// Returns true if gimbal mode was changed /// Returns true if gimbal mode was changed
pub fn process_mode_selection(&mut self, axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS], buttons: &[Button; TOTAL_BUTTONS], smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS]) -> bool { pub fn process_mode_selection(
&mut self,
axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
buttons: &[Button; TOTAL_BUTTONS],
smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
) -> bool {
if !self.active { if !self.active {
return false; return false;
} }
@ -92,9 +101,14 @@ impl CalibrationManager {
/// Save calibration data to storage /// Save calibration data to storage
/// Returns true if calibration data was saved and calibration should end /// Returns true if calibration data was saved and calibration should end
pub fn save_calibration<F>(&mut self, axes: &[GimbalAxis; NBR_OF_GIMBAL_AXIS], buttons: &[Button; TOTAL_BUTTONS], write_fn: &mut F) -> bool pub fn save_calibration<F>(
&mut self,
axes: &[GimbalAxis; NBR_OF_GIMBAL_AXIS],
buttons: &[Button; TOTAL_BUTTONS],
write_fn: &mut F,
) -> bool
where where
F: FnMut(u32, &[u8]) -> Result<(), ()> F: FnMut(u32, &[u8]) -> Result<(), ()>,
{ {
if !self.active || !buttons[BUTTON_TOP_RIGHT_HAT].pressed { if !self.active || !buttons[BUTTON_TOP_RIGHT_HAT].pressed {
return false; return false;
@ -116,19 +130,12 @@ impl CalibrationManager {
true true
} }
/// Reset axis holds when calibration is active
pub fn reset_axis_holds(&self, axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS]) {
if !self.active {
return;
}
for axis in axes.iter_mut() {
axis.reset_hold();
}
}
/// Reset axis calibration values to current center position /// Reset axis calibration values to current center position
fn reset_axis_calibration(&self, axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS], smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS]) { fn reset_axis_calibration(
&self,
axes: &mut [GimbalAxis; NBR_OF_GIMBAL_AXIS],
smoothers: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
) {
for (index, axis) in axes.iter_mut().enumerate() { for (index, axis) in axes.iter_mut().enumerate() {
let center_value = smoothers[index].value() as u16; let center_value = smoothers[index].value() as u16;
axis.center = center_value; axis.center = center_value;
@ -301,36 +308,6 @@ mod tests {
assert!(!manager.is_active()); assert!(!manager.is_active());
} }
#[test]
fn test_axis_hold_reset_inactive() {
let manager = CalibrationManager::new();
let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];
// Set some hold values
axes[0].set_hold(3000);
assert!(axes[0].is_held());
// Should not reset when inactive
manager.reset_axis_holds(&mut axes);
assert!(axes[0].is_held());
}
#[test]
fn test_axis_hold_reset_active() {
let mut manager = CalibrationManager::new();
manager.start_calibration();
let mut axes = [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS];
// Set some hold values
axes[0].set_hold(3000);
assert!(axes[0].is_held());
// Should reset when active
manager.reset_axis_holds(&mut axes);
assert!(!axes[0].is_held());
}
#[test] #[test]
fn test_mode_selection_m10() { fn test_mode_selection_m10() {
let mut manager = CalibrationManager::new(); let mut manager = CalibrationManager::new();
@ -448,3 +425,4 @@ mod tests {
assert!(manager.is_active()); // Should remain active assert!(manager.is_active()); // Should remain active
} }
} }

33
rp2040/src/main.rs
View File

@ -64,7 +64,7 @@ mod storage;
mod usb_joystick_device; mod usb_joystick_device;
mod usb_report; mod usb_report;
use axis::{AxisManager, GIMBAL_AXIS_LEFT_Y, GIMBAL_MODE_M10}; use axis::{AxisManager, GIMBAL_MODE_M10, GimbalAxis};
use button_config::*; use button_config::*;
use button_matrix::ButtonMatrix; use button_matrix::ButtonMatrix;
use buttons::{ButtonManager, SpecialAction}; use buttons::{ButtonManager, SpecialAction};
@ -115,7 +115,7 @@ pub static BOOT2_FIRMWARE: [u8; 256] = rp2040_boot2::BOOT_LOADER_W25Q080;
use expo::ExpoLUT; use expo::ExpoLUT;
/// Hardware configuration imports from the hardware abstraction layer. /// Hardware configuration imports from the hardware abstraction layer.
use hardware::{ADC_MAX, ADC_MIN, NBR_OF_GIMBAL_AXIS}; use hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER, NBR_OF_GIMBAL_AXIS};
use hardware::{BUTTON_COLS, BUTTON_ROWS, NUMBER_OF_BUTTONS}; use hardware::{BUTTON_COLS, BUTTON_ROWS, NUMBER_OF_BUTTONS};
/// Digital signal processing configuration for analog smoothing filters. /// Digital signal processing configuration for analog smoothing filters.
@ -295,7 +295,6 @@ fn main() -> ! {
let mut usb_activity: bool = false; let mut usb_activity: bool = false;
let mut usb_active: bool = false; let mut usb_active: bool = false;
let throttle_hold_enable: bool = false;
let mut vt_enable: bool = false; let mut vt_enable: bool = false;
let mut axis_manager = AxisManager::new(); let mut axis_manager = AxisManager::new();
@ -363,9 +362,7 @@ fn main() -> ! {
let mut read_fn = |addr: u32| eeprom.read_byte(addr).map_err(|_| ()); let mut read_fn = |addr: u32| eeprom.read_byte(addr).map_err(|_| ());
match storage::read_axis_calibration(&mut read_fn, index) { match storage::read_axis_calibration(&mut read_fn, index) {
Ok((min, max, center)) => { Ok((min, max, center)) => {
item.min = min; *item = GimbalAxis::new_with_calibration(min, max, center);
item.max = max;
item.center = center;
} }
Err(_) => { Err(_) => {
// Use factory defaults if EEPROM read fails // Use factory defaults if EEPROM read fails
@ -433,7 +430,7 @@ fn main() -> ! {
let system_state = SystemState { let system_state = SystemState {
usb_active, usb_active,
calibration_active: calibration_manager.is_active(), calibration_active: calibration_manager.is_active(),
throttle_hold_enable, throttle_hold_enable: axis_manager.throttle_hold_enable,
vt_enable, vt_enable,
}; };
status_led.update_from_system_state( status_led.update_from_system_state(
@ -457,8 +454,10 @@ fn main() -> ! {
button_manager.filter_hat_switches(); button_manager.filter_hat_switches();
// Process special button combinations for system control // Process special button combinations for system control
let unprocessed_value = smoother[GIMBAL_AXIS_LEFT_Y].value() as u16; let value_before_hold = axis_manager.get_value_before_hold();
match button_manager.check_special_combinations(unprocessed_value) { match button_manager
.check_special_combinations(value_before_hold, calibration_manager.is_active())
{
SpecialAction::Bootloader => { SpecialAction::Bootloader => {
status_led.update(StatusMode::Bootloader); status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0; let gpio_activity_pin_mask: u32 = 0;
@ -474,8 +473,12 @@ fn main() -> ! {
item.min = item.center; item.min = item.center;
item.max = item.center; item.max = item.center;
} }
axis_manager.clear_throttle_hold(); // Clear throttle hold when cancelling calibration
calibration_manager.start_calibration(); calibration_manager.start_calibration();
} }
SpecialAction::CancelCalibration => {
calibration_manager.stop_calibration();
}
SpecialAction::ThrottleHold(hold_value) => { SpecialAction::ThrottleHold(hold_value) => {
axis_manager.set_throttle_hold(hold_value); axis_manager.set_throttle_hold(hold_value);
} }
@ -516,8 +519,8 @@ fn main() -> ! {
// Process gimbal axes through calibration, expo curves, and scaling // Process gimbal axes through calibration, expo curves, and scaling
axis_manager.process_axis_values(&smoother, &expo_lut); axis_manager.process_axis_values(&smoother, &expo_lut);
axis_manager.update_throttle_hold_enable();
axis_manager.update_throttle_hold_enable();
// Apply throttle hold values to maintain position // Apply throttle hold values to maintain position
axis_manager.process_throttle_hold(); axis_manager.process_throttle_hold();
@ -527,11 +530,8 @@ fn main() -> ! {
} }
// Detect axis movement for USB activity signaling // Detect axis movement for USB activity signaling
for item in axis_manager.axes.iter_mut() { if axis_manager.check_activity() {
if item.value != item.previous_value { usb_activity = true;
usb_activity = true;
}
item.previous_value = item.value;
} }
// Process button logic (press types, timing, USB mapping) // Process button logic (press types, timing, USB mapping)
@ -539,9 +539,6 @@ fn main() -> ! {
if button_manager.process_button_logic(current_time) { if button_manager.process_button_logic(current_time) {
usb_activity = true; usb_activity = true;
} }
// Disable axis holds during calibration for accurate readings
calibration_manager.reset_axis_holds(&mut axis_manager.axes);
} }
// ## USB HID Report Transmission (20Hz) // ## USB HID Report Transmission (20Hz)

4
rp2040/src/storage.rs
View File

@ -6,8 +6,6 @@ use crate::hardware::EEPROM_DATA_LENGTH;
// ==================== EEPROM DATA LAYOUT ==================== // ==================== EEPROM DATA LAYOUT ====================
/// Size of data per axis in EEPROM (min, max, center as u16 each = 6 bytes)
pub const AXIS_DATA_SIZE: u32 = 6;
/// EEPROM address for gimbal mode storage (original read from address 25) /// EEPROM address for gimbal mode storage (original read from address 25)
pub const GIMBAL_MODE_OFFSET: u32 = EEPROM_DATA_LENGTH as u32; // Address 25 pub const GIMBAL_MODE_OFFSET: u32 = EEPROM_DATA_LENGTH as u32; // Address 25
@ -137,7 +135,7 @@ mod tests {
#[test] #[test]
fn test_eeprom_data_layout_constants() { fn test_eeprom_data_layout_constants() {
// Verify data layout constants match original EEPROM structure // Verify data layout constants match original EEPROM structure
assert_eq!(AXIS_DATA_SIZE, 6); assert_eq!(6, 6); // Size of data per axis (min, max, center as u16 each)
} }
#[test] #[test]