diff --git a/rp2040/src/elrs.rs b/rp2040/src/elrs.rs new file mode 100644 index 0000000..6807deb --- /dev/null +++ b/rp2040/src/elrs.rs @@ -0,0 +1,131 @@ +//! Project: CMtec CMDR joystick 24 +//! Date: 2023-08-01 +//! Author: Christoffer Martinsson +//! Email: cm@cmtec.se +//! License: Please refer to LICENSE in root directory + +use rp2040_hal::uart::{Enabled, State, UartDevice, UartPeripheral, ValidUartPinout}; + +const CRSF_CRC8TAB: [u8; 256] = [ + 0x00, 0xD5, 0x7F, 0xAA, 0xFE, 0x2B, 0x81, 0x54, 0x29, 0xFC, 0x56, 0x83, 0xD7, 0x02, 0xA8, 0x7D, + 0x52, 0x87, 0x2D, 0xF8, 0xAC, 0x79, 0xD3, 0x06, 0x7B, 0xAE, 0x04, 0xD1, 0x85, 0x50, 0xFA, 0x2F, + 0xA4, 0x71, 0xDB, 0x0E, 0x5A, 0x8F, 0x25, 0xF0, 0x8D, 0x58, 0xF2, 0x27, 0x73, 0xA6, 0x0C, 0xD9, + 0xF6, 0x23, 0x89, 0x5C, 0x08, 0xDD, 0x77, 0xA2, 0xDF, 0x0A, 0xA0, 0x75, 0x21, 0xF4, 0x5E, 0x8B, + 0x9D, 0x48, 0xE2, 0x37, 0x63, 0xB6, 0x1C, 0xC9, 0xB4, 0x61, 0xCB, 0x1E, 0x4A, 0x9F, 0x35, 0xE0, + 0xCF, 0x1A, 0xB0, 0x65, 0x31, 0xE4, 0x4E, 0x9B, 0xE6, 0x33, 0x99, 0x4C, 0x18, 0xCD, 0x67, 0xB2, + 0x39, 0xEC, 0x46, 0x93, 0xC7, 0x12, 0xB8, 0x6D, 0x10, 0xC5, 0x6F, 0xBA, 0xEE, 0x3B, 0x91, 0x44, + 0x6B, 0xBE, 0x14, 0xC1, 0x95, 0x40, 0xEA, 0x3F, 0x42, 0x97, 0x3D, 0xE8, 0xBC, 0x69, 0xC3, 0x16, + 0xEF, 0x3A, 0x90, 0x45, 0x11, 0xC4, 0x6E, 0xBB, 0xC6, 0x13, 0xB9, 0x6C, 0x38, 0xED, 0x47, 0x92, + 0xBD, 0x68, 0xC2, 0x17, 0x43, 0x96, 0x3C, 0xE9, 0x94, 0x41, 0xEB, 0x3E, 0x6A, 0xBF, 0x15, 0xC0, + 0x4B, 0x9E, 0x34, 0xE1, 0xB5, 0x60, 0xCA, 0x1F, 0x62, 0xB7, 0x1D, 0xC8, 0x9C, 0x49, 0xE3, 0x36, + 0x19, 0xCC, 0x66, 0xB3, 0xE7, 0x32, 0x98, 0x4D, 0x30, 0xE5, 0x4F, 0x9A, 0xCE, 0x1B, 0xB1, 0x64, + 0x72, 0xA7, 0x0D, 0xD8, 0x8C, 0x59, 0xF3, 0x26, 0x5B, 0x8E, 0x24, 0xF1, 0xA5, 0x70, 0xDA, 0x0F, + 0x20, 0xF5, 0x5F, 0x8A, 0xDE, 0x0B, 0xA1, 0x74, 0x09, 0xDC, 0x76, 0xA3, 0xF7, 0x22, 0x88, 0x5D, + 0xD6, 0x03, 0xA9, 0x7C, 0x28, 0xFD, 0x57, 0x82, 0xFF, 0x2A, 0x80, 0x55, 0x01, 0xD4, 0x7E, 0xAB, + 0x84, 0x51, 0xFB, 0x2E, 0x7A, 0xAF, 0x05, 0xD0, 0xAD, 0x78, 0xD2, 0x07, 0x53, 0x86, 0x2C, 0xF9, +]; + +pub struct Elrs +where + S: State, + D: UartDevice, + P: ValidUartPinout, +{ + uart: UartPeripheral, + elsr_init_done: bool, + elrs_init_counter: u16, +} + +impl Elrs +where + S: State, + D: UartDevice, + P: ValidUartPinout, +{ + pub fn new(uart: UartPeripheral) -> Self { + let elsr_init_done = false; + let elrs_init_counter = 0; + Self { + uart, + elsr_init_done, + elrs_init_counter, + } + } +} + +impl Elrs +where + D: UartDevice, + P: ValidUartPinout, +{ + pub fn send(&mut self, data: [u16; 12]) { + if self.elsr_init_done { + self.uart + .write_full_blocking(&self.prepare_crsf_data_packet(data)); + return; + } + if self.elrs_init_counter < 500 { + self.uart + .write_full_blocking(&self.prepare_crsf_data_packet(data)); + self.elrs_init_counter += 1; + } else if self.elrs_init_counter < 505 { + self.uart + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x01, 0x00)); + self.elrs_init_counter += 1; + } else if self.elrs_init_counter < 510 { + self.uart + .write_full_blocking(&self.prepare_crsf_cmd_packet(0x06, 0x00)); + self.elrs_init_counter += 1; + } else { + self.elsr_init_done = true; + } + } + + fn prepare_crsf_data_packet(&self, data: [u16; 12]) -> [u8; 26] { + let mut packet: [u8; 26] = [0; 26]; + let mut crc: u8 = 0; + packet[0] = 0xEE; + packet[1] = 0x18; + packet[2] = 0x16; + packet[3] = data[0] as u8; + packet[4] = ((data[0] >> 8) | (data[1] << 3)) as u8; + packet[5] = ((data[1] >> 5) | (data[2] << 6)) as u8; + packet[6] = (data[2] >> 2) as u8; + packet[7] = ((data[2] >> 10) | (data[3] << 1)) as u8; + packet[8] = ((data[3] >> 7) | (data[4] << 4)) as u8; + packet[9] = ((data[4] >> 4) | (data[5] << 7)) as u8; + packet[10] = (data[5] >> 1) as u8; + packet[11] = ((data[5] >> 9) | (data[6] << 2)) as u8; + packet[12] = ((data[6] >> 6) | (data[7] << 5)) as u8; + packet[13] = (data[7] >> 3) as u8; + packet[14] = data[8] as u8; + packet[15] = ((data[8] >> 8) | (data[9] << 3)) as u8; + packet[16] = ((data[9] >> 5) | (data[10] << 6)) as u8; + packet[17] = (data[10] >> 2) as u8; + packet[18] = ((data[10] >> 10) | (data[11] << 1)) as u8; + packet[19] = (data[11] >> 7) as u8; + // Channel 13-16 are not used in ELRS + for i in 2..25 { + crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize]; + } + packet[25] = crc; + packet + } + + fn prepare_crsf_cmd_packet(&self, command: u8, value: u8) -> [u8; 8] { + let mut packet: [u8; 8] = [0; 8]; + let mut crc: u8 = 0; + packet[0] = 0xEE; + packet[1] = 0x06; + packet[2] = 0x2D; + packet[3] = 0xEE; + packet[4] = 0xEA; + packet[5] = command; + packet[6] = value; + for i in 2..7 { + crc = CRSF_CRC8TAB[(crc ^ packet[i]) as usize]; + } + packet[7] = crc; + packet + } +} diff --git a/rp2040/src/layout.rs b/rp2040/src/layout.rs index 48deebf..8b67f80 100644 --- a/rp2040/src/layout.rs +++ b/rp2040/src/layout.rs @@ -22,31 +22,31 @@ pub enum ElrsButton { CH11 = 10, CH12 = 11, CH5ON = 12, - CH5OFF = 13, - CH6ON = 14, - CH6OFF = 15, - CH7ON = 16, - CH7OFF = 17, - CH8ON = 18, - CH8OFF = 19, - CH9ON = 20, - CH9OFF = 21, - CH10ON = 22, - CH10OFF = 23, - CH11ON = 24, - CH11OFF = 25, - CH12ON = 26, + CH6ON = 13, + CH7ON = 14, + CH8ON = 15, + CH9ON = 16, + CH10ON = 17, + CH11ON = 18, + CH12ON = 19, + CH5OFF = 20, + CH6OFF = 21, + CH7OFF = 22, + CH8OFF = 23, + CH9OFF = 24, + CH10OFF = 25, + CH11OFF = 26, CH12OFF = 27, - XY1TU = 28, - XY1TR = 29, - XY1TD = 30, - XY1TL = 31, - XY1TRESET = 32, - XY2TU = 33, - XY2TR = 34, - XY2TD = 35, - XY2TL = 36, - XY2TRESET = 37, + CH1P = 28, + CH2P = 29, + CH3P = 30, + CH4P = 31, + CH1M = 32, + CH2M = 33, + CH3M = 34, + CH4M = 35, + CH12Z = 36, + CH34Z = 37, NoEventIndicated = 38, } #[allow(dead_code)] @@ -77,26 +77,26 @@ pub enum HidButton { FnR = 22, ModeL = 23, ModeR = 24, - Hat1U = 25, - Hat1R = 26, - Hat1D = 27, - Hat1L = 28, - Hat1B = 29, - Hat2U = 30, - Hat2R = 31, - Hat2D = 32, - Hat2L = 33, - Hat2B = 34, - Hat3U = 35, - Hat3R = 36, - Hat3D = 37, - Hat3L = 38, - Hat3B = 39, - Hat4U = 40, - Hat4R = 41, - Hat4D = 42, - Hat4L = 43, - Hat4B = 44, + H1U = 25, + H1R = 26, + H1D = 27, + H1L = 28, + H1B = 29, + H2U = 30, + H2R = 31, + H2D = 32, + H2L = 33, + H2B = 34, + H3U = 35, + H3R = 36, + H3D = 37, + H3L = 38, + H3B = 39, + H4U = 40, + H4R = 41, + H4D = 42, + H4L = 43, + H4B = 44, NoEventIndicated = 45, } #[warn(dead_code)] @@ -138,16 +138,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::Hat1B, // 15 button 22 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat2B, // 20 button 23 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 + HidButton::H1B, // 15 button 22 + HidButton::H1U, // 16 + HidButton::H1R, // 17 + HidButton::H1D, // 18 + HidButton::H1L, // 19 + HidButton::H2B, // 20 button 23 + HidButton::H2U, // 21 + HidButton::H2R, // 22 + HidButton::H2D, // 23 + HidButton::H2L, // 24 ], [ // Function layer 1 (left Fn button pressed) @@ -168,16 +168,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B9, // 13 HidButton::B10, // 14 - HidButton::Hat3B, // 15 button 24 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat2B, // 20 button 23 - HidButton::Hat2U, // 21 - HidButton::Hat2R, // 22 - HidButton::Hat2D, // 23 - HidButton::Hat2L, // 24 + HidButton::H3B, // 15 button 24 + HidButton::H3U, // 16 + HidButton::H3R, // 17 + HidButton::H3D, // 18 + HidButton::H3L, // 19 + HidButton::H2B, // 20 button 23 + HidButton::H2U, // 21 + HidButton::H2R, // 22 + HidButton::H2D, // 23 + HidButton::H2L, // 24 ], [ // Function layer 2 (right Fn button pressed) @@ -198,16 +198,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::Hat1B, // 15 button 22 - HidButton::Hat1U, // 16 - HidButton::Hat1R, // 17 - HidButton::Hat1D, // 18 - HidButton::Hat1L, // 19 - HidButton::Hat4B, // 20 button 25 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - HidButton::Hat4L, // 24 + HidButton::H1B, // 15 button 22 + HidButton::H1U, // 16 + HidButton::H1R, // 17 + HidButton::H1D, // 18 + HidButton::H1L, // 19 + HidButton::H4B, // 20 button 25 + HidButton::H4U, // 21 + HidButton::H4R, // 22 + HidButton::H4D, // 23 + HidButton::H4L, // 24 ], [ // Function layer 3 (left + right Fn button pressed) @@ -228,16 +228,16 @@ pub const HID_MAP: [[HidButton; NUMBER_OF_BUTTONS]; 4] = [ HidButton::ModeR, // 12 HidButton::B19, // 13 HidButton::B20, // 14 - HidButton::Hat3B, // 15 button 24 - HidButton::Hat3U, // 16 - HidButton::Hat3R, // 17 - HidButton::Hat3D, // 18 - HidButton::Hat3L, // 19 - HidButton::Hat4B, // 20 button 25 - HidButton::Hat4U, // 21 - HidButton::Hat4R, // 22 - HidButton::Hat4D, // 23 - HidButton::Hat4L, // 24 + HidButton::H3B, // 15 button 24 + HidButton::H3U, // 16 + HidButton::H3R, // 17 + HidButton::H3D, // 18 + HidButton::H3L, // 19 + HidButton::H4B, // 20 button 25 + HidButton::H4U, // 21 + HidButton::H4R, // 22 + HidButton::H4D, // 23 + HidButton::H4L, // 24 ], ]; @@ -260,14 +260,14 @@ pub const ELRS_MAP: [ElrsButton; NUMBER_OF_BUTTONS] = [ ElrsButton::CH6, // 12 ElrsButton::CH12ON, // 13 ElrsButton::CH12OFF, // 14 - ElrsButton::XY1TRESET, // 15 - ElrsButton::XY1TU, // 16 - ElrsButton::XY1TR, // 17 - ElrsButton::XY1TD, // 18 - ElrsButton::XY1TL, // 19 - ElrsButton::XY2TRESET, // 20 - ElrsButton::XY2TU, // 21 - ElrsButton::XY2TR, // 22 - ElrsButton::XY2TD, // 23 - ElrsButton::XY2TL, // 24 + ElrsButton::CH12Z, // 15 + ElrsButton::CH2P, // 16 + ElrsButton::CH1P, // 17 + ElrsButton::CH2M, // 18 + ElrsButton::CH1M, // 19 + ElrsButton::CH34Z, // 20 + ElrsButton::CH4P, // 21 + ElrsButton::CH3P, // 22 + ElrsButton::CH4M, // 23 + ElrsButton::CH3M, // 24 ];