Updated ELRS support. Code cleanup
This commit is contained in:
parent
3c2ab2c5e8
commit
f2f40a2a7e
@ -34,6 +34,22 @@ where
|
||||
uart: UartPeripheral<S, D, P>,
|
||||
elsr_init_done: bool,
|
||||
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>
|
||||
@ -45,10 +61,42 @@ where
|
||||
pub fn new(uart: UartPeripheral<S, D, P>) -> Self {
|
||||
let elsr_init_done = false;
|
||||
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 {
|
||||
uart,
|
||||
elsr_init_done,
|
||||
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));
|
||||
return;
|
||||
}
|
||||
if self.elrs_init_counter < 500 {
|
||||
if self.elrs_init_counter < 5000 {
|
||||
self.uart
|
||||
.write_full_blocking(&self.prepare_crsf_data_packet(data));
|
||||
self.elrs_init_counter += 1;
|
||||
} else if self.elrs_init_counter < 505 {
|
||||
} else if self.elrs_init_counter < 5005 {
|
||||
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;
|
||||
} else if self.elrs_init_counter < 510 {
|
||||
} else if self.elrs_init_counter < 5010 {
|
||||
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));
|
||||
self.elrs_init_counter += 1;
|
||||
} 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] {
|
||||
let mut packet: [u8; 26] = [0; 26];
|
||||
let mut crc: u8 = 0;
|
||||
|
||||
@ -76,12 +76,18 @@ 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;
|
||||
|
||||
pub const DEBOUNCE: u8 = 10;
|
||||
|
||||
// Public types
|
||||
#[derive(Copy, Clone, Default)]
|
||||
pub struct Button {
|
||||
pub pressed: bool,
|
||||
pub previous_pressed: bool,
|
||||
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)]
|
||||
@ -162,7 +168,7 @@ fn main() -> ! {
|
||||
)
|
||||
.unwrap();
|
||||
|
||||
let mut i2c = I2C::i2c1(
|
||||
let i2c = I2C::i2c1(
|
||||
pac.I2C1,
|
||||
pins.gp14.into_mode(), // sda
|
||||
pins.gp15.into_mode(), // scl
|
||||
@ -171,8 +177,8 @@ fn main() -> ! {
|
||||
125_000_000.Hz(),
|
||||
);
|
||||
|
||||
let i2c_address = SlaveAddr::default();
|
||||
let mut eeprom = Eeprom24x::new_24x02(i2c, i2c_address);
|
||||
let i2c_address = SlaveAddr::Alternative(false, false, false);
|
||||
let mut eeprom = Eeprom24x::new_24x32(i2c, i2c_address);
|
||||
|
||||
// Enable adc
|
||||
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
|
||||
let button_matrix_row_pins: &[&dyn InputPin<Error = Infallible>; BUTTON_ROWS] = &[
|
||||
&pins.gp11.into_pull_up_input(),
|
||||
&pins.gp13.into_pull_up_input(),
|
||||
&pins.gp9.into_pull_up_input(),
|
||||
&pins.gp12.into_pull_up_input(),
|
||||
&pins.gp10.into_pull_up_input(),
|
||||
&pins.gp6.into_pull_up_input(),
|
||||
&pins.gp8.into_pull_up_input(),
|
||||
&pins.gp4.into_pull_up_input(),
|
||||
&pins.gp7.into_pull_up_input(),
|
||||
&pins.gp5.into_pull_up_input(),
|
||||
];
|
||||
|
||||
// Setting up array with pins connected to button columns
|
||||
let button_matrix_col_pins: &mut [&mut dyn OutputPin<Error = Infallible>; BUTTON_COLS] = &mut [
|
||||
&mut pins.gp4.into_push_pull_output(),
|
||||
&mut pins.gp5.into_push_pull_output(),
|
||||
&mut pins.gp6.into_push_pull_output(),
|
||||
&mut pins.gp7.into_push_pull_output(),
|
||||
&mut pins.gp8.into_push_pull_output(),
|
||||
&mut pins.gp9.into_push_pull_output(),
|
||||
&mut pins.gp10.into_push_pull_output(),
|
||||
&mut pins.gp11.into_push_pull_output(),
|
||||
&mut pins.gp12.into_push_pull_output(),
|
||||
&mut pins.gp13.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
|
||||
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
|
||||
button_matrix.init_pins();
|
||||
@ -239,35 +245,39 @@ fn main() -> ! {
|
||||
// Create timers
|
||||
let timer = Timer::new(pac.TIMER, &mut pac.RESETS);
|
||||
|
||||
let mut usb_hid_report_count_down = timer.count_down();
|
||||
usb_hid_report_count_down.start(10.millis());
|
||||
let mut status_led_count_down = timer.count_down();
|
||||
status_led_count_down.start(250.millis());
|
||||
|
||||
let mut scan_count_down = timer.count_down();
|
||||
scan_count_down.start(200u32.micros());
|
||||
|
||||
let mut status_led_count_down = timer.count_down();
|
||||
status_led_count_down.start(50.millis());
|
||||
|
||||
let mut main_count_down = timer.count_down();
|
||||
main_count_down.start(1660u32.micros());
|
||||
let mut data_process_count_down = timer.count_down();
|
||||
data_process_count_down.start(1200u32.micros());
|
||||
|
||||
let mut elrs_start_count_down = timer.count_down();
|
||||
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 safety_check: bool = false;
|
||||
let mut activity: bool = false;
|
||||
let mut usb_activity: bool = false;
|
||||
let mut idle: bool = false;
|
||||
|
||||
let mut usb_active: bool = false;
|
||||
let mut elrs_active: bool = false;
|
||||
let mut elrs_connected: bool = false;
|
||||
let mut calibration_active: bool = false;
|
||||
|
||||
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 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);
|
||||
|
||||
// Set up left gimbal Y axis as full range without return to center spring
|
||||
@ -303,20 +313,18 @@ fn main() -> ! {
|
||||
.build();
|
||||
|
||||
// Read calibration data from eeprom
|
||||
// if !calibration_active {
|
||||
// for (index, item) in axis.iter_mut().enumerate() {
|
||||
// item.min = eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
|
||||
// item.min <<= 8;
|
||||
// item.min |= eeprom.read_byte(index as u32 * 6).unwrap() as u16;
|
||||
// item.max = eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
|
||||
// item.max <<= 8;
|
||||
// item.max = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
|
||||
// item.center = eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
|
||||
// item.center <<= 8;
|
||||
// item.center = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
|
||||
// }
|
||||
// gimbal_mode = eeprom.read_byte(24).unwrap();
|
||||
// }
|
||||
for (index, item) in axis.iter_mut().enumerate() {
|
||||
item.min = eeprom.read_byte((index as u32 * 6) + 2).unwrap() as u16;
|
||||
item.min <<= 8;
|
||||
item.min |= eeprom.read_byte((index as u32 * 6) + 1).unwrap() as u16;
|
||||
item.max = eeprom.read_byte((index as u32 * 6) + 4).unwrap() as u16;
|
||||
item.max <<= 8;
|
||||
item.max |= eeprom.read_byte((index as u32 * 6) + 3).unwrap() as u16;
|
||||
item.center = eeprom.read_byte((index as u32 * 6) + 6).unwrap() as u16;
|
||||
item.center <<= 8;
|
||||
item.center |= eeprom.read_byte((index as u32 * 6) + 5).unwrap() as u16;
|
||||
}
|
||||
gimbal_mode = eeprom.read_byte(25).unwrap();
|
||||
|
||||
loop {
|
||||
// Take care of USB HID poll requests
|
||||
@ -324,11 +332,6 @@ fn main() -> ! {
|
||||
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() {
|
||||
button_matrix.scan_matrix(&mut delay);
|
||||
|
||||
@ -356,46 +359,22 @@ fn main() -> ! {
|
||||
if status_led_count_down.wait().is_ok() {
|
||||
update_status_led(
|
||||
&mut status_led,
|
||||
&mut activity,
|
||||
&usb_active,
|
||||
&elrs_active,
|
||||
&idle,
|
||||
&elrs.connected(),
|
||||
&safety_check,
|
||||
&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
|
||||
if idle && !safety_check && elrs_active {
|
||||
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
|
||||
if button_matrix.buttons_pressed()[0]
|
||||
&& 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]
|
||||
{
|
||||
if button_matrix.buttons_pressed()[0] && button_matrix.buttons_pressed()[2] {
|
||||
status_led.update(StatusMode::Bootloader);
|
||||
let gpio_activity_pin_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
|
||||
// switch)
|
||||
if button_matrix.buttons_pressed()[3]
|
||||
&& 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]
|
||||
{
|
||||
if button_matrix.buttons_pressed()[1] && button_matrix.buttons_pressed()[2] {
|
||||
for (index, item) in axis.iter_mut().enumerate() {
|
||||
item.center = smoother[index].value() as u16;
|
||||
item.min = item.center;
|
||||
@ -450,15 +437,17 @@ fn main() -> ! {
|
||||
}
|
||||
// Save calibration data to eeprom (pressing right hat switch)
|
||||
else if calibration_active && button_matrix.buttons_pressed()[20] {
|
||||
// for (index, item) in axis.iter_mut().enumerate() {
|
||||
// let _ = eeprom.write_byte(index as u32 * 6, item.min as u8);
|
||||
// let _ = eeprom.write_byte((index as u32 * 6) + 1, (item.min >> 8) as u8);
|
||||
// let _ = eeprom.write_byte((index as u32 * 6) + 2, item.max as u8);
|
||||
// let _ = eeprom.write_byte((index as u32 * 6) + 3, (item.max >> 8) as u8);
|
||||
// let _ = eeprom.write_byte((index as u32 * 6) + 4, item.center as u8);
|
||||
// let _ = eeprom.write_byte((index as u32 * 6) + 5, (item.center >> 8) as u8);
|
||||
// }
|
||||
// let _ = eeprom.write_byte(24, gimbal_mode);
|
||||
let mut eeprom_data: [u8; 25] = [0; 25];
|
||||
for (index, item) in axis.iter_mut().enumerate() {
|
||||
eeprom_data[index * 6] = item.min as u8;
|
||||
eeprom_data[(index * 6) + 1] = (item.min >> 8) as u8;
|
||||
eeprom_data[(index * 6) + 2] = item.max as u8;
|
||||
eeprom_data[(index * 6) + 3] = (item.max >> 8) as u8;
|
||||
eeprom_data[(index * 6) + 4] = item.center as u8;
|
||||
eeprom_data[(index * 6) + 5] = (item.center >> 8) as u8;
|
||||
}
|
||||
eeprom_data[24] = gimbal_mode;
|
||||
let _ = eeprom.write_page(0x01, &eeprom_data);
|
||||
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::FnR
|
||||
&& 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
|
||||
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated)
|
||||
&& layout::ELRS_MAP[index] != layout::ElrsButton::NoEventIndicated
|
||||
&& index != 2)
|
||||
{
|
||||
idle = false;
|
||||
}
|
||||
@ -512,8 +503,9 @@ fn main() -> ! {
|
||||
// Generate led activity when gimbal is moved from idle position
|
||||
for item in axis.iter_mut() {
|
||||
if item.value != item.previous_value {
|
||||
activity = true;
|
||||
usb_activity = true;
|
||||
}
|
||||
item.previous_value = item.value;
|
||||
}
|
||||
|
||||
// Generate led activity when a button is pressed
|
||||
@ -528,8 +520,13 @@ fn main() -> ! {
|
||||
&& key.pressed != key.previous_pressed
|
||||
&& 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
|
||||
@ -538,7 +535,9 @@ fn main() -> ! {
|
||||
*lock_active = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if elrs_update_count_down.wait().is_ok() {
|
||||
// Send ELRS data
|
||||
if elrs_active {
|
||||
elrs_en_pin.set_high().unwrap();
|
||||
@ -546,20 +545,30 @@ fn main() -> ! {
|
||||
&mut buttons,
|
||||
&mut axis,
|
||||
&mut channel_locks,
|
||||
elrs_active,
|
||||
));
|
||||
// elrs.check_link();
|
||||
} else {
|
||||
elrs_en_pin.set_low().unwrap();
|
||||
elrs.reset();
|
||||
}
|
||||
}
|
||||
|
||||
// Clear axis status
|
||||
for item in axis.iter_mut() {
|
||||
item.previous_value = item.value;
|
||||
}
|
||||
// Clear key status
|
||||
for key in buttons.iter_mut() {
|
||||
key.previous_pressed = key.pressed;
|
||||
}
|
||||
// Dont send USB HID joystick report if there is no activity
|
||||
// This is to avoid preventing the computer from going to sleep
|
||||
if usb_update_count_down.wait().is_ok() && usb_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)
|
||||
}
|
||||
};
|
||||
usb_activity = false;
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -579,10 +588,9 @@ fn main() -> ! {
|
||||
/// * `safety_check` - Reference to bool that indicates if safety check has passed
|
||||
fn update_status_led<P, SM, I>(
|
||||
status_led: &mut Ws2812StatusLed<P, SM, I>,
|
||||
activity: &mut bool,
|
||||
usb_active: &bool,
|
||||
elrs_active: &bool,
|
||||
axis_idle: &bool,
|
||||
elrs_connected: &bool,
|
||||
safety_check: &bool,
|
||||
calibration_active: &bool,
|
||||
) where
|
||||
@ -591,33 +599,18 @@ fn update_status_led<P, SM, I>(
|
||||
Function<P>: ValidPinMode<I>,
|
||||
SM: StateMachineIndex,
|
||||
{
|
||||
// If calibration is active, flash the status LED green
|
||||
if *calibration_active && status_led.get_mode() == StatusMode::Normal {
|
||||
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
|
||||
if *calibration_active {
|
||||
status_led.update(StatusMode::ActivityFlash);
|
||||
} else if *elrs_active && !*safety_check {
|
||||
status_led.update(StatusMode::Warning);
|
||||
// If activity occurs, flash status LED blue
|
||||
} else if *activity && status_led.get_mode() != StatusMode::Activity {
|
||||
status_led.update(StatusMode::Activity);
|
||||
} 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
|
||||
{
|
||||
} else if !*usb_active && !*elrs_active {
|
||||
status_led.update(StatusMode::NormalFlash);
|
||||
} else if *usb_active && !*elrs_active {
|
||||
status_led.update(StatusMode::Normal);
|
||||
// Else device idle in ELRS mode, turn status LED steady orange
|
||||
} else if *axis_idle && *elrs_active && status_led.get_mode() != StatusMode::Other {
|
||||
} else if *elrs_active && *elrs_connected {
|
||||
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],
|
||||
axis: &mut [GimbalAxis; 4],
|
||||
channel_locks: &mut [bool; 12],
|
||||
elrs_active: bool,
|
||||
) -> [u16; 12] {
|
||||
let mut channels: [u16; 12] = [ELRS_MIN; 12];
|
||||
|
||||
// Check and store trim values
|
||||
let mut trim_active = false;
|
||||
for (index, key) in matrix_keys.iter_mut().enumerate() {
|
||||
if elrs_active
|
||||
&& key.pressed
|
||||
&& key.pressed != key.previous_pressed
|
||||
if key.pressed
|
||||
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1P
|
||||
&& 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;
|
||||
} else if elrs_active
|
||||
&& key.pressed
|
||||
&& key.pressed != key.previous_pressed
|
||||
} else if key.pressed
|
||||
&& layout::ELRS_MAP[index] >= layout::ElrsButton::CH1M
|
||||
&& 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;
|
||||
}
|
||||
}
|
||||
|
||||
// Check and reser trim values
|
||||
// Check and reset trim values
|
||||
for (index, key) in matrix_keys.iter_mut().enumerate() {
|
||||
if elrs_active
|
||||
&& !trim_active
|
||||
&& key.pressed
|
||||
&& key.pressed != key.previous_pressed
|
||||
if !trim_active
|
||||
&& key.elrs_changed
|
||||
&& key.elrs_changed_to
|
||||
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH12Z
|
||||
{
|
||||
axis[GIMBAL_AXIS_LEFT_X].trim = 0;
|
||||
axis[GIMBAL_AXIS_LEFT_Y].trim = 0;
|
||||
} else if elrs_active
|
||||
&& !trim_active
|
||||
&& key.pressed
|
||||
&& key.pressed != key.previous_pressed
|
||||
} else if !trim_active
|
||||
&& key.elrs_changed
|
||||
&& key.elrs_changed_to
|
||||
&& layout::ELRS_MAP[index] == layout::ElrsButton::CH34Z
|
||||
{
|
||||
axis[GIMBAL_AXIS_RIGHT_X].trim = 0;
|
||||
axis[GIMBAL_AXIS_RIGHT_Y].trim = 0;
|
||||
}
|
||||
key.elrs_changed = false;
|
||||
}
|
||||
|
||||
// Match ELRS channel 1-4 to new min/max values
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user