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

Updated ELRS support. Code cleanup

Browse Source
This commit is contained in:
Christoffer Martinsson 2023-08-30 07:27:14 +02:00
parent 3c2ab2c5e8
commit f2f40a2a7e
2 changed files with 241 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

106
rp2040/src/elrs.rs
View File

@ -34,6 +34,22 @@ where
uart: UartPeripheral<S, D, P>, uart: UartPeripheral<S, D, P>,
elsr_init_done: bool, elsr_init_done: bool,
elrs_init_counter: u16, elrs_init_counter: u16,
elrs_connected: bool,
elrs_connected_timeout: u16,
uplink_rssi_1: u8,
uplink_rssi_2: u8,
uplink_package_success: u8,
uplink_snr: i8,
diversity: u8,
rf_mode: u8,
uplink_tx_power: u8,
downlink_rssi: u8,
downlink_package_success: u8,
downlink_snr: i8,
rx_buffer: [u8; 64],
rx_buffer_index: usize,
rx_sync: bool,
rx_package_size: usize,
} }
impl<S, D, P> Elrs<S, D, P> impl<S, D, P> Elrs<S, D, P>
@ -45,10 +61,42 @@ where
pub fn new(uart: UartPeripheral<S, D, P>) -> Self { pub fn new(uart: UartPeripheral<S, D, P>) -> Self {
let elsr_init_done = false; let elsr_init_done = false;
let elrs_init_counter = 0; let elrs_init_counter = 0;
let elrs_connected = false;
let elrs_connected_timeout = 500;
let uplink_rssi_1 = 0;
let uplink_rssi_2 = 0;
let uplink_package_success = 0;
let uplink_snr = 0;
let diversity = 0;
let rf_mode = 0;
let uplink_tx_power = 0;
let downlink_rssi = 0;
let downlink_package_success = 0;
let downlink_snr = 0;
let rx_buffer = [0; 64];
let rx_buffer_index = 0;
let rx_sync = false;
let rx_package_size = 0;
Self { Self {
uart, uart,
elsr_init_done, elsr_init_done,
elrs_init_counter, elrs_init_counter,
elrs_connected,
elrs_connected_timeout,
uplink_rssi_1,
uplink_rssi_2,
uplink_package_success,
uplink_snr,
diversity,
rf_mode,
uplink_tx_power,
downlink_rssi,
downlink_package_success,
downlink_snr,
rx_buffer,
rx_buffer_index,
rx_sync,
rx_package_size,
} }
} }
} }
@ -64,16 +112,23 @@ where
.write_full_blocking(&self.prepare_crsf_data_packet(data)); .write_full_blocking(&self.prepare_crsf_data_packet(data));
return; return;
} }
if self.elrs_init_counter < 500 { if self.elrs_init_counter < 5000 {
self.uart self.uart
.write_full_blocking(&self.prepare_crsf_data_packet(data)); .write_full_blocking(&self.prepare_crsf_data_packet(data));
self.elrs_init_counter += 1; self.elrs_init_counter += 1;
} else if self.elrs_init_counter < 505 { } else if self.elrs_init_counter < 5005 {
self.uart self.uart
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); // Setting Packet Rate to 150Hz
// 0 = 50Hz LoRa, 1 = 100Hz Full, 2 = 150Hz LoRa, 3 = 250Hz LoRa,
// 4 = 333H Full, 5 = 500Hz LoRa, 6 = 250Hz DejaVu, 7 = 500Hz DejaVu,
// 8 = 500Hz FLRC, 9 = 1000Hz FLRC
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x02));
self.elrs_init_counter += 1; self.elrs_init_counter += 1;
} else if self.elrs_init_counter < 510 { } else if self.elrs_init_counter < 5010 {
self.uart self.uart
// Setting Power to 10mW
// 0 = 10mW, 1 = 25mW, 2 = 50mW, 3 = 100mW,
// 4 = 200mW, 5 = 500mW, 6 = 1000mW, 7 = 2000mW
.write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00)); .write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00));
self.elrs_init_counter += 1; self.elrs_init_counter += 1;
} else { } else {
@ -81,6 +136,49 @@ where
} }
} }
pub fn check_link(&mut self) {
let mut buffer: [u8; 640] = [0; 640];
// let _ = self.uart.read_raw(&mut buffer);
let mut crc: u8 = 0;
for byte in buffer.iter() {
if *byte == 0xEA && !self.rx_sync {
self.rx_buffer_index = 0;
self.rx_package_size = 0;
self.rx_sync = true;
} else if self.rx_sync && self.rx_package_size == 0 {
self.rx_package_size = *byte as usize;
} else if self.rx_sync && self.rx_buffer_index < self.rx_package_size {
self.rx_buffer[self.rx_buffer_index] = *byte;
self.rx_buffer_index += 1;
} else if self.rx_buffer_index == self.rx_package_size {
for i in 0..self.rx_package_size {
crc = CRSF_CRC8TAB[(crc ^ self.rx_buffer[i]) as usize];
}
if crc == *byte {
self.elrs_connected = true;
self.elrs_connected_timeout = 500;
}
self.rx_sync = false;
}
}
if self.elrs_connected_timeout == 0 {
self.elrs_connected = false;
} else {
self.elrs_connected_timeout -= 1;
}
}
pub fn reset(&mut self) {
self.elsr_init_done = false;
self.elrs_init_counter = 0;
}
pub fn connected(&self) -> bool {
self.elrs_connected
}
fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] { fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] {
let mut packet: [u8; 26] = [0; 26]; let mut packet: [u8; 26] = [0; 26];
let mut crc: u8 = 0; let mut crc: u8 = 0;

285
rp2040/src/main.rs
View File

@ -76,12 +76,18 @@ pub const BASE_FREQ: i32 = 2 << I32_FRAC_BITS;
pub const SAMPLE_FREQ: i32 = 1000 << 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; pub const SENSITIVITY: i32 = (0.01 * ((1 << I32_FRAC_BITS) as f32)) as i32;
pub const DEBOUNCE: u8 = 10;
// Public types // Public types
#[derive(Copy, Clone, Default)] #[derive(Copy, Clone, Default)]
pub struct Button { pub struct Button {
pub pressed: bool, pub pressed: bool,
pub previous_pressed: bool, pub previous_pressed: bool,
pub fn_mode: u8, pub fn_mode: u8,
pub usb_changed: bool,
pub usb_changed_to: bool,
pub elrs_changed: bool,
pub elrs_changed_to: bool,
} }
#[derive(Copy, Clone)] #[derive(Copy, Clone)]
@ -162,7 +168,7 @@ fn main() -> ! {
) )
.unwrap(); .unwrap();
let mut i2c = I2C::i2c1( let i2c = I2C::i2c1(
pac.I2C1, pac.I2C1,
pins.gp14.into_mode(), // sda pins.gp14.into_mode(), // sda
pins.gp15.into_mode(), // scl pins.gp15.into_mode(), // scl
@ -171,8 +177,8 @@ fn main() -> ! {
125_000_000.Hz(), 125_000_000.Hz(),
); );
let i2c_address = SlaveAddr::default(); let i2c_address = SlaveAddr::Alternative(false, false, false);
let mut eeprom = Eeprom24x::new_24x02(i2c, i2c_address); let mut eeprom = Eeprom24x::new_24x32(i2c, i2c_address);
// Enable adc // Enable adc
let mut adc = Adc::new(pac.ADC, &mut pac.RESETS); let mut adc = Adc::new(pac.ADC, &mut pac.RESETS);
@ -187,20 +193,20 @@ fn main() -> ! {
// Setting up array with pins connected to button rows // Setting up array with pins connected to button rows
let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[ let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[
&pins.gp11.into_pull_up_input(), &pins.gp6.into_pull_up_input(),
&pins.gp13.into_pull_up_input(), &pins.gp8.into_pull_up_input(),
&pins.gp9.into_pull_up_input(), &pins.gp4.into_pull_up_input(),
&pins.gp12.into_pull_up_input(), &pins.gp7.into_pull_up_input(),
&pins.gp10.into_pull_up_input(), &pins.gp5.into_pull_up_input(),
]; ];
// Setting up array with pins connected to button columns // Setting up array with pins connected to button columns
let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [ let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [
&mut pins.gp4.into_push_pull_output(), &mut pins.gp9.into_push_pull_output(),
&mut pins.gp5.into_push_pull_output(), &mut pins.gp10.into_push_pull_output(),
&mut pins.gp6.into_push_pull_output(), &mut pins.gp11.into_push_pull_output(),
&mut pins.gp7.into_push_pull_output(), &mut pins.gp12.into_push_pull_output(),
&mut pins.gp8.into_push_pull_output(), &mut pins.gp13.into_push_pull_output(),
]; ];
let mut elrs_en_pin = pins.gp2.into_push_pull_output(); let mut elrs_en_pin = pins.gp2.into_push_pull_output();
@ -208,7 +214,7 @@ fn main() -> ! {
// Create button matrix object that scans all buttons // Create button matrix object that scans all buttons
let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> = let mut button_matrix: ButtonMatrix<BUTTON_ROWS, BUTTON_COLS, NUMBER_OF_BUTTONS> =
ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, 5); ButtonMatrix::new(button_matrix_row_pins, button_matrix_col_pins, DEBOUNCE);
// Initialize button matrix // Initialize button matrix
button_matrix.init_pins(); button_matrix.init_pins();
@ -239,35 +245,39 @@ fn main() -> ! {
// Create timers // Create timers
let timer = Timer::new(pac.TIMER, &mut pac.RESETS); let timer = Timer::new(pac.TIMER, &mut pac.RESETS);
let mut usb_hid_report_count_down = timer.count_down(); let mut status_led_count_down = timer.count_down();
usb_hid_report_count_down.start(10.millis()); status_led_count_down.start(250.millis());
let mut scan_count_down = timer.count_down(); let mut scan_count_down = timer.count_down();
scan_count_down.start(200u32.micros()); scan_count_down.start(200u32.micros());
let mut status_led_count_down = timer.count_down(); let mut data_process_count_down = timer.count_down();
status_led_count_down.start(50.millis()); data_process_count_down.start(1200u32.micros());
let mut main_count_down = timer.count_down();
main_count_down.start(1660u32.micros());
let mut elrs_start_count_down = timer.count_down(); let mut elrs_start_count_down = timer.count_down();
elrs_start_count_down.start(2000.millis()); elrs_start_count_down.start(2000.millis());
let mut elrs_update_count_down = timer.count_down();
elrs_update_count_down.start(1666u32.micros());
let mut usb_update_count_down = timer.count_down();
usb_update_count_down.start(10.millis());
let mut mode: u8 = 0; let mut mode: u8 = 0;
let mut safety_check: bool = false; let mut safety_check: bool = false;
let mut activity: bool = false; let mut usb_activity: bool = false;
let mut idle: bool = false; let mut idle: bool = false;
let mut usb_active: bool = false; let mut usb_active: bool = false;
let mut elrs_active: bool = false; let mut elrs_active: bool = false;
let mut elrs_connected: bool = false;
let mut calibration_active: bool = false; let mut calibration_active: bool = false;
let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS]; let mut axis: [GimbalAxis; NBR_OF_GIMBAL_AXIS] = [Default::default(); NBR_OF_GIMBAL_AXIS];
let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS]; let mut buttons: [Button; NUMBER_OF_BUTTONS] = [Button::default(); NUMBER_OF_BUTTONS];
let mut channel_locks: [bool; 12] = [false; 12]; let mut channel_locks: [bool; 12] = [false; 12];
let mut gimbal_mode: u8 = GIMBAL_MODE_M10; let mut gimbal_mode: u8;
// 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; AXIS_MAX as usize + 1] = generate_expo_lut(0.3);
// Set up left gimbal Y axis as full range without return to center spring // Set up left gimbal Y axis as full range without return to center spring
@ -303,20 +313,18 @@ fn main() -> ! {
.build(); .build();
// Read calibration data from eeprom // Read calibration data from eeprom
// if !calibration_active { for (index, item) in axis.iter_mut().enumerate() {
// for (index, item) in axis.iter_mut().enumerate() { item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
// item.min = eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16; item.min <<= 8;
// item.min <<= 8; item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
// item.min |= eeprom.read_byte(index as u32 * 6).unwrap() as u16; item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
// item.max = eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16; item.max <<= 8;
// item.max <<= 8; item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
// item.max = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16; item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16;
// item.center = eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16; item.center <<= 8;
// item.center <<= 8; item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
// item.center = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16; }
// } gimbal_mode = eeprom.read_byte(25).unwrap();
// gimbal_mode = eeprom.read_byte(24).unwrap();
// }
loop { loop {
// Take care of USB HID poll requests // Take care of USB HID poll requests
@ -324,11 +332,6 @@ fn main() -> ! {
usb_active = true; usb_active = true;
} }
// Power up ELRS TX
if elrs_start_count_down.wait().is_ok() {
elrs_active = true;
}
if scan_count_down.wait().is_ok() { if scan_count_down.wait().is_ok() {
button_matrix.scan_matrix(&mut delay); button_matrix.scan_matrix(&mut delay);
@ -356,46 +359,22 @@ fn main() -> ! {
if status_led_count_down.wait().is_ok() { if status_led_count_down.wait().is_ok() {
update_status_led( update_status_led(
&mut status_led, &mut status_led,
&mut activity,
&usb_active, &usb_active,
&elrs_active, &elrs_active,
&idle, &elrs.connected(),
&safety_check, &safety_check,
&calibration_active, &calibration_active,
); );
} }
// Dont send USB HID joystick report if there is no activity
// This is to avoid preventing the computer from going to sleep
if usb_hid_report_count_down.wait().is_ok() && activity {
match usb_hid_joystick.device().write_report(&get_joystick_report(
&mut buttons,
&mut axis,
&mode,
)) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
Err(e) => {
status_led.update(StatusMode::Error);
core::panic!("Failed to write joystick report: {:?}", e)
}
};
}
// Check if all axis are in idle position and no buttons are pressed // Check if all axis are in idle position and no buttons are pressed
if idle && !safety_check && elrs_active { if idle && !safety_check && elrs_active {
safety_check = true; safety_check = true;
} }
if main_count_down.wait().is_ok() { if data_process_count_down.wait().is_ok() {
// Secondary way to enter bootloader (pressing all left hands buttons except the hat // Secondary way to enter bootloader (pressing all left hands buttons except the hat
if button_matrix.buttons_pressed()[0] if button_matrix.buttons_pressed()[0] && button_matrix.buttons_pressed()[2] {
&& button_matrix.buttons_pressed()[1]
&& button_matrix.buttons_pressed()[5]
&& button_matrix.buttons_pressed()[6]
&& button_matrix.buttons_pressed()[8]
&& button_matrix.buttons_pressed()[9]
{
status_led.update(StatusMode::Bootloader); status_led.update(StatusMode::Bootloader);
let gpio_activity_pin_mask: u32 = 0; let gpio_activity_pin_mask: u32 = 0;
let disable_interface_mask: u32 = 0; let disable_interface_mask: u32 = 0;
@ -405,15 +384,23 @@ fn main() -> ! {
); );
} }
// ON/OFF switch for ELRS radio
if button_matrix.buttons_pressed()[4]
&& button_matrix.buttons_pressed()[2]
&& !elrs_active
{
safety_check = false;
elrs_active = true;
} else if button_matrix.buttons_pressed()[3]
&& button_matrix.buttons_pressed()[2]
&& elrs_active
{
elrs_active = false;
}
// Calibration of center position (pressing all right hands buttons except the hat // Calibration of center position (pressing all right hands buttons except the hat
// switch) // switch)
if button_matrix.buttons_pressed()[3] if button_matrix.buttons_pressed()[1] && button_matrix.buttons_pressed()[2] {
&& button_matrix.buttons_pressed()[4]
&& button_matrix.buttons_pressed()[10]
&& button_matrix.buttons_pressed()[11]
&& button_matrix.buttons_pressed()[13]
&& button_matrix.buttons_pressed()[14]
{
for (index, item) in axis.iter_mut().enumerate() { for (index, item) in axis.iter_mut().enumerate() {
item.center = smoother[index].value() as u16; item.center = smoother[index].value() as u16;
item.min = item.center; item.min = item.center;
@ -450,15 +437,17 @@ fn main() -> ! {
} }
// Save calibration data to eeprom (pressing right hat switch) // Save calibration data to eeprom (pressing right hat switch)
else if calibration_active && button_matrix.buttons_pressed()[20] { else if calibration_active && button_matrix.buttons_pressed()[20] {
// for (index, item) in axis.iter_mut().enumerate() { let mut eeprom_data: [u8; 25] = [0; 25];
// let _ = eeprom.write_byte(index as u32 * 6, item.min as u8); for (index, item) in axis.iter_mut().enumerate() {
// let _ = eeprom.write_byte((index as u32 * 6) + 1, (item.min >> 8) as u8); eeprom_data[index * 6] = item.min as u8;
// let _ = eeprom.write_byte((index as u32 * 6) + 2, item.max as u8); eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8;
// let _ = eeprom.write_byte((index as u32 * 6) + 3, (item.max >> 8) as u8); eeprom_data[(index * 6) + 2] = item.max as u8;
// let _ = eeprom.write_byte((index as u32 * 6) + 4, item.center as u8); eeprom_data[(index * 6) + 3] = (item.max >> 8) as u8;
// let _ = eeprom.write_byte((index as u32 * 6) + 5, (item.center >> 8) as u8); eeprom_data[(index * 6) + 4] = item.center as u8;
// } eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8;
// let _ = eeprom.write_byte(24, gimbal_mode); }
eeprom_data[24] = gimbal_mode;
let _ = eeprom.write_page(0x01, &eeprom_data);
calibration_active = false; calibration_active = false;
} }
@ -501,9 +490,11 @@ fn main() -> ! {
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::FnR
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeL
&& layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR) && layout::HID_MAP[key.fn_mode as usize][index] != layout::HidButton::ModeR
&& index != 2)
|| (elrs_active || (elrs_active
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated
&& index != 2)
{ {
idle = false; idle = false;
} }
@ -512,8 +503,9 @@ fn main() -> ! {
// Generate led activity when gimbal is moved from idle position // Generate led activity when gimbal is moved from idle position
for item in axis.iter_mut() { for item in axis.iter_mut() {
if item.value != item.previous_value { if item.value != item.previous_value {
activity = true; usb_activity = true;
} }
item.previous_value = item.value;
} }
// Generate led activity when a button is pressed // Generate led activity when a button is pressed
@ -528,8 +520,13 @@ fn main() -> ! {
&& key.pressed != key.previous_pressed && key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated) && layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
{ {
activity = true; key.usb_changed = true;
key.usb_changed_to = key.pressed;
key.elrs_changed = true;
key.elrs_changed_to = key.pressed;
usb_activity = true;
} }
key.previous_pressed = key.pressed;
} }
// Reset channel locks when calibration is active // Reset channel locks when calibration is active
@ -538,7 +535,9 @@ fn main() -> ! {
*lock_active = false; *lock_active = false;
} }
} }
}
if elrs_update_count_down.wait().is_ok() {
// Send ELRS data // Send ELRS data
if elrs_active { if elrs_active {
elrs_en_pin.set_high().unwrap(); elrs_en_pin.set_high().unwrap();
@ -546,20 +545,30 @@ fn main() -> ! {
&mut buttons, &mut buttons,
&mut axis, &mut axis,
&mut channel_locks, &mut channel_locks,
elrs_active,
)); ));
// elrs.check_link();
} else { } else {
elrs_en_pin.set_low().unwrap(); elrs_en_pin.set_low().unwrap();
elrs.reset();
} }
}
// Clear axis status // Dont send USB HID joystick report if there is no activity
for item in axis.iter_mut() { // This is to avoid preventing the computer from going to sleep
item.previous_value = item.value; if usb_update_count_down.wait().is_ok() && usb_activity {
} match usb_hid_joystick.device().write_report(&get_joystick_report(
// Clear key status &mut buttons,
for key in buttons.iter_mut() { &mut axis,
key.previous_pressed = key.pressed; &mode,
} )) {
Err(UsbHidError::WouldBlock) => {}
Ok(_) => {}
Err(e) => {
status_led.update(StatusMode::Error);
core::panic!("Failed to write joystick report: {:?}", e)
}
};
usb_activity = false;
} }
} }
} }
@ -579,10 +588,9 @@ fn main() -> ! {
/// * `safety_check` - Reference to bool that indicates if safety check has passed /// * `safety_check` - Reference to bool that indicates if safety check has passed
fn update_status_led<P, SM, I>( fn update_status_led<P, SM, I>(
status_led: &mut Ws2812StatusLed<P, SM, I>, status_led: &mut Ws2812StatusLed<P, SM, I>,
activity: &mut bool,
usb_active: &bool, usb_active: &bool,
elrs_active: &bool, elrs_active: &bool,
axis_idle: &bool, elrs_connected: &bool,
safety_check: &bool, safety_check: &bool,
calibration_active: &bool, calibration_active: &bool,
) where ) where
@ -591,33 +599,18 @@ fn update_status_led<P, SM, I>(
Function<P>: ValidPinMode<I>, Function<P>: ValidPinMode<I>,
SM: StateMachineIndex, SM: StateMachineIndex,
{ {
// If calibration is active, flash the status LED green if *calibration_active {
if *calibration_active && status_led.get_mode() == StatusMode::Normal { status_led.update(StatusMode::ActivityFlash);
status_led.update(StatusMode::Off);
} else if *calibration_active && status_led.get_mode() != StatusMode::Normal {
status_led.update(StatusMode::Normal);
// If in ELRS mode and safety chack failed, flash status LED red
} else if *elrs_active && !*safety_check { } else if *elrs_active && !*safety_check {
status_led.update(StatusMode::Warning); status_led.update(StatusMode::Warning);
// If activity occurs, flash status LED blue } else if !*usb_active && !*elrs_active {
} else if *activity && status_led.get_mode() != StatusMode::Activity { status_led.update(StatusMode::NormalFlash);
status_led.update(StatusMode::Activity); } else if *usb_active && !*elrs_active {
} else if *activity && status_led.get_mode() == StatusMode::Activity {
status_led.update(StatusMode::Off);
*activity = false;
// If no activity but not in idle position, turn status LED steady blue
} else if !*axis_idle && status_led.get_mode() != StatusMode::Activity {
status_led.update(StatusMode::Activity);
// Else device idle in USB mode, turn status LED steady green
} else if *axis_idle
&& *usb_active
&& !*elrs_active
&& status_led.get_mode() != StatusMode::Normal
{
status_led.update(StatusMode::Normal); status_led.update(StatusMode::Normal);
// Else device idle in ELRS mode, turn status LED steady orange } else if *elrs_active && *elrs_connected {
} else if *axis_idle && *elrs_active && status_led.get_mode() != StatusMode::Other {
status_led.update(StatusMode::Other); status_led.update(StatusMode::Other);
} else if *elrs_active && !*elrs_connected {
status_led.update(StatusMode::OtherFlash);
} }
} }
@ -948,55 +941,55 @@ fn get_elrs_channels(
matrix_keys: &mut [Button; NUMBER_OF_BUTTONS], matrix_keys: &mut [Button; NUMBER_OF_BUTTONS],
axis: &mut [GimbalAxis; 4], axis: &mut [GimbalAxis; 4],
channel_locks: &mut [bool; 12], channel_locks: &mut [bool; 12],
elrs_active: bool,
) -> [u16; 12] { ) -> [u16; 12] {
let mut channels: [u16; 12] = [ELRS_MIN; 12]; let mut channels: [u16; 12] = [ELRS_MIN; 12];
// Check and store trim values // Check and store trim values
let mut trim_active = false; let mut trim_active = false;
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if elrs_active if key.pressed
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P
&& layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4P
&& axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim
< ELRS_CENTER as i16
{ {
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim += 1; if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim
< ELRS_CENTER as i16
{
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1P as usize].trim +=
1;
}
trim_active = true; trim_active = true;
} else if elrs_active } else if key.pressed
&& key.pressed
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M && layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M
&& layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M && layout::ELRS_MAP[index] <= layout::ElrsButton::CH4M
&& axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim
> (0 - ELRS_CENTER as i16)
{ {
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -= 1; if axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim
> (0 - ELRS_CENTER as i16)
{
axis[layout::ELRS_MAP[index] as usize - layout::ElrsButton::CH1M as usize].trim -=
1;
}
trim_active = true; trim_active = true;
} }
} }
// Check and reser trim values // Check and reset trim values
for (index, key) in matrix_keys.iter_mut().enumerate() { for (index, key) in matrix_keys.iter_mut().enumerate() {
if elrs_active if !trim_active
&& !trim_active && key.elrs_changed
&& key.pressed && key.elrs_changed_to
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z && layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z
{ {
axis[GIMBAL_AXIS_LEFT_X].trim = 0; axis[GIMBAL_AXIS_LEFT_X].trim = 0;
axis[GIMBAL_AXIS_LEFT_Y].trim = 0; axis[GIMBAL_AXIS_LEFT_Y].trim = 0;
} else if elrs_active } else if !trim_active
&& !trim_active && key.elrs_changed
&& key.pressed && key.elrs_changed_to
&& key.pressed != key.previous_pressed
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z && layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z
{ {
axis[GIMBAL_AXIS_RIGHT_X].trim = 0; axis[GIMBAL_AXIS_RIGHT_X].trim = 0;
axis[GIMBAL_AXIS_RIGHT_Y].trim = 0; axis[GIMBAL_AXIS_RIGHT_Y].trim = 0;
} }
key.elrs_changed = false;
} }
// Match ELRS channel 1-4 to new min/max values // Match ELRS channel 1-4 to new min/max values