diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs index 6807deb..8f8e571 100644 --- a/rp2040/src/elrs.rs +++ b/rp2040/src/elrs.rs @@ -34,6 +34,22 @@ where uart: UartPeripheral, 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 Elrs @@ -45,10 +61,42 @@ where pub fn new(uart: UartPeripheral) -> 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; diff --git a/rp2040/src/main.rs b/rp2040/src/main.rs index bee0c80..a6b1450 100644 --- a/rp2040/src/main.rs +++ b/rp2040/src/main.rs @@ -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; 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; 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 = - 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( status_led: &mut Ws2812StatusLed, - 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( Function

: ValidPinMode, 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