219 lines
7.5 KiB
Rust
219 lines
7.5 KiB
Rust
//! EEPROM storage for CMDR Joystick 25
|
||
//!
|
||
//! Provides helpers to read/write per‑axis calibration and gimbal mode to an
|
||
//! external 24x‑series EEPROM. The API is closure‑based to allow testing on
|
||
//! a host without hardware access.
|
||
|
||
use crate::hardware::EEPROM_DATA_LENGTH;
|
||
|
||
// ==================== EEPROM DATA LAYOUT ====================
|
||
/// EEPROM address for gimbal mode storage (original layout uses address 25).
|
||
pub const GIMBAL_MODE_OFFSET: u32 = EEPROM_DATA_LENGTH as u32; // Address 25
|
||
|
||
// Original format uses: base = axis_index * 6, addresses = base+1,2,3,4,5,6
|
||
|
||
// ==================== ERROR TYPES ====================
|
||
/// Errors returned by storage helpers.
|
||
#[derive(Debug)]
|
||
pub enum StorageError {
|
||
ReadError,
|
||
WriteError,
|
||
#[allow(dead_code)]
|
||
InvalidAxisIndex,
|
||
}
|
||
|
||
// ==================== CORE FUNCTIONS ====================
|
||
|
||
/// Read calibration data for a single axis from EEPROM.
|
||
/// Returns (min, max, center) values as u16.
|
||
pub fn read_axis_calibration(
|
||
read_byte_fn: &mut dyn FnMut(u32) -> Result<u8, ()>,
|
||
axis_index: usize,
|
||
) -> Result<(u16, u16, u16), StorageError> {
|
||
// Original format uses: base = axis_index * 6, addresses = base+1,2,3,4,5,6
|
||
let base = axis_index as u32 * 6;
|
||
|
||
let min = read_u16_with_closure(read_byte_fn, base + 1, base + 2)?;
|
||
let max = read_u16_with_closure(read_byte_fn, base + 3, base + 4)?;
|
||
let center = read_u16_with_closure(read_byte_fn, base + 5, base + 6)?;
|
||
|
||
Ok((min, max, center))
|
||
}
|
||
|
||
/// Read gimbal mode from EEPROM.
|
||
pub fn read_gimbal_mode(
|
||
read_byte_fn: &mut dyn FnMut(u32) -> Result<u8, ()>,
|
||
) -> Result<u8, StorageError> {
|
||
read_byte_fn(GIMBAL_MODE_OFFSET).map_err(|_| StorageError::ReadError)
|
||
}
|
||
|
||
/// Write all calibration data and gimbal mode to EEPROM.
|
||
#[allow(clippy::type_complexity)]
|
||
pub fn write_calibration_data(
|
||
write_page_fn: &mut dyn FnMut(u32, &[u8]) -> Result<(), ()>,
|
||
axis_data: &[(u16, u16, u16)],
|
||
gimbal_mode: u8,
|
||
) -> Result<(), StorageError> {
|
||
let mut eeprom_data: [u8; EEPROM_DATA_LENGTH] = [0; EEPROM_DATA_LENGTH];
|
||
|
||
// Pack all axis data into the buffer (original format)
|
||
for (index, &(min, max, center)) in axis_data.iter().enumerate() {
|
||
let base = index * 6;
|
||
// Original format: base+1,2,3,4,5,6
|
||
eeprom_data[base + 1] = min as u8;
|
||
eeprom_data[base + 2] = (min >> 8) as u8;
|
||
eeprom_data[base + 3] = max as u8;
|
||
eeprom_data[base + 4] = (max >> 8) as u8;
|
||
eeprom_data[base + 5] = center as u8;
|
||
eeprom_data[base + 6] = (center >> 8) as u8;
|
||
}
|
||
|
||
// Pack gimbal mode at the end
|
||
eeprom_data[EEPROM_DATA_LENGTH - 1] = gimbal_mode;
|
||
|
||
// Write entire page to EEPROM
|
||
write_page_fn(0x01, &eeprom_data).map_err(|_| StorageError::WriteError)
|
||
}
|
||
|
||
// ==================== HELPER FUNCTIONS ====================
|
||
|
||
/// Read a u16 value from EEPROM in little‑endian (low then high byte) format.
|
||
fn read_u16_with_closure(
|
||
read_byte_fn: &mut dyn FnMut(u32) -> Result<u8, ()>,
|
||
low_addr: u32,
|
||
high_addr: u32,
|
||
) -> Result<u16, StorageError> {
|
||
let low_byte = read_byte_fn(low_addr).map_err(|_| StorageError::ReadError)? as u16;
|
||
let high_byte = read_byte_fn(high_addr).map_err(|_| StorageError::ReadError)? as u16;
|
||
|
||
Ok(low_byte | (high_byte << 8))
|
||
}
|
||
|
||
// ==================== TESTS ====================
|
||
|
||
#[cfg(all(test, feature = "std"))]
|
||
mod tests {
|
||
use super::*;
|
||
|
||
#[test]
|
||
fn test_axis_address_calculation() {
|
||
// Test that axis addresses are calculated correctly (original format)
|
||
// Axis 0: base = 0 * 6 = 0, uses addresses 1,2,3,4,5,6
|
||
// Axis 1: base = 1 * 6 = 6, uses addresses 7,8,9,10,11,12
|
||
// Axis 2: base = 2 * 6 = 12, uses addresses 13,14,15,16,17,18
|
||
// Axis 3: base = 3 * 6 = 18, uses addresses 19,20,21,22,23,24
|
||
assert_eq!(0 * 6, 0);
|
||
assert_eq!(1 * 6, 6);
|
||
assert_eq!(2 * 6, 12);
|
||
assert_eq!(3 * 6, 18);
|
||
}
|
||
|
||
#[test]
|
||
fn test_u16_byte_packing_little_endian() {
|
||
// Test manual byte packing (original format)
|
||
let mut buffer = [0u8; 4];
|
||
|
||
let value = 0x1234u16;
|
||
buffer[0] = value as u8; // 0x34 (low byte)
|
||
buffer[1] = (value >> 8) as u8; // 0x12 (high byte)
|
||
assert_eq!(buffer[0], 0x34);
|
||
assert_eq!(buffer[1], 0x12);
|
||
|
||
let value = 0xABCDu16;
|
||
buffer[2] = value as u8; // 0xCD (low byte)
|
||
buffer[3] = (value >> 8) as u8; // 0xAB (high byte)
|
||
assert_eq!(buffer[2], 0xCD);
|
||
assert_eq!(buffer[3], 0xAB);
|
||
}
|
||
|
||
#[test]
|
||
fn test_eeprom_data_layout_constants() {
|
||
// Verify data layout constants match original EEPROM structure
|
||
assert_eq!(6, 6); // Size of data per axis (min, max, center as u16 each)
|
||
}
|
||
|
||
#[test]
|
||
fn test_gimbal_mode_address() {
|
||
// Gimbal mode should be stored at the end of the EEPROM data area
|
||
assert_eq!(GIMBAL_MODE_OFFSET, EEPROM_DATA_LENGTH as u32);
|
||
}
|
||
|
||
#[test]
|
||
fn test_calibration_data_format() {
|
||
// Test that calibration data format matches original (addresses base+1,2,3,4,5,6)
|
||
let test_data = [
|
||
(100, 3900, 2000),
|
||
(150, 3850, 2048),
|
||
(200, 3800, 1900),
|
||
(250, 3750, 2100),
|
||
];
|
||
let mut buffer = [0u8; EEPROM_DATA_LENGTH];
|
||
|
||
// Pack data using original format
|
||
for (index, &(min, max, center)) in test_data.iter().enumerate() {
|
||
let base = index * 6;
|
||
buffer[base + 1] = min as u8;
|
||
buffer[base + 2] = (min >> 8) as u8;
|
||
buffer[base + 3] = max as u8;
|
||
buffer[base + 4] = (max >> 8) as u8;
|
||
buffer[base + 5] = center as u8;
|
||
buffer[base + 6] = (center >> 8) as u8;
|
||
}
|
||
|
||
// Verify first axis data (addresses 1-6)
|
||
assert_eq!(buffer[1], 100 as u8); // min low
|
||
assert_eq!(buffer[2], 0); // min high
|
||
assert_eq!(buffer[3], (3900 & 0xFF) as u8); // max low
|
||
assert_eq!(buffer[4], (3900 >> 8) as u8); // max high
|
||
assert_eq!(buffer[5], (2000 & 0xFF) as u8); // center low
|
||
assert_eq!(buffer[6], (2000 >> 8) as u8); // center high
|
||
}
|
||
|
||
#[test]
|
||
fn test_boundary_values() {
|
||
let mut buffer = [0u8; 4];
|
||
|
||
// Test minimum value (manual packing)
|
||
let value = 0u16;
|
||
buffer[0] = value as u8;
|
||
buffer[1] = (value >> 8) as u8;
|
||
assert_eq!(buffer[0], 0);
|
||
assert_eq!(buffer[1], 0);
|
||
|
||
// Test maximum value (manual packing)
|
||
let value = u16::MAX;
|
||
buffer[2] = value as u8;
|
||
buffer[3] = (value >> 8) as u8;
|
||
assert_eq!(buffer[2], 0xFF);
|
||
assert_eq!(buffer[3], 0xFF);
|
||
}
|
||
|
||
#[test]
|
||
fn test_read_axis_calibration_with_mock() {
|
||
// Mock EEPROM data: axis 0 with min=100, max=4000, center=2050
|
||
// Original format: addresses 1,2,3,4,5,6 for axis 0
|
||
let mut mock_data = [0u8; 7]; // Need indices 0-6
|
||
mock_data[1] = 100; // min low byte
|
||
mock_data[2] = 0; // min high byte
|
||
mock_data[3] = 160; // max low byte (4000 = 0x0FA0)
|
||
mock_data[4] = 15; // max high byte
|
||
mock_data[5] = 2; // center low byte (2050 = 0x0802)
|
||
mock_data[6] = 8; // center high byte
|
||
|
||
let mut read_fn = |addr: u32| -> Result<u8, ()> {
|
||
if addr < mock_data.len() as u32 {
|
||
Ok(mock_data[addr as usize])
|
||
} else {
|
||
Err(())
|
||
}
|
||
};
|
||
|
||
let result = read_axis_calibration(&mut read_fn, 0);
|
||
assert!(result.is_ok());
|
||
let (min, max, center) = result.unwrap();
|
||
assert_eq!(min, 100);
|
||
assert_eq!(max, 4000);
|
||
assert_eq!(center, 2050);
|
||
}
|
||
}
|