Fixed Warning and throttle hold issue
This commit is contained in:
parent
87cb98a100
commit
d4218641e8
@ -3,10 +3,13 @@
|
||||
//! Handles gimbal axis processing, virtual axis management, throttle hold system,
|
||||
//! 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::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;
|
||||
|
||||
// ==================== AXIS CONSTANTS ====================
|
||||
@ -23,14 +26,13 @@ pub const GIMBAL_MODE_M7: u8 = 1;
|
||||
#[derive(Copy, Clone)]
|
||||
pub struct GimbalAxis {
|
||||
pub value: u16,
|
||||
pub value_before_hold: u16,
|
||||
pub previous_value: u16,
|
||||
pub idle_value: u16,
|
||||
pub max: u16,
|
||||
pub min: u16,
|
||||
pub center: u16,
|
||||
pub deadzone: (u16, u16, u16),
|
||||
pub expo: bool,
|
||||
pub trim: i16,
|
||||
pub hold: u16,
|
||||
pub hold_pending: bool,
|
||||
}
|
||||
@ -39,14 +41,13 @@ impl Default for GimbalAxis {
|
||||
fn default() -> Self {
|
||||
GimbalAxis {
|
||||
value: AXIS_CENTER,
|
||||
value_before_hold: AXIS_CENTER,
|
||||
previous_value: AXIS_CENTER,
|
||||
idle_value: AXIS_CENTER,
|
||||
max: ADC_MAX,
|
||||
min: ADC_MIN,
|
||||
center: AXIS_CENTER,
|
||||
deadzone: (100, 50, 100),
|
||||
expo: true,
|
||||
trim: 0,
|
||||
hold: AXIS_CENTER,
|
||||
hold_pending: false,
|
||||
}
|
||||
@ -62,16 +63,15 @@ impl GimbalAxis {
|
||||
/// Create a new GimbalAxis with calibration data
|
||||
pub fn new_with_calibration(min: u16, max: u16, center: u16) -> Self {
|
||||
let mut axis = Self::new();
|
||||
axis.calibrate(min, max, center);
|
||||
axis.set_calibration(min, max, center);
|
||||
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.max = max;
|
||||
self.center = center;
|
||||
self.idle_value = center;
|
||||
}
|
||||
|
||||
/// Process filtered ADC value through calibration and expo curve
|
||||
@ -89,6 +89,7 @@ impl GimbalAxis {
|
||||
self.expo,
|
||||
expo_lut,
|
||||
);
|
||||
self.value_before_hold = self.value;
|
||||
}
|
||||
|
||||
/// Set axis hold at current value
|
||||
@ -97,24 +98,6 @@ impl GimbalAxis {
|
||||
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)
|
||||
pub fn check_activity(&mut self) -> bool {
|
||||
let activity = self.value != self.previous_value;
|
||||
@ -122,38 +105,16 @@ impl GimbalAxis {
|
||||
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)
|
||||
pub fn process_throttle_hold(&mut self) {
|
||||
if self.hold == AXIS_CENTER {
|
||||
return; // No hold value set
|
||||
}
|
||||
|
||||
if self.value < AXIS_CENTER && !self.hold_pending {
|
||||
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 {
|
||||
pub fn process_throttle_hold(&mut self, throttle_hold_enable: bool) {
|
||||
if throttle_hold_enable && self.value < AXIS_CENTER && !self.hold_pending {
|
||||
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);
|
||||
} else if throttle_hold_enable && self.value == AXIS_CENTER {
|
||||
self.value = self.hold;
|
||||
self.hold_pending = false;
|
||||
} else {
|
||||
} else if throttle_hold_enable {
|
||||
self.value = self.hold;
|
||||
}
|
||||
}
|
||||
@ -227,10 +188,16 @@ pub struct AxisManager {
|
||||
pub throttle_hold_enable: bool,
|
||||
}
|
||||
|
||||
impl Default for AxisManager {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl AxisManager {
|
||||
pub fn new() -> Self {
|
||||
Self {
|
||||
axes: [Default::default(); NBR_OF_GIMBAL_AXIS],
|
||||
axes: [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS],
|
||||
virtual_ry: VirtualAxis::new(5),
|
||||
virtual_rz: VirtualAxis::new(5),
|
||||
gimbal_mode: GIMBAL_MODE_M10,
|
||||
@ -256,7 +223,11 @@ impl AxisManager {
|
||||
}
|
||||
|
||||
/// 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_Y].tick(raw_values[GIMBAL_AXIS_LEFT_Y] 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
|
||||
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() {
|
||||
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) {
|
||||
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
|
||||
pub fn process_throttle_hold(&mut self) {
|
||||
if self.throttle_hold_enable {
|
||||
self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold();
|
||||
}
|
||||
self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold(self.throttle_hold_enable);
|
||||
}
|
||||
|
||||
/// 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
|
||||
/// 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;
|
||||
|
||||
// Update Virtual RY
|
||||
@ -323,21 +306,15 @@ impl AxisManager {
|
||||
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) {
|
||||
self.axes[GIMBAL_AXIS_LEFT_Y].set_hold(hold_value);
|
||||
}
|
||||
|
||||
/// Reset axis holds when calibration is active
|
||||
pub fn reset_holds(&mut self) {
|
||||
for axis in self.axes.iter_mut() {
|
||||
axis.reset_hold();
|
||||
}
|
||||
}
|
||||
|
||||
/// Get unprocessed value for special button combinations
|
||||
pub fn get_unprocessed_value(&self) -> u16 {
|
||||
self.axes[GIMBAL_AXIS_LEFT_Y].value
|
||||
/// Get unprocessed value for special button combinations (original logic)
|
||||
pub fn get_value_before_hold(&self) -> u16 {
|
||||
// Original code captured axis.value BEFORE throttle hold was applied
|
||||
self.axes[GIMBAL_AXIS_LEFT_Y].value_before_hold
|
||||
}
|
||||
|
||||
/// Get virtual RY axis value for joystick report
|
||||
@ -369,7 +346,6 @@ impl AxisManager {
|
||||
|
||||
// ==================== AXIS PROCESSING FUNCTIONS ====================
|
||||
|
||||
|
||||
/// Remapping values from one range to another
|
||||
///
|
||||
/// # Arguments
|
||||
@ -413,7 +389,7 @@ pub fn calculate_axis_value(
|
||||
expo: bool,
|
||||
expo_lut: &ExpoLUT,
|
||||
) -> u16 {
|
||||
use crate::hardware::{ADC_MIN, ADC_MAX, AXIS_CENTER};
|
||||
use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER};
|
||||
|
||||
if value <= min {
|
||||
return ADC_MIN;
|
||||
@ -482,7 +458,6 @@ mod tests {
|
||||
assert_eq!(axis.min, 100);
|
||||
assert_eq!(axis.max, 3900);
|
||||
assert_eq!(axis.center, 2000);
|
||||
assert_eq!(axis.idle_value, 2000);
|
||||
}
|
||||
|
||||
#[test]
|
||||
@ -492,7 +467,6 @@ mod tests {
|
||||
assert_eq!(axis.min, 200);
|
||||
assert_eq!(axis.max, 3800);
|
||||
assert_eq!(axis.center, 1900);
|
||||
assert_eq!(axis.idle_value, 1900);
|
||||
}
|
||||
|
||||
#[test]
|
||||
@ -690,7 +664,8 @@ mod tests {
|
||||
manager.update_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.update_throttle_hold_enable();
|
||||
assert!(manager.throttle_hold_enable);
|
||||
@ -743,5 +718,4 @@ mod tests {
|
||||
let result = remap(25, 0, 100, 100, 0);
|
||||
assert_eq!(result, 100);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -83,7 +83,6 @@ pub const USB_HAT_RIGHT: usize = 34;
|
||||
pub const USB_HAT_DOWN: usize = 35;
|
||||
pub const USB_HAT_LEFT: usize = 36;
|
||||
|
||||
pub const USB_MIN_HOLD_MS: u32 = 50;
|
||||
|
||||
use crate::buttons::Button;
|
||||
|
||||
|
||||
@ -18,7 +18,6 @@ pub struct Button {
|
||||
pub pressed: bool,
|
||||
pub previous_pressed: bool,
|
||||
pub usb_changed: bool,
|
||||
pub usb_changed_to_pressed: bool,
|
||||
pub usb_button: usize, // For short press
|
||||
pub usb_button_long: usize, // For long press
|
||||
pub enable_long_press: bool, // Flag to enable special behavior
|
||||
@ -39,6 +38,7 @@ pub enum SpecialAction {
|
||||
None,
|
||||
Bootloader,
|
||||
StartCalibration,
|
||||
CancelCalibration,
|
||||
ThrottleHold(u16), // Value to hold
|
||||
VirtualThrottleToggle,
|
||||
}
|
||||
@ -49,6 +49,12 @@ pub struct ButtonManager {
|
||||
pub buttons: [Button; TOTAL_BUTTONS],
|
||||
}
|
||||
|
||||
impl Default for ButtonManager {
|
||||
fn default() -> Self {
|
||||
Self::new()
|
||||
}
|
||||
}
|
||||
|
||||
impl ButtonManager {
|
||||
pub fn new() -> Self {
|
||||
let mut buttons = [Button::default(); TOTAL_BUTTONS];
|
||||
@ -139,7 +145,7 @@ impl ButtonManager {
|
||||
}
|
||||
|
||||
/// 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
|
||||
if self.buttons[BUTTON_FRONT_LEFT_LOWER].pressed
|
||||
&& self.buttons[BUTTON_TOP_LEFT_MODE].pressed
|
||||
@ -148,31 +154,33 @@ impl ButtonManager {
|
||||
return SpecialAction::Bootloader;
|
||||
}
|
||||
|
||||
// Calibration of center position
|
||||
// Calibration mode toggle (start/cancel with same button combination)
|
||||
if self.buttons[BUTTON_FRONT_LEFT_UPPER].pressed
|
||||
&& self.buttons[BUTTON_TOP_LEFT_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
|
||||
if let Some(th_button) = self.get_button_press_event(TH_BUTTON) {
|
||||
if th_button {
|
||||
if let Some(th_button) = self.get_button_press_event(TH_BUTTON)
|
||||
&& th_button {
|
||||
if unprocessed_axis_value != AXIS_CENTER {
|
||||
return SpecialAction::ThrottleHold(unprocessed_axis_value);
|
||||
} else {
|
||||
return SpecialAction::ThrottleHold(AXIS_CENTER);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Check for virtual throttle button press
|
||||
if let Some(vt_button) = self.get_button_press_event(VT_BUTTON) {
|
||||
if vt_button {
|
||||
if let Some(vt_button) = self.get_button_press_event(VT_BUTTON)
|
||||
&& vt_button {
|
||||
return SpecialAction::VirtualThrottleToggle;
|
||||
}
|
||||
}
|
||||
|
||||
SpecialAction::None
|
||||
}
|
||||
@ -293,7 +301,7 @@ mod tests {
|
||||
manager.buttons[BUTTON_TOP_LEFT_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);
|
||||
}
|
||||
|
||||
@ -306,7 +314,7 @@ mod tests {
|
||||
manager.buttons[BUTTON_TOP_LEFT_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);
|
||||
}
|
||||
|
||||
@ -316,7 +324,7 @@ mod tests {
|
||||
manager.buttons[TH_BUTTON].pressed = true;
|
||||
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));
|
||||
}
|
||||
|
||||
@ -327,7 +335,7 @@ mod tests {
|
||||
manager.buttons[TH_BUTTON].previous_pressed = false;
|
||||
|
||||
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));
|
||||
}
|
||||
|
||||
@ -337,7 +345,7 @@ mod tests {
|
||||
manager.buttons[VT_BUTTON].pressed = true;
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
@ -3,9 +3,9 @@
|
||||
//! Handles axis calibration, gimbal mode selection, and calibration data persistence.
|
||||
//! 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::button_config::{BUTTON_TOP_LEFT_UP, BUTTON_TOP_LEFT_DOWN, BUTTON_TOP_RIGHT_HAT};
|
||||
use crate::hardware::NBR_OF_GIMBAL_AXIS;
|
||||
use crate::storage;
|
||||
use dyn_smooth::DynamicSmootherEcoI32;
|
||||
@ -52,7 +52,11 @@ impl CalibrationManager {
|
||||
}
|
||||
|
||||
/// 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 {
|
||||
return;
|
||||
}
|
||||
@ -69,7 +73,12 @@ impl CalibrationManager {
|
||||
|
||||
/// Process gimbal mode selection and center position setting
|
||||
/// 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 {
|
||||
return false;
|
||||
}
|
||||
@ -92,9 +101,14 @@ impl CalibrationManager {
|
||||
|
||||
/// Save calibration data to storage
|
||||
/// 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
|
||||
F: FnMut(u32, &[u8]) -> Result<(), ()>
|
||||
F: FnMut(u32, &[u8]) -> Result<(), ()>,
|
||||
{
|
||||
if !self.active || !buttons[BUTTON_TOP_RIGHT_HAT].pressed {
|
||||
return false;
|
||||
@ -116,19 +130,12 @@ impl CalibrationManager {
|
||||
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
|
||||
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() {
|
||||
let center_value = smoothers[index].value() as u16;
|
||||
axis.center = center_value;
|
||||
@ -301,36 +308,6 @@ mod tests {
|
||||
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]
|
||||
fn test_mode_selection_m10() {
|
||||
let mut manager = CalibrationManager::new();
|
||||
@ -447,4 +424,5 @@ mod tests {
|
||||
assert!(!write_called);
|
||||
assert!(manager.is_active()); // Should remain active
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -64,7 +64,7 @@ mod storage;
|
||||
mod usb_joystick_device;
|
||||
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_matrix::ButtonMatrix;
|
||||
use buttons::{ButtonManager, SpecialAction};
|
||||
@ -115,7 +115,7 @@ pub static BOOT2_FIRMWARE: [u8; 256] = rp2040_boot2::BOOT_LOADER_W25Q080;
|
||||
use expo::ExpoLUT;
|
||||
|
||||
/// 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};
|
||||
|
||||
/// Digital signal processing configuration for analog smoothing filters.
|
||||
@ -295,7 +295,6 @@ fn main() -> ! {
|
||||
|
||||
let mut usb_activity: bool = false;
|
||||
let mut usb_active: bool = false;
|
||||
let throttle_hold_enable: bool = false;
|
||||
let mut vt_enable: bool = false;
|
||||
|
||||
let mut axis_manager = AxisManager::new();
|
||||
@ -363,9 +362,7 @@ fn main() -> ! {
|
||||
let mut read_fn = |addr: u32| eeprom.read_byte(addr).map_err(|_| ());
|
||||
match storage::read_axis_calibration(&mut read_fn, index) {
|
||||
Ok((min, max, center)) => {
|
||||
item.min = min;
|
||||
item.max = max;
|
||||
item.center = center;
|
||||
*item = GimbalAxis::new_with_calibration(min, max, center);
|
||||
}
|
||||
Err(_) => {
|
||||
// Use factory defaults if EEPROM read fails
|
||||
@ -433,7 +430,7 @@ fn main() -> ! {
|
||||
let system_state = SystemState {
|
||||
usb_active,
|
||||
calibration_active: calibration_manager.is_active(),
|
||||
throttle_hold_enable,
|
||||
throttle_hold_enable: axis_manager.throttle_hold_enable,
|
||||
vt_enable,
|
||||
};
|
||||
status_led.update_from_system_state(
|
||||
@ -457,8 +454,10 @@ fn main() -> ! {
|
||||
button_manager.filter_hat_switches();
|
||||
|
||||
// Process special button combinations for system control
|
||||
let unprocessed_value = smoother[GIMBAL_AXIS_LEFT_Y].value() as u16;
|
||||
match button_manager.check_special_combinations(unprocessed_value) {
|
||||
let value_before_hold = axis_manager.get_value_before_hold();
|
||||
match button_manager
|
||||
.check_special_combinations(value_before_hold, calibration_manager.is_active())
|
||||
{
|
||||
SpecialAction::Bootloader => {
|
||||
status_led.update(StatusMode::Bootloader);
|
||||
let gpio_activity_pin_mask: u32 = 0;
|
||||
@ -474,8 +473,12 @@ fn main() -> ! {
|
||||
item.min = item.center;
|
||||
item.max = item.center;
|
||||
}
|
||||
axis_manager.clear_throttle_hold(); // Clear throttle hold when cancelling calibration
|
||||
calibration_manager.start_calibration();
|
||||
}
|
||||
SpecialAction::CancelCalibration => {
|
||||
calibration_manager.stop_calibration();
|
||||
}
|
||||
SpecialAction::ThrottleHold(hold_value) => {
|
||||
axis_manager.set_throttle_hold(hold_value);
|
||||
}
|
||||
@ -516,8 +519,8 @@ fn main() -> ! {
|
||||
|
||||
// Process gimbal axes through calibration, expo curves, and scaling
|
||||
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
|
||||
axis_manager.process_throttle_hold();
|
||||
|
||||
@ -527,11 +530,8 @@ fn main() -> ! {
|
||||
}
|
||||
|
||||
// Detect axis movement for USB activity signaling
|
||||
for item in axis_manager.axes.iter_mut() {
|
||||
if item.value != item.previous_value {
|
||||
usb_activity = true;
|
||||
}
|
||||
item.previous_value = item.value;
|
||||
if axis_manager.check_activity() {
|
||||
usb_activity = true;
|
||||
}
|
||||
|
||||
// Process button logic (press types, timing, USB mapping)
|
||||
@ -539,9 +539,6 @@ fn main() -> ! {
|
||||
if button_manager.process_button_logic(current_time) {
|
||||
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)
|
||||
|
||||
@ -6,8 +6,6 @@ use crate::hardware::EEPROM_DATA_LENGTH;
|
||||
|
||||
// ==================== 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)
|
||||
pub const GIMBAL_MODE_OFFSET: u32 = EEPROM_DATA_LENGTH as u32; // Address 25
|
||||
@ -137,7 +135,7 @@ mod tests {
|
||||
#[test]
|
||||
fn test_eeprom_data_layout_constants() {
|
||||
// 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]
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user