diff --git a/Cargo.toml b/Cargo.toml index c5b4022..20b990e 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -1,19 +1,21 @@ [package] -name = "multiwii_serial_protocol" +name = "multiwii_serial_protocol_v2" description = "A Multiwii Serial Protocol (MSP) implementation for Rust" -repository = "https://github.com/hashmismatch/multiwii_serial_protocol.rs" -version = "0.1.1" -authors = ["Rudi Benkovic "] +repository = "https://github.com/amfern/multiwii_serial_protocol.rs" +version = "0.1.11" +authors = ["Rudi Benkovic ", "Ilya Guterman "] license = "MIT OR Apache-2.0" readme = "README.md" [dependencies] -packed_struct = "0.2" -packed_struct_codegen = "0.2" +packed_struct = "0.3" +packed_struct_codegen = "0.3" serde = "1.0" serde_derive = "1.0" +crc-any = "2.3" [features] default = ["std"] std = [] -no_std = [] \ No newline at end of file +no_std = [] +suppport_int32_setting_type = [] \ No newline at end of file diff --git a/README.md b/README.md index 7a179a4..8c95550 100644 --- a/README.md +++ b/README.md @@ -7,7 +7,9 @@ A Multiwii Serial Protocol (MSP) implementation for Rust ## Introduction -An incomplete implementation of the MSP protocol, with some Cleanflight and Betaflight extensions. Allows one to implement a flight controller that can connect to the Cleanflight or Baseflight configurator. +This is a fork of https://github.com/hashmismatch/multiwii_serial_protocol.rs! + +An incomplete implementation of the MSP2 protocol, with some Cleanflight, Betaflight and iNav extensions. Allows one to implement a flight controller that can connect to the Cleanflight or Baseflight configurator. # Installation @@ -15,13 +17,13 @@ MSP is available on crates.io and can be included in your Cargo enabled project ```toml [dependencies] -multiwii_serial_protocol = "0.1.0" +multiwii_serial_protocol_2 = "0.1.9" ``` Then include it in your code like this: ```rust -extern crate multiwii_serial_protocol; +extern crate multiwii_serial_protocol_2; ``` License: MIT OR Apache-2.0 diff --git a/src/commands.rs b/src/commands.rs index 7850433..e4d77ed 100644 --- a/src/commands.rs +++ b/src/commands.rs @@ -1,4 +1,4 @@ -#[derive(PrimitiveEnum_u8)] +#[derive(PrimitiveEnum)] #[derive(Debug, Copy, Clone, PartialEq)] #[allow(non_camel_case_types)] @@ -57,6 +57,12 @@ pub enum MspCommandCode { MSP_TRANSPONDER_CONFIG = 82, MSP_SET_TRANSPONDER_CONFIG = 83, + MSP_OSD_CONFIG = 84, //out message Get osd settings - baseflight + MSP_SET_OSD_CONFIG = 85, //in message Set osd settings - baseflight + + MSP_OSD_CHAR_READ = 86, //out message Get osd settings - betaflight + MSP_OSD_CHAR_WRITE = 87, //in message Set osd settings - betaflight + MSP_LED_STRIP_MODECOLOR = 127, MSP_SET_LED_STRIP_MODECOLOR = 221, @@ -157,5 +163,28 @@ pub enum MspCommandCode { MSP_SET_PID_ADVANCED = 95, MSP_SENSOR_CONFIG = 96, - MSP_SET_SENSOR_CONFIG = 97 + MSP_SET_SENSOR_CONFIG = 97, + + // Inav + MSP2_COMMON_SETTING = 0x1003, //in/out message Returns the value for a setting + MSP2_COMMON_SET_SETTING = 0x1004, //in message Sets the value for a setting + + MSP2_MOTOR_MIXER = 0x1005, + MSP2_SET_MOTOR_MIXER = 0x1006, + + MSP2_COMMON_SETTING_INFO = 0x1007, + MSP2_COMMON_PG_LIST = 0x1008, + + MSP2_SERIAL_CONFIG = 0x1009, + MSP2_SET_SERIAL_CONFIG = 0x100A, + + MSP2_INAV_OSD_LAYOUTS = 0x2012, + MSP2_INAV_OSD_SET_LAYOUT_ITEM = 0x2013, + MSP2_INAV_OSD_ALARMS = 0x2014, + MSP2_INAV_OSD_SET_ALARMS = 0x2015, + MSP2_INAV_OSD_PREFERENCES = 0x2016, + MSP2_INAV_OSD_SET_PREFERENCES = 0x2017, + + MSP2_INAV_SERVO_MIXER = 0x2020, + MSP2_INAV_SET_SERVO_MIXER = 0x2021, } diff --git a/src/lib.rs b/src/lib.rs index 1db7ec1..937e3f3 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -12,6 +12,8 @@ extern crate alloc; extern crate packed_struct; +extern crate crc_any; + #[macro_use] extern crate packed_struct_codegen; diff --git a/src/packet.rs b/src/packet.rs index c9b5926..2bfb402 100644 --- a/src/packet.rs +++ b/src/packet.rs @@ -1,4 +1,6 @@ use prelude::v1::*; +use crc_any::CRCu8; + /// Packet parsing error #[derive(Copy, Clone, Debug, PartialEq)] @@ -37,10 +39,10 @@ impl MspPacketDirection { #[derive(Debug, Clone, PartialEq)] /// A decoded MSP packet, with a command code, direction and payload -pub struct MspPacket<'a> { - pub cmd: u8, +pub struct MspPacket { + pub cmd: u16, pub direction: MspPacketDirection, - pub data: Cow<'a, [u8]> + pub data: Vec } #[derive(Copy, Clone, PartialEq, Debug)] @@ -48,33 +50,47 @@ enum MspParserState { Header1, Header2, Direction, + FlagV2, DataLength, + DataLengthV2, Command, + CommandV2, Data, - Crc + DataV2, + Crc, +} + +#[derive(Copy, Clone, PartialEq, Debug)] +enum MSPVersion { + V1, + V2, } #[derive(Debug)] /// Parser that can find packets from a raw byte stream pub struct MspParser { state: MspParserState, + packet_version: MSPVersion, packet_direction: MspPacketDirection, - packet_cmd: u8, + packet_cmd: u16, packet_data_length_remaining: usize, packet_data: Vec, - packet_crc: u8 + packet_crc: u8, + packet_crc_v2: CRCu8, } impl MspParser { /// Create a new parser pub fn new() -> MspParser { - MspParser { + Self { state: MspParserState::Header1, + packet_version: MSPVersion::V1, packet_direction: MspPacketDirection::ToFlightController, packet_data_length_remaining: 0, packet_cmd: 0, packet_data: Vec::new(), - packet_crc: 0 + packet_crc: 0, + packet_crc_v2: CRCu8::crc8dvb_s2(), } } @@ -85,7 +101,7 @@ impl MspParser { /// Parse the next input byte. Returns a valid packet whenever a full packet is received, otherwise /// restarts the state of the parser. - pub fn parse<'b>(&mut self, input: u8) -> Result>, MspPacketParseError> { + pub fn parse(&mut self, input: u8) -> Result, MspPacketParseError> { match self.state { MspParserState::Header1 => { if input == ('$' as u8) { @@ -96,29 +112,84 @@ impl MspParser { }, MspParserState::Header2 => { - if input == ('M' as u8) { - self.state = MspParserState::Direction; - } else { - self.reset(); - } + self.packet_version = match input as char { + 'M' => MSPVersion::V1, + 'X' => MSPVersion::V2, + _=> { + self.reset(); + return Ok(None); + }, + }; + + self.state = MspParserState::Direction; }, MspParserState::Direction => { match input { - 60 => self.packet_direction = MspPacketDirection::ToFlightController, - 62 => self.packet_direction = MspPacketDirection::FromFlightController, - 33 => self.packet_direction = MspPacketDirection::Unsupported, + 60 => self.packet_direction = MspPacketDirection::ToFlightController, // '>' + 62 => self.packet_direction = MspPacketDirection::FromFlightController, // '<' + 33 => self.packet_direction = MspPacketDirection::Unsupported, // '!' error _ => { self.reset(); return Ok(None); } } - self.state = MspParserState::DataLength; + self.state = match self.packet_version { + MSPVersion::V1 => MspParserState::DataLength, + MSPVersion::V2 => MspParserState::FlagV2, + }; }, - MspParserState::DataLength => { + MspParserState::FlagV2 => { + // uint8, flag, usage to be defined (set to zero) + self.state = MspParserState::CommandV2; + self.packet_data = Vec::with_capacity(2); + self.packet_crc_v2.digest(&[input]); + }, + + MspParserState::CommandV2 => { + self.packet_data.push(input); + + if self.packet_data.len() == 2 { + let mut s = [0; 2]; + s.copy_from_slice(&self.packet_data); + self.packet_cmd = u16::from_le_bytes(s); + + self.packet_crc_v2.digest(&self.packet_data); + self.packet_data.clear(); + self.state = MspParserState::DataLengthV2; + } + }, + + MspParserState::DataLengthV2 => { + self.packet_data.push(input); + + if self.packet_data.len() == 2 { + let mut s = [0; 8]; + s[0..2].copy_from_slice(&self.packet_data); + self.packet_data_length_remaining = usize::from_le_bytes(s); + self.packet_crc_v2.digest(&self.packet_data); + self.packet_data = Vec::with_capacity(self.packet_data_length_remaining as usize); + + if self.packet_data_length_remaining == 0 { + self.state = MspParserState::Crc; + } else { + self.state = MspParserState::DataV2; + } + } + }, + + MspParserState::DataV2 => { + self.packet_data.push(input); + self.packet_data_length_remaining -= 1; + + if self.packet_data_length_remaining == 0 { + self.state = MspParserState::Crc; + } + }, + MspParserState::DataLength => { self.packet_data_length_remaining = input as usize; self.state = MspParserState::Command; self.packet_crc ^= input; @@ -127,7 +198,7 @@ impl MspParser { MspParserState::Command => { - self.packet_cmd = input; + self.packet_cmd = input as u16; if self.packet_data_length_remaining == 0 { self.state = MspParserState::Crc; @@ -153,6 +224,10 @@ impl MspParser { }, MspParserState::Crc => { + if self.packet_version == MSPVersion::V2 { + self.packet_crc_v2.digest(&self.packet_data); + self.packet_crc = self.packet_crc_v2.get_crc(); + } let packet_crc = self.packet_crc; if input != packet_crc { @@ -166,7 +241,7 @@ impl MspParser { let packet = MspPacket { cmd: self.packet_cmd, direction: self.packet_direction, - data: Cow::Owned(n) + data: n }; self.reset(); @@ -179,22 +254,28 @@ impl MspParser { Ok(None) } - fn reset(&mut self) { + pub fn reset(&mut self) { self.state = MspParserState::Header1; self.packet_direction = MspPacketDirection::ToFlightController; self.packet_data_length_remaining = 0; self.packet_cmd = 0; self.packet_data.clear(); self.packet_crc = 0; + self.packet_crc_v2.reset(); } } -impl<'a> MspPacket<'a> { +impl MspPacket { /// Number of bytes that this packet requires to be packed pub fn packet_size_bytes(&self) -> usize { 6 + self.data.len() } + /// Number of bytes that this packet requires to be packed + pub fn packet_size_bytes_v2(&self) -> usize { + 9 + self.data.len() + } + /// Serialize to network bytes pub fn serialize(&self, output: &mut [u8]) -> Result<(), MspPacketParseError> { let l = output.len(); @@ -207,7 +288,7 @@ impl<'a> MspPacket<'a> { output[1] = 'M' as u8; output[2] = self.direction.to_byte(); output[3] = self.data.len() as u8; - output[4] = self.cmd; + output[4] = self.cmd as u8; output[5..l - 1].copy_from_slice(&self.data); @@ -220,6 +301,30 @@ impl<'a> MspPacket<'a> { Ok(()) } + + /// Serialize to network bytes + pub fn serialize_v2(&self, output: &mut [u8]) -> Result<(), MspPacketParseError> { + let l = output.len(); + + if l != self.packet_size_bytes_v2() { + return Err(MspPacketParseError::OutputBufferSizeMismatch); + } + + output[0] = '$' as u8; + output[1] = 'X' as u8; + output[2] = self.direction.to_byte(); + output[3] = 0; + output[4..6].copy_from_slice(&self.cmd.to_le_bytes()); + output[6..8].copy_from_slice(&(self.data.len() as u16).to_le_bytes()); + + output[8..l - 1].copy_from_slice(&self.data); + + let mut crc = CRCu8::crc8dvb_s2(); + crc.digest(&output[3..l - 1]); + output[l - 1] = crc.get_crc(); + + Ok(()) + } } @@ -301,4 +406,4 @@ fn test_roundtrip() { }; roundtrip(&packet); } -} \ No newline at end of file +} diff --git a/src/prelude/std.rs b/src/prelude/std.rs index a2a037a..f48a347 100644 --- a/src/prelude/std.rs +++ b/src/prelude/std.rs @@ -20,4 +20,5 @@ pub use std::io; pub use std::io::Write; pub use std::sync::Arc; pub use std::str::from_utf8; -pub use std::ops::Deref; \ No newline at end of file +pub use std::ops::Deref; +pub use std::convert::TryFrom; diff --git a/src/structs.rs b/src/structs.rs index 562d4bb..89e011c 100644 --- a/src/structs.rs +++ b/src/structs.rs @@ -125,6 +125,20 @@ pub struct MspDataFlashSummaryReply { pub used_size_bytes: u32 } +#[derive(PackedStruct, Serialize, Deserialize, Debug, Copy, Clone)] +#[packed_struct(bytes="1", endian="lsb", bit_numbering="msb0")] +pub struct MspDataFlashReply { + pub read_address: u32, + // pub payload: Vec, // TODO: packed_struct should support dynamic size too the end +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "6", endian = "lsb", bit_numbering = "msb0")] +pub struct MspDataFlashRead { + pub read_address: u32, + pub read_length: u16, +} + #[derive(PackedStruct, Serialize, Deserialize, Debug, Copy, Clone)] #[packed_struct(endian="lsb")] pub struct MspAccTrim { @@ -279,7 +293,7 @@ pub struct MspRcChannelValue { pub value: u16 } -#[derive(PrimitiveEnum_u8, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] pub enum MspRcChannel { /// Ailerons Roll = 0, @@ -417,6 +431,28 @@ pub struct MspServos { pub servos: [u16; 8] } +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "14", endian = "lsb", bit_numbering = "msb0")] +pub struct MspServoConfig { + pub min: u16, + pub max: u16, + pub middle: u16, + pub rate: i8, + pub unused1: u8, + pub unused2: u8, + pub forward_from_channel: u8, // Depracted, set to 255 for backward compatibility + pub reverse_input: u32, // Depracted, Input reversing is not required since it can be done on mixer level +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetServoConfig { + pub index: u8, + #[packed_field(size_bytes="14")] + pub servo_config: MspServoConfig, +} + + #[derive(PackedStruct, Serialize, Deserialize, Debug, Copy, Clone)] #[packed_struct(endian="lsb")] pub struct MspMixerConfig { @@ -424,7 +460,25 @@ pub struct MspMixerConfig { pub mixer_mode: MixerMode } -#[derive(PrimitiveEnum_u8, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "4", endian = "lsb", bit_numbering = "msb0")] +pub struct MspModeRange { + pub box_id: u8, + #[packed_field(size_bits="8", ty="enum")] + pub aux_channel_index: MspRcChannel, + pub start_step: u8, + pub end_step: u8, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "5", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetModeRange { + pub index: u8, + #[packed_field(size_bytes="4")] + pub mode_range: MspModeRange, +} + +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] pub enum MixerMode { Tri = 1, QuadPlus = 2, @@ -439,6 +493,324 @@ pub enum MixerMode { OctoX8 = 11 } +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "8", endian = "lsb", bit_numbering = "msb0")] +pub struct MspMotorMixer { + pub throttle: u16, + pub roll: u16, + pub pitch: u16, + pub yaw: u16, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "9", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetMotorMixer { + pub index: u8, + #[packed_field(size_bytes="8")] + pub motor_mixer: MspMotorMixer, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "13", endian = "lsb", bit_numbering = "msb0")] +pub struct MspOsdConfig { + pub video_system: u8, + pub units: u8, + pub rssi_alarm: u8, + pub capacity_warning: u16, + pub time_alarm: u16, + pub alt_alarm: u16, + pub dist_alarm: u16, + pub neg_alt_alarm: u16, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetGetOsdConfig { + pub item_index: u8, + #[packed_field(size_bytes="13")] + pub config: MspOsdConfig, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "2", endian = "lsb", bit_numbering = "msb0")] +pub struct MspOsdItemPosition { + pub col: u8, + pub row: u8, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetOsdLayout { + pub item_index: u8, + #[packed_field(size_bytes="2")] + pub item: MspOsdItemPosition, +} + +// inav msp layout item +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetOsdLayoutItem { + pub layout_index: u8, + #[packed_field(size_bytes="3")] + pub item: MspSetOsdLayout, +} + +#[derive(Debug)] +pub struct MspOsdSettings { + pub osd_support: u8, + pub config: MspOsdConfig, + pub item_positions: Vec, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "2", endian = "lsb", bit_numbering = "msb0")] +pub struct MspOsdLayouts { + pub layout_count: u8, + pub item_count: u8, +} + +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +pub enum SerialIdentifier { + None = 255, + USART1 = 0, + USART2 = 1, + USART3 = 2, + USART4 = 3, + USART5 = 4, + USART6 = 5, + USART7 = 6, + USART8 = 7, + UsbVcp = 20, + SoftSerial1 = 30, + SoftSerial2 = 31, +} + +impl TryFrom for SerialIdentifier { + type Error = &'static str; + + fn try_from(value: u8) -> Result { + let serial = match value { + 255 => SerialIdentifier::None, + 0 => SerialIdentifier::USART1, + 1 => SerialIdentifier::USART2, + 2 => SerialIdentifier::USART3, + 3 => SerialIdentifier::USART4, + 4 => SerialIdentifier::USART5, + 5 => SerialIdentifier::USART6, + 6 => SerialIdentifier::USART7, + 7 => SerialIdentifier::USART8, + 20 => SerialIdentifier::UsbVcp, + 30 => SerialIdentifier::SoftSerial1, + 31 => SerialIdentifier::SoftSerial2, + _ => return Err("Serial identifier not found"), + }; + + Ok(serial) + } +} + +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +pub enum Baudrate { + BaudAuto = 0, + Baud1200 = 1, + Baud2400 = 2, + Baud4800 = 3, + Baud9600 = 4, + Baud19200 = 5, + Baud38400 = 6, + Baud57600 = 7, + Baud115200 = 8, + Baud230400 = 9, + Baud250000 = 10, + Baud460800 = 11, + Baud921600 = 12, + Baud1000000 = 13, + Baud1500000 = 14, + Baud2000000 = 15, + Baud2470000 = 16 +} + +impl TryFrom<&str> for Baudrate { + type Error = &'static str; + + fn try_from(value: &str) -> Result { + let baudrate = match value { + "0" => Baudrate::BaudAuto, + "1200" => Baudrate::Baud1200, + "2400" => Baudrate::Baud2400, + "4800" => Baudrate::Baud4800, + "9600" => Baudrate::Baud9600, + "19200" => Baudrate::Baud19200, + "38400" => Baudrate::Baud38400, + "57600" => Baudrate::Baud57600, + "115200" => Baudrate::Baud115200, + "230400" => Baudrate::Baud230400, + "250000" => Baudrate::Baud250000, + "460800" => Baudrate::Baud460800, + "921600" => Baudrate::Baud921600, + "1000000" => Baudrate::Baud1000000, + "1500000" => Baudrate::Baud1500000, + "2000000" => Baudrate::Baud2000000, + "2470000" => Baudrate::Baud2470000, + _ => return Err("Baudrate identifier not found"), + }; + + return Ok(baudrate); + } +} + +impl From for String { + fn from(value: Baudrate) -> Self { + match value { + Baudrate::BaudAuto => "0", + Baudrate::Baud1200 => "1200", + Baudrate::Baud2400 => "2400", + Baudrate::Baud4800 => "4800", + Baudrate::Baud9600 => "9600", + Baudrate::Baud19200 => "19200", + Baudrate::Baud38400 => "38400", + Baudrate::Baud57600 => "57600", + Baudrate::Baud115200 => "115200", + Baudrate::Baud230400 => "230400", + Baudrate::Baud250000 => "250000", + Baudrate::Baud460800 => "460800", + Baudrate::Baud921600 => "921600", + Baudrate::Baud1000000 => "1000000", + Baudrate::Baud1500000 => "1500000", + Baudrate::Baud2000000 => "2000000", + Baudrate::Baud2470000 => "2470000", + }.to_owned() + } +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(endian = "lsb", bit_numbering = "msb0")] +pub struct MspSerialSetting { + #[packed_field(size_bits="8", ty="enum")] + pub identifier: SerialIdentifier, + pub function_mask: u32, + #[packed_field(size_bits="8", ty="enum")] + pub msp_baudrate_index: Baudrate, + #[packed_field(size_bits="8", ty="enum")] + pub gps_baudrate_index: Baudrate, + #[packed_field(size_bits="8", ty="enum")] + pub telemetry_baudrate_index: Baudrate, + #[packed_field(size_bits="8", ty="enum")] + pub peripheral_baudrate_index: Baudrate, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetServoMixRule { + pub index: u8, + #[packed_field(size_bytes="8")] + pub servo_rule: MspServoMixRule, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "8", endian = "lsb", bit_numbering = "msb0")] +pub struct MspServoMixRule { + pub target_channel: u8, + pub input_source: u8, + pub rate: u16, + pub speed: u8, + pub min: u8, + pub max: u8, + pub box_id: u8, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "1", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSetServoMixer { + pub index: u8, + #[packed_field(size_bytes="6")] + pub servo_rule: MspServoMixer, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "6", endian = "lsb", bit_numbering = "msb0")] +pub struct MspServoMixer { + pub target_channel: u8, + pub input_source: u8, + pub rate: i16, + pub speed: u8, + pub condition_id: i8, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(endian = "lsb", bit_numbering = "msb0")] +pub struct MspRxMap { + pub map: [u8; 4], // MAX_MAPPABLE_RX_INPUTS +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(endian = "lsb", bit_numbering = "msb0")] +pub struct MspSettingGroup { + pub group_id: u16, + pub start_id: u16, + pub end_id: u16, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(endian = "lsb", bit_numbering = "msb0")] +pub struct MspSettingInfoRequest { + pub null: u8, + pub id: u16, +} + + + +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +pub enum SettingMode { + ModeDirect = 0, + ModeLookup = 0x40, +} + +#[derive(PrimitiveEnum, Serialize, Deserialize, Debug, Copy, Clone, PartialEq)] +pub enum SettingType { + VarUint8 = 0, + VarInt8, + VarUint16, + VarInt16, + VarUint32, + #[cfg(feature = "suppport_int32_setting_type")] + VarInt32, + VarFloat, + VarString, +} + +#[derive(PackedStruct, Debug, Copy, Clone)] +#[packed_struct(bytes = "15", endian = "lsb", bit_numbering = "msb0")] +pub struct MspSettingInfo { + // pub name: [u8; ?], null terminated strings + + // Parameter Group ID + pub group_id: u16, + + // Type, section and mode + #[packed_field(size_bits="8", ty="enum")] + pub setting_type: SettingType, + pub setting_section: u8, + #[packed_field(size_bits="8", ty="enum")] + pub setting_mode: SettingMode, + + pub min: u32, + pub max: u32, + + // Absolute setting index + pub absolute_index: u16, + + // If the setting is profile based, send the current one + // and the count, both as uint8_t. For MASTER_VALUE, we + // send two zeroes, so the MSP client can assume there + pub profile_id: u8, + pub profile_count: u8, + + // if setting uses enum values, it will be written here + // pub enum_names: [String; ?] // TODO: packed_struct should support null terminated string parsing + // pub value: [u8; ?] +} + #[test] fn test_mixer() { use packed_struct::prelude::*; @@ -449,4 +821,4 @@ fn test_mixer() { assert_eq!(3, m.mixer_mode.to_primitive()); let p = m.pack(); assert_eq!(&[3], &p); -} \ No newline at end of file +}