674 lines
22 KiB
Rust
674 lines
22 KiB
Rust
//! Axis processing for CMDR Joystick 25
|
||
//!
|
||
//! Responsibilities
|
||
//! - Apply gimbal mode compensation (M10/M7) to raw ADC readings
|
||
//! - Feed smoothed samples into per‑axis filters
|
||
//! - Calibrate + apply deadzones + optional expo via lookup table
|
||
//! - Implement throttle‑hold remapping on the throttle axis
|
||
//! - Maintain virtual axes updated by button presses
|
||
//! - Detect activity to throttle USB traffic
|
||
//!
|
||
//! Data flow
|
||
//! raw ADC → gimbal compensation → smoothing → `GimbalAxis::process_value`
|
||
//! → throttle hold → activity detection → USB report conversion
|
||
|
||
use crate::buttons::{Button, TOTAL_BUTTONS};
|
||
use crate::expo::{constrain, ExpoLUT};
|
||
use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER, NBR_OF_GIMBAL_AXIS};
|
||
use crate::mapping::{
|
||
BUTTON_FRONT_LEFT_EXTRA, BUTTON_FRONT_LEFT_LOWER, BUTTON_FRONT_LEFT_UPPER,
|
||
BUTTON_FRONT_RIGHT_EXTRA,
|
||
};
|
||
use dyn_smooth::{DynamicSmootherEcoI32, I32_FRAC_BITS};
|
||
|
||
// ==================== AXIS CONSTANTS ====================
|
||
|
||
pub const GIMBAL_AXIS_LEFT_X: usize = 0;
|
||
pub const GIMBAL_AXIS_LEFT_Y: usize = 1;
|
||
pub const GIMBAL_AXIS_RIGHT_X: usize = 2;
|
||
pub const GIMBAL_AXIS_RIGHT_Y: usize = 3;
|
||
pub const GIMBAL_MODE_M10: u8 = 0;
|
||
pub const GIMBAL_MODE_M7: u8 = 1;
|
||
|
||
/// Digital smoothing parameters for `DynamicSmootherEcoI32` filters.
|
||
/// Reduce ADC noise and jitter for stable axis values.
|
||
pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
|
||
pub const SAMPLE_FREQ: i32 = 1000 << I32_FRAC_BITS;
|
||
pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
|
||
|
||
// ==================== AXIS STRUCTS ====================
|
||
|
||
#[derive(Copy, Clone)]
|
||
pub struct GimbalAxis {
|
||
pub value: u16,
|
||
pub value_before_hold: u16,
|
||
pub previous_value: u16,
|
||
pub max: u16,
|
||
pub min: u16,
|
||
pub center: u16,
|
||
pub deadzone: (u16, u16, u16),
|
||
pub expo: bool,
|
||
pub hold: u16,
|
||
pub hold_pending: bool,
|
||
}
|
||
|
||
impl Default for GimbalAxis {
|
||
fn default() -> Self {
|
||
GimbalAxis {
|
||
value: AXIS_CENTER,
|
||
value_before_hold: AXIS_CENTER,
|
||
previous_value: AXIS_CENTER,
|
||
max: ADC_MAX,
|
||
min: ADC_MIN,
|
||
center: AXIS_CENTER,
|
||
deadzone: (100, 50, 100),
|
||
expo: true,
|
||
hold: AXIS_CENTER,
|
||
hold_pending: false,
|
||
}
|
||
}
|
||
}
|
||
|
||
impl GimbalAxis {
|
||
/// Create a new GimbalAxis with default settings.
|
||
pub fn new() -> Self {
|
||
Self::default()
|
||
}
|
||
|
||
/// Create a new GimbalAxis with calibration data.
|
||
pub fn with_calibration(min: u16, max: u16, center: u16) -> Self {
|
||
let mut axis = Self::new();
|
||
axis.set_calibration(min, max, center);
|
||
axis
|
||
}
|
||
|
||
/// Apply calibration data to the axis.
|
||
pub fn set_calibration(&mut self, min: u16, max: u16, center: u16) {
|
||
self.min = min;
|
||
self.max = max;
|
||
self.center = center;
|
||
}
|
||
|
||
/// Process filtered ADC value through calibration, deadzone and optional expo.
|
||
pub fn process_value(&mut self, filtered_value: i32, expo_lut: &ExpoLUT) {
|
||
// Convert filtered value to u16 range
|
||
let raw_value = constrain(filtered_value, ADC_MIN as i32, ADC_MAX as i32) as u16;
|
||
|
||
// Apply calibration and expo processing
|
||
self.value = calculate_axis_value(
|
||
raw_value,
|
||
self.min,
|
||
self.max,
|
||
self.center,
|
||
self.deadzone,
|
||
self.expo,
|
||
expo_lut,
|
||
);
|
||
self.value_before_hold = self.value;
|
||
}
|
||
|
||
/// Set/arm a hold value for the axis (used by throttle hold).
|
||
pub fn set_hold(&mut self, value: u16) {
|
||
self.hold = value;
|
||
self.hold_pending = true;
|
||
}
|
||
|
||
/// Check if the axis value changed since the last probe (activity flag).
|
||
pub fn check_activity(&mut self) -> bool {
|
||
let activity = self.value != self.previous_value;
|
||
self.previous_value = self.value;
|
||
activity
|
||
}
|
||
|
||
/// Throttle‑hold remapping for throttle axis (center → hold; below/above remapped).
|
||
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 if throttle_hold_enable {
|
||
self.value = self.hold;
|
||
}
|
||
}
|
||
}
|
||
|
||
#[derive(Copy, Clone)]
|
||
pub struct VirtualAxis {
|
||
pub value: u16,
|
||
step: u16,
|
||
}
|
||
|
||
impl Default for VirtualAxis {
|
||
fn default() -> Self {
|
||
VirtualAxis {
|
||
value: AXIS_CENTER,
|
||
step: 5,
|
||
}
|
||
}
|
||
}
|
||
|
||
impl VirtualAxis {
|
||
pub fn new(step: u16) -> Self {
|
||
VirtualAxis {
|
||
value: AXIS_CENTER,
|
||
step,
|
||
}
|
||
}
|
||
|
||
/// Update virtual axis based on button inputs.
|
||
/// Returns true if USB activity should be signaled.
|
||
pub fn update(&mut self, up_pressed: bool, down_pressed: bool, _vt_enable: bool) -> bool {
|
||
let mut activity = false;
|
||
|
||
// Compensate value when changing direction
|
||
if up_pressed && !down_pressed && self.value < AXIS_CENTER {
|
||
self.value = AXIS_CENTER + (AXIS_CENTER - self.value) / 2;
|
||
} else if down_pressed && !up_pressed && self.value > AXIS_CENTER {
|
||
self.value = AXIS_CENTER - (self.value - AXIS_CENTER) / 2;
|
||
}
|
||
|
||
// Move virtual axis
|
||
if up_pressed && !down_pressed && self.value < ADC_MAX - self.step {
|
||
self.value += self.step;
|
||
activity = true;
|
||
} else if down_pressed && !up_pressed && self.value > ADC_MIN + self.step {
|
||
self.value -= self.step;
|
||
activity = true;
|
||
} else if (self.value != AXIS_CENTER && !up_pressed && !down_pressed)
|
||
|| (up_pressed && down_pressed)
|
||
{
|
||
// Return to center when no buttons are pressed or both are pressed
|
||
let before = self.value;
|
||
if self.value > AXIS_CENTER {
|
||
self.value = self.value.saturating_sub(self.step);
|
||
} else if self.value < AXIS_CENTER {
|
||
self.value = self.value.saturating_add(self.step);
|
||
}
|
||
activity |= self.value != before;
|
||
}
|
||
|
||
activity
|
||
}
|
||
}
|
||
|
||
// ==================== AXIS MANAGER ====================
|
||
|
||
pub struct AxisManager {
|
||
pub axes: [GimbalAxis; NBR_OF_GIMBAL_AXIS],
|
||
pub virtual_ry: VirtualAxis,
|
||
pub virtual_rz: VirtualAxis,
|
||
pub gimbal_mode: u8,
|
||
pub throttle_hold_enable: bool,
|
||
}
|
||
|
||
impl Default for AxisManager {
|
||
fn default() -> Self {
|
||
Self::new()
|
||
}
|
||
}
|
||
|
||
impl AxisManager {
|
||
pub fn new() -> Self {
|
||
Self {
|
||
axes: [GimbalAxis::new(); NBR_OF_GIMBAL_AXIS],
|
||
virtual_ry: VirtualAxis::new(5),
|
||
virtual_rz: VirtualAxis::new(5),
|
||
gimbal_mode: GIMBAL_MODE_M10,
|
||
throttle_hold_enable: false,
|
||
}
|
||
}
|
||
|
||
pub fn set_gimbal_mode(&mut self, mode: u8) {
|
||
self.gimbal_mode = mode;
|
||
}
|
||
|
||
/// Apply gimbal mode compensation to raw ADC values.
|
||
pub fn apply_gimbal_compensation(&self, raw_values: &mut [u16; 4]) {
|
||
if self.gimbal_mode == GIMBAL_MODE_M10 {
|
||
// Invert X1 and Y2 axis (M10 gimbals)
|
||
raw_values[GIMBAL_AXIS_LEFT_X] = ADC_MAX - raw_values[GIMBAL_AXIS_LEFT_X];
|
||
raw_values[GIMBAL_AXIS_RIGHT_Y] = ADC_MAX - raw_values[GIMBAL_AXIS_RIGHT_Y];
|
||
} else if self.gimbal_mode == GIMBAL_MODE_M7 {
|
||
// Invert Y1 and X2 axis (M7 gimbals)
|
||
raw_values[GIMBAL_AXIS_LEFT_Y] = ADC_MAX - raw_values[GIMBAL_AXIS_LEFT_Y];
|
||
raw_values[GIMBAL_AXIS_RIGHT_X] = ADC_MAX - raw_values[GIMBAL_AXIS_RIGHT_X];
|
||
}
|
||
}
|
||
|
||
/// Tick smoothing filters with latest raw samples.
|
||
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);
|
||
smoother[GIMBAL_AXIS_RIGHT_Y].tick(raw_values[GIMBAL_AXIS_RIGHT_Y] as i32);
|
||
}
|
||
|
||
/// Update all axes from smoothers (calibration, deadzone, expo) and apply holds.
|
||
pub fn process_axis_values(
|
||
&mut self,
|
||
smoother: &[DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS],
|
||
expo_lut: &ExpoLUT,
|
||
) -> bool {
|
||
for (index, axis) in self.axes.iter_mut().enumerate() {
|
||
axis.process_value(smoother[index].value(), expo_lut);
|
||
}
|
||
self.process_throttle_hold();
|
||
self.check_activity()
|
||
}
|
||
|
||
/// Apply throttle‑hold to the left‑Y axis and derive enable flag.
|
||
pub fn process_throttle_hold(&mut self) {
|
||
self.throttle_hold_enable = self.axes[GIMBAL_AXIS_LEFT_Y].hold != AXIS_CENTER;
|
||
self.axes[GIMBAL_AXIS_LEFT_Y].process_throttle_hold(self.throttle_hold_enable);
|
||
}
|
||
|
||
/// Clear throttle‑hold state 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 {
|
||
let mut activity = false;
|
||
|
||
// Update Virtual RY
|
||
let ry_activity = self.virtual_ry.update(
|
||
buttons[BUTTON_FRONT_LEFT_UPPER].pressed,
|
||
buttons[BUTTON_FRONT_LEFT_LOWER].pressed,
|
||
vt_enable,
|
||
);
|
||
if ry_activity {
|
||
activity = true;
|
||
}
|
||
|
||
// Update Virtual RZ
|
||
let rz_activity = self.virtual_rz.update(
|
||
buttons[BUTTON_FRONT_RIGHT_EXTRA].pressed,
|
||
buttons[BUTTON_FRONT_LEFT_EXTRA].pressed,
|
||
vt_enable,
|
||
);
|
||
if rz_activity {
|
||
activity = true;
|
||
}
|
||
|
||
activity
|
||
}
|
||
|
||
/// Check for axis activity (movement from previous value).
|
||
pub fn check_activity(&mut self) -> bool {
|
||
let mut activity = false;
|
||
|
||
for axis in self.axes.iter_mut() {
|
||
if axis.check_activity() {
|
||
activity = true;
|
||
}
|
||
}
|
||
|
||
activity
|
||
}
|
||
|
||
/// 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);
|
||
}
|
||
|
||
/// Get pre‑hold 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.
|
||
pub fn get_virtual_ry_value(&self, expo_lut: &ExpoLUT) -> u16 {
|
||
calculate_axis_value(
|
||
self.virtual_ry.value,
|
||
ADC_MIN,
|
||
ADC_MAX,
|
||
AXIS_CENTER,
|
||
(0, 0, 0),
|
||
true,
|
||
expo_lut,
|
||
)
|
||
}
|
||
|
||
/// Get virtual RZ axis value for joystick report.
|
||
pub fn get_virtual_rz_value(&self, expo_lut: &ExpoLUT) -> u16 {
|
||
calculate_axis_value(
|
||
self.virtual_rz.value,
|
||
ADC_MIN,
|
||
ADC_MAX,
|
||
AXIS_CENTER,
|
||
(0, 0, 0),
|
||
true,
|
||
expo_lut,
|
||
)
|
||
}
|
||
|
||
/// Initialize digital smoothing filters for each gimbal axis.
|
||
/// Creates an array of `DynamicSmootherEcoI32` filters configured with shared DSP parameters.
|
||
pub fn create_smoothers() -> [DynamicSmootherEcoI32; NBR_OF_GIMBAL_AXIS] {
|
||
[
|
||
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
|
||
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
|
||
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
|
||
DynamicSmootherEcoI32::new(BASE_FREQ, SAMPLE_FREQ, SENSITIVITY),
|
||
]
|
||
}
|
||
}
|
||
|
||
// ==================== AXIS PROCESSING FUNCTIONS ====================
|
||
|
||
/// Remap a value from one range to another using integer math and clamping.
|
||
///
|
||
/// # Arguments
|
||
/// * `value` - Value to remap
|
||
/// * `in_min` - Lower bound of the value's current range
|
||
/// * `in_max` - Upper bound of the value's current range
|
||
/// * `out_min` - Lower bound of the value's target range
|
||
/// * `out_max` - Upper bound of the value's target range
|
||
pub fn remap(value: u16, in_min: u16, in_max: u16, out_min: u16, out_max: u16) -> u16 {
|
||
constrain(
|
||
(value as i64 - in_min as i64) * (out_max as i64 - out_min as i64)
|
||
/ (in_max as i64 - in_min as i64)
|
||
+ out_min as i64,
|
||
out_min as i64,
|
||
out_max as i64,
|
||
) as u16
|
||
}
|
||
|
||
/// Calculate calibrated axis value with deadzone and optional expo.
|
||
///
|
||
/// Process raw input through [min, center, max] calibration, apply deadzones,
|
||
/// then apply expo via LUT if enabled for smooth response.
|
||
///
|
||
/// # Arguments
|
||
/// * `value` - Raw axis value to process
|
||
/// * `min` - Lower bound of the axis calibrated range
|
||
/// * `max` - Upper bound of the axis calibrated range
|
||
/// * `center` - Center position of the axis calibrated range
|
||
/// * `deadzone` - Deadzone settings (min, center, max) for axis
|
||
/// * `expo` - Whether exponential curve is enabled for this axis
|
||
/// * `expo_lut` - Exponential curve lookup table for smooth response
|
||
///
|
||
/// # Returns
|
||
/// Processed axis value after calibration, deadzone, and optional expo
|
||
pub fn calculate_axis_value(
|
||
value: u16,
|
||
min: u16,
|
||
max: u16,
|
||
center: u16,
|
||
deadzone: (u16, u16, u16),
|
||
expo: bool,
|
||
expo_lut: &ExpoLUT,
|
||
) -> u16 {
|
||
use crate::hardware::{ADC_MAX, ADC_MIN, AXIS_CENTER};
|
||
|
||
if value <= min {
|
||
return ADC_MIN;
|
||
}
|
||
if value >= max {
|
||
return ADC_MAX;
|
||
}
|
||
|
||
let mut calibrated_value = AXIS_CENTER;
|
||
|
||
if value > (center + deadzone.1) {
|
||
calibrated_value = remap(
|
||
value,
|
||
center + deadzone.1,
|
||
max - deadzone.2,
|
||
AXIS_CENTER,
|
||
ADC_MAX,
|
||
);
|
||
} else if value < (center - deadzone.1) {
|
||
calibrated_value = remap(
|
||
value,
|
||
min + deadzone.0,
|
||
center - deadzone.1,
|
||
ADC_MIN,
|
||
AXIS_CENTER,
|
||
);
|
||
}
|
||
|
||
if expo && calibrated_value != AXIS_CENTER {
|
||
calibrated_value = expo_lut.apply(calibrated_value);
|
||
}
|
||
|
||
calibrated_value
|
||
}
|
||
|
||
// ==================== TESTS ====================
|
||
|
||
#[cfg(all(test, feature = "std"))]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn test_gimbal_axis_default() {
|
||
let axis = GimbalAxis::default();
|
||
assert_eq!(axis.value, AXIS_CENTER);
|
||
assert_eq!(axis.min, ADC_MIN);
|
||
assert_eq!(axis.max, ADC_MAX);
|
||
assert_eq!(axis.center, AXIS_CENTER);
|
||
assert_eq!(axis.hold, AXIS_CENTER);
|
||
assert!(!axis.hold_pending);
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_axis_new() {
|
||
let axis = GimbalAxis::new();
|
||
assert_eq!(axis.value, AXIS_CENTER);
|
||
assert_eq!(axis.min, ADC_MIN);
|
||
assert_eq!(axis.max, ADC_MAX);
|
||
assert_eq!(axis.center, AXIS_CENTER);
|
||
assert!(!axis.hold_pending);
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_axis_with_calibration() {
|
||
let axis = GimbalAxis::with_calibration(100, 3900, 2000);
|
||
assert_eq!(axis.min, 100);
|
||
assert_eq!(axis.max, 3900);
|
||
assert_eq!(axis.center, 2000);
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_axis_activity_detection() {
|
||
let mut axis = GimbalAxis::new();
|
||
|
||
// Initially no activity (same as previous)
|
||
assert!(!axis.check_activity());
|
||
|
||
// Change value
|
||
axis.value = 3000;
|
||
assert!(axis.check_activity());
|
||
|
||
// No change from previous check
|
||
assert!(!axis.check_activity());
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_axis_throttle_hold_processing() {
|
||
let mut axis = GimbalAxis::new();
|
||
axis.set_hold(1500); // Set hold value below center
|
||
|
||
// Test when not held, no processing occurs
|
||
axis.hold = AXIS_CENTER;
|
||
axis.hold_pending = false;
|
||
axis.value = 1000;
|
||
axis.process_throttle_hold(true);
|
||
assert_eq!(axis.value, 1000); // Should remain unchanged
|
||
|
||
// Test when held but hold_pending = false, remapping occurs
|
||
axis.set_hold(1500);
|
||
axis.value = 1000;
|
||
axis.hold_pending = false; // This allows remapping
|
||
axis.process_throttle_hold(true);
|
||
let expected = remap(1000, ADC_MIN, AXIS_CENTER, ADC_MIN, 1500);
|
||
assert_eq!(axis.value, expected);
|
||
|
||
// Test center value gets hold value and clears pending flag
|
||
axis.set_hold(1500);
|
||
axis.value = AXIS_CENTER;
|
||
axis.process_throttle_hold(true);
|
||
assert_eq!(axis.value, 1500);
|
||
assert!(!axis.hold_pending); // Should clear pending flag
|
||
|
||
// Test when hold_pending = true, just uses hold value
|
||
axis.set_hold(1500);
|
||
axis.value = 2000;
|
||
axis.hold_pending = true;
|
||
axis.process_throttle_hold(true);
|
||
assert_eq!(axis.value, 1500);
|
||
}
|
||
|
||
#[test]
|
||
fn test_virtual_axis_default() {
|
||
let virtual_axis = VirtualAxis::default();
|
||
assert_eq!(virtual_axis.value, AXIS_CENTER);
|
||
assert_eq!(virtual_axis.step, 5);
|
||
}
|
||
|
||
#[test]
|
||
fn test_virtual_axis_movement_up() {
|
||
let mut virtual_axis = VirtualAxis::new(10);
|
||
|
||
// Test upward movement
|
||
let activity = virtual_axis.update(true, false, false);
|
||
assert!(activity);
|
||
assert_eq!(virtual_axis.value, AXIS_CENTER + 10);
|
||
}
|
||
|
||
#[test]
|
||
fn test_virtual_axis_movement_down() {
|
||
let mut virtual_axis = VirtualAxis::new(10);
|
||
|
||
// Test downward movement
|
||
let activity = virtual_axis.update(false, true, false);
|
||
assert!(activity);
|
||
assert_eq!(virtual_axis.value, AXIS_CENTER - 10);
|
||
}
|
||
|
||
#[test]
|
||
fn test_virtual_axis_return_to_center() {
|
||
let mut virtual_axis = VirtualAxis::new(10);
|
||
virtual_axis.value = AXIS_CENTER + 20;
|
||
|
||
// Test return to center when no buttons pressed
|
||
let activity = virtual_axis.update(false, false, false);
|
||
assert!(activity);
|
||
assert_eq!(virtual_axis.value, AXIS_CENTER + 10);
|
||
}
|
||
|
||
#[test]
|
||
fn test_virtual_axis_direction_compensation() {
|
||
let mut virtual_axis = VirtualAxis::new(10);
|
||
virtual_axis.value = AXIS_CENTER - 100;
|
||
|
||
// Test direction change compensation (includes compensation + movement)
|
||
virtual_axis.update(true, false, false);
|
||
let compensated = AXIS_CENTER + (AXIS_CENTER - (AXIS_CENTER - 100)) / 2;
|
||
let expected = compensated + 10; // Plus step movement
|
||
assert_eq!(virtual_axis.value, expected);
|
||
}
|
||
|
||
#[test]
|
||
fn test_axis_manager_creation() {
|
||
let manager = AxisManager::new();
|
||
assert_eq!(manager.axes.len(), NBR_OF_GIMBAL_AXIS);
|
||
assert_eq!(manager.gimbal_mode, GIMBAL_MODE_M10);
|
||
assert_eq!(manager.virtual_ry.value, AXIS_CENTER);
|
||
assert_eq!(manager.virtual_rz.value, AXIS_CENTER);
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_compensation_m10() {
|
||
let manager = AxisManager::new(); // Default is M10
|
||
let mut raw_values = [1000, 1500, 2000, 2500];
|
||
|
||
manager.apply_gimbal_compensation(&mut raw_values);
|
||
|
||
// M10 mode inverts X1 and Y2
|
||
assert_eq!(raw_values[GIMBAL_AXIS_LEFT_X], ADC_MAX - 1000);
|
||
assert_eq!(raw_values[GIMBAL_AXIS_LEFT_Y], 1500); // Not inverted
|
||
assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_X], 2000); // Not inverted
|
||
assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_Y], ADC_MAX - 2500);
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_compensation_m7() {
|
||
let mut manager = AxisManager::new();
|
||
manager.set_gimbal_mode(GIMBAL_MODE_M7);
|
||
let mut raw_values = [1000, 1500, 2000, 2500];
|
||
|
||
manager.apply_gimbal_compensation(&mut raw_values);
|
||
|
||
// M7 mode inverts Y1 and X2
|
||
assert_eq!(raw_values[GIMBAL_AXIS_LEFT_X], 1000); // Not inverted
|
||
assert_eq!(raw_values[GIMBAL_AXIS_LEFT_Y], ADC_MAX - 1500);
|
||
assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_X], ADC_MAX - 2000);
|
||
assert_eq!(raw_values[GIMBAL_AXIS_RIGHT_Y], 2500); // Not inverted
|
||
}
|
||
|
||
#[test]
|
||
fn test_axis_activity_detection() {
|
||
let mut manager = AxisManager::new();
|
||
|
||
// No activity initially
|
||
assert!(!manager.check_activity());
|
||
|
||
// Change value and check activity
|
||
manager.axes[0].value = 1000;
|
||
assert!(manager.check_activity());
|
||
|
||
// No activity after previous_value is updated
|
||
assert!(!manager.check_activity());
|
||
}
|
||
|
||
#[test]
|
||
fn test_calculate_axis_value_boundaries() {
|
||
let expo_lut = ExpoLUT::new(0.0); // No expo for testing
|
||
|
||
// Test min boundary
|
||
let result = calculate_axis_value(0, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
|
||
assert_eq!(result, ADC_MIN);
|
||
|
||
// Test max boundary
|
||
let result = calculate_axis_value(4000, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
|
||
assert_eq!(result, ADC_MAX);
|
||
}
|
||
|
||
#[test]
|
||
fn test_calculate_axis_value_deadzone() {
|
||
let expo_lut = ExpoLUT::new(0.0); // No expo for testing
|
||
|
||
// Test center deadzone
|
||
let result = calculate_axis_value(2000, 100, 3000, 2000, (50, 50, 50), false, &expo_lut);
|
||
assert_eq!(result, AXIS_CENTER);
|
||
}
|
||
|
||
#[test]
|
||
fn test_remap_function() {
|
||
// Test basic remapping
|
||
let result = remap(50, 0, 100, 0, 200);
|
||
assert_eq!(result, 100);
|
||
|
||
// Test reverse remapping (constrain limits to out_min when out_min > out_max)
|
||
let result = remap(25, 0, 100, 100, 0);
|
||
assert_eq!(result, 100);
|
||
}
|
||
}
|