extern crate alloc;
extern crate multiwii_serial_protocol_v2;
extern crate serialport;
extern crate packed_struct;
use multiwii_serial_protocol_v2::{MspCommandCode, MspPacket, MspPacketDirection};
use multiwii_serial_protocol_v2::structs::*;
use serialport::SerialPort;
use packed_struct::prelude::*;
use async_std::sync::{channel, Sender, Receiver};
use async_std::{io, task};
use async_std::future;
use std::time::Duration;
mod core;
pub struct MspDataFlashReplyWithData {
pub read_address: u32,
pub payload: Vec<u8>,
}
pub struct FlashDataFile {
core: core::Core,
chunk_recv: Receiver<Result<Vec<u8>, ()>>,
used_size: u32,
next_address: u32,
received_address: u32,
}
#[derive(Debug)]
pub struct SettingInfo {
pub info: MspSettingInfo,
pub name: String,
pub value: Vec<u8>,
pub enum_names: Vec<String>,
}
impl FlashDataFile {
pub async fn read_chunk(&mut self) -> io::Result<Vec<u8>> {
if self.received_address >= self.used_size {
return Err(io::Error::new(io::ErrorKind::ConnectionReset, "use after close"));
}
loop {
if self.next_address > self.received_address || self.next_address == 0 {
let payload = MspDataFlashRead {
read_address: self.next_address,
read_length: 0x1000,
};
let packed = payload.pack();
let packet = MspPacket {
cmd: MspCommandCode::MSP_DATAFLASH_READ as u16,
direction: MspPacketDirection::ToFlightController,
data: packed.to_vec(),
};
self.core.write(packet).await;
}
let timeout_res = future::timeout(Duration::from_millis(50), self.chunk_recv.recv()).await;
if timeout_res.is_ok() {
match timeout_res.unwrap() {
None => return Err(io::Error::new(io::ErrorKind::ConnectionAborted, "device disconnected")),
Some(payload) => {
let packet = INavMsp::parse_chunk(payload.unwrap());
if packet.read_address >= self.next_address {
self.received_address = packet.read_address;
self.next_address = packet.read_address + packet.payload.len() as u32;
} else {
continue;
}
println!("{:?}/{:?}", packet.read_address, self.used_size);
if self.received_address >= self.used_size {
return Ok(vec![]);
}
return Ok(packet.payload);
}
}
} else {
self.core.reset_parser().await;
}
}
}
}
pub struct INavMsp {
core: core::Core,
mode_ranges: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_mode_range_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
motor_mixers: (Sender<Result<Vec<u8>, ()>>,Receiver<Result<Vec<u8>, ()>>),
set_motor_mixer_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
osd_configs: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_osd_config_ack: (Sender<Result<Vec<u8>, ()>>,Receiver<Result<Vec<u8>, ()>>),
serial_settings: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_serial_settings_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
features: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_features_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
servo_mix_rules: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_servo_mix_rules_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
rx_map: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_rx_map_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
pg_settings: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
setting_info: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
set_setting_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
summary: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
chunk: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
write_eeprom_ack: (Sender<Result<Vec<u8>, ()>>, Receiver<Result<Vec<u8>, ()>>),
}
impl INavMsp {
pub fn new() -> INavMsp {
let core = core::Core::new();
return INavMsp {
core: core,
mode_ranges: channel::<Result<Vec<u8>, ()>>(100),
set_mode_range_ack: channel::<Result<Vec<u8>, ()>>(100),
motor_mixers: channel::<Result<Vec<u8>, ()>>(100),
set_motor_mixer_ack: channel::<Result<Vec<u8>, ()>>(100),
osd_configs: channel::<Result<Vec<u8>, ()>>(100),
set_osd_config_ack: channel::<Result<Vec<u8>, ()>>(100),
serial_settings: channel::<Result<Vec<u8>, ()>>(100),
set_serial_settings_ack: channel::<Result<Vec<u8>, ()>>(100),
features: channel::<Result<Vec<u8>, ()>>(100),
set_features_ack: channel::<Result<Vec<u8>, ()>>(100),
servo_mix_rules: channel::<Result<Vec<u8>, ()>>(100),
set_servo_mix_rules_ack: channel::<Result<Vec<u8>, ()>>(100),
rx_map: channel::<Result<Vec<u8>, ()>>(100),
set_rx_map_ack: channel::<Result<Vec<u8>, ()>>(100),
pg_settings: channel::<Result<Vec<u8>, ()>>(100),
setting_info: channel::<Result<Vec<u8>, ()>>(100),
set_setting_ack: channel::<Result<Vec<u8>, ()>>(100),
summary: channel::<Result<Vec<u8>, ()>>(100),
chunk: channel::<Result<Vec<u8>, ()>>(4096),
write_eeprom_ack: channel::<Result<Vec<u8>, ()>>(1),
};
}
pub fn start(&self, serial: Box<dyn SerialPort>) {
&self.core.start(serial);
INavMsp::process_route(
self.core.clone(),
self.mode_ranges.0.clone(),
self.set_mode_range_ack.0.clone(),
self.motor_mixers.0.clone(),
self.set_motor_mixer_ack.0.clone(),
self.osd_configs.0.clone(),
self.set_osd_config_ack.0.clone(),
self.serial_settings.0.clone(),
self.set_serial_settings_ack.0.clone(),
self.features.0.clone(),
self.set_features_ack.0.clone(),
self.servo_mix_rules.0.clone(),
self.set_servo_mix_rules_ack.0.clone(),
self.rx_map.0.clone(),
self.set_rx_map_ack.0.clone(),
self.pg_settings.0.clone(),
self.setting_info.0.clone(),
self.set_setting_ack.0.clone(),
self.summary.0.clone(),
self.chunk.0.clone(),
self.write_eeprom_ack.0.clone(),
);
}
fn process_route(
core: core::Core,
mode_ranges_send: Sender<Result<Vec<u8>, ()>>,
set_mode_range_ack_send: Sender<Result<Vec<u8>, ()>>,
motor_mixers_send: Sender<Result<Vec<u8>, ()>>,
set_motor_mixer_ack_send: Sender<Result<Vec<u8>, ()>>,
osd_configs_send: Sender<Result<Vec<u8>, ()>>,
set_osd_config_ack_send: Sender<Result<Vec<u8>, ()>>,
serial_settings_send: Sender<Result<Vec<u8>, ()>>,
set_serial_settings_ack_send: Sender<Result<Vec<u8>, ()>>,
features_send: Sender<Result<Vec<u8>, ()>>,
set_features_ack_send: Sender<Result<Vec<u8>, ()>>,
servo_mix_rules_send: Sender<Result<Vec<u8>, ()>>,
set_servo_mix_rules_ack_send: Sender<Result<Vec<u8>, ()>>,
rx_map_send: Sender<Result<Vec<u8>, ()>>,
set_rx_map_ack_send: Sender<Result<Vec<u8>, ()>>,
pg_settings: Sender<Result<Vec<u8>, ()>>,
setting_info: Sender<Result<Vec<u8>, ()>>,
set_setting_ack: Sender<Result<Vec<u8>, ()>>,
summary_send: Sender<Result<Vec<u8>, ()>>,
chunk_send: Sender<Result<Vec<u8>, ()>>,
write_eeprom_ack: Sender<Result<Vec<u8>, ()>>,
) {
task::spawn(async move {
loop {
let packet = match core.read().await {
None => break,
Some(packet) => packet,
};
let cmd = MspCommandCode::from_primitive(packet.cmd);
let result = match packet.direction {
MspPacketDirection::FromFlightController => Ok(packet.data),
MspPacketDirection::Unsupported => Err(()),
_ => continue,
};
let channel = match cmd {
Some(MspCommandCode::MSP_MODE_RANGES) => &mode_ranges_send,
Some(MspCommandCode::MSP_SET_MODE_RANGE) => &set_mode_range_ack_send,
Some(MspCommandCode::MSP2_MOTOR_MIXER) => &motor_mixers_send,
Some(MspCommandCode::MSP2_SET_MOTOR_MIXER) => &set_motor_mixer_ack_send,
Some(MspCommandCode::MSP_OSD_CONFIG) => &osd_configs_send,
Some(MspCommandCode::MSP_SET_OSD_CONFIG) => &set_osd_config_ack_send,
Some(MspCommandCode::MSP2_SERIAL_CONFIG) => &serial_settings_send,
Some(MspCommandCode::MSP2_SET_SERIAL_CONFIG) => &set_serial_settings_ack_send,
Some(MspCommandCode::MSP_FEATURE) => &features_send,
Some(MspCommandCode::MSP_SET_FEATURE) => &set_features_ack_send,
Some(MspCommandCode::MSP_SERVO_MIX_RULES) => &servo_mix_rules_send,
Some(MspCommandCode::MSP_SET_SERVO_MIX_RULE) => &set_servo_mix_rules_ack_send,
Some(MspCommandCode::MSP_RX_MAP) => &rx_map_send,
Some(MspCommandCode::MSP_SET_RX_MAP) => &set_rx_map_ack_send,
Some(MspCommandCode::MSP2_COMMON_PG_LIST) => &pg_settings,
Some(MspCommandCode::MSP2_COMMON_SETTING_INFO) => &setting_info,
Some(MspCommandCode::MSP2_COMMON_SET_SETTING) => &set_setting_ack,
Some(MspCommandCode::MSP_DATAFLASH_SUMMARY) => &summary_send,
Some(MspCommandCode::MSP_DATAFLASH_READ) => &chunk_send,
Some(MspCommandCode::MSP_EEPROM_WRITE) => &write_eeprom_ack,
_ => continue,
};
channel.send(result).await;
}
});
}
pub async fn open_flash_data(&self) -> FlashDataFile {
let summary = self.flash_summary().await.unwrap();
let used_size = summary.used_size_bytes;
return FlashDataFile {
core: self.core.clone(),
chunk_recv: self.chunk.1.clone(),
used_size: used_size,
next_address: 0u32,
received_address: 0u32,
};
}
pub fn parse_chunk(payload: Vec<u8>) -> MspDataFlashReplyWithData {
let flash_reply = MspDataFlashReply::unpack_from_slice(&payload[..4]).unwrap();
let packet_payload = &payload[4..];
return MspDataFlashReplyWithData {
read_address: flash_reply.read_address,
payload: packet_payload.to_vec(),
};
}
pub async fn read_flash_data(&self, chunk_size: usize, callback: fn(chunk: usize, total: usize)) -> io::Result<Vec<u8>> {
let summary = self.flash_summary().await.unwrap();
let used_size = summary.used_size_bytes as usize;
let blocks_count = used_size / chunk_size;
let mut expected_address = vec![];
for x in (0..blocks_count * chunk_size).step_by(chunk_size as usize) {
&expected_address.push(x);
}
println!("expected_address: {:?}", &expected_address.len());
let mut accumulated_payload = vec![vec![]; blocks_count as usize];
loop {
for addr in &expected_address {
let payload = MspDataFlashRead {
read_address: *addr as u32,
read_length: chunk_size as u16,
};
let packed = payload.pack();
let packet = MspPacket {
cmd: MspCommandCode::MSP_DATAFLASH_READ as u16,
direction: MspPacketDirection::ToFlightController,
data: packed.to_vec(),
};
self.core.write(packet).await;
}
loop {
let timeout_res = future::timeout(Duration::from_millis(500), self.chunk.1.recv()).await;
if !timeout_res.is_ok() {
self.core.reset_parser().await;
break;
}
match timeout_res.unwrap() {
None => return Err(io::Error::new(io::ErrorKind::ConnectionAborted, "device disconnected")),
Some(payload) => {
let packet = INavMsp::parse_chunk(payload.unwrap());
let idx = match expected_address.binary_search(&(packet.read_address as usize)) {
Ok(idx) => idx,
Err(_) => continue,
};
let insert_location = &(packet.read_address as usize) / &chunk_size;
(&mut accumulated_payload)[insert_location as usize] = packet.payload;
expected_address.remove(idx);
callback(used_size - expected_address.len() * chunk_size, used_size);
if expected_address.is_empty() {
let buff = accumulated_payload.iter().cloned().flatten().collect::<Vec<u8>>();
return Ok(buff);
}
}
}
}
}
}
pub async fn flash_summary(&self) -> Result<MspDataFlashSummaryReply, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_DATAFLASH_SUMMARY as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.summary.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get flash summary")
};
let summary = MspDataFlashSummaryReply::unpack_from_slice(&payload).unwrap();
return Ok(summary);
}
pub async fn set_mode_range(&self, index: u8, range: MspModeRange) -> Result<(), &str>{
let payload = MspSetModeRange {
index: index,
mode_range: range,
};
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_MODE_RANGE as u16,
direction: MspPacketDirection::ToFlightController,
data: payload.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_mode_range_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set mode range")
};
}
pub async fn get_mode_ranges(&self) -> Result<Vec<MspModeRange>, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_MODE_RANGES as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.mode_ranges.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get mode_ranges")
};
let mut ranges = vec![];
let len = MspModeRange::packed_bytes();
for i in (0..payload.len()).step_by(len) {
let r = MspModeRange::unpack_from_slice(&payload[i..i+len]).unwrap();
ranges.push(r);
}
return Ok(ranges);
}
pub async fn set_motor_mixer(&self, index: u8, mmix: MspMotorMixer) -> Result<(), &str> {
let payload = MspSetMotorMixer {
index: index,
motor_mixer: MspMotorMixer {
throttle: mmix.throttle,
roll: mmix.roll,
pitch: mmix.pitch,
yaw: mmix.yaw,
}
};
let packet = MspPacket {
cmd: MspCommandCode::MSP2_SET_MOTOR_MIXER as u16,
direction: MspPacketDirection::ToFlightController,
data: payload.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_motor_mixer_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set motor mixer")
};
}
pub async fn get_motor_mixers(&self) -> Result<Vec<MspMotorMixer>, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP2_MOTOR_MIXER as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.motor_mixers.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get motor mixers")
};
let mut mmixers = vec![];
let len = MspMotorMixer::packed_bytes();
for i in (0..payload.len()).step_by(len) {
let m = MspMotorMixer::unpack_from_slice(&payload[i..i+len]).unwrap();
if m.throttle != 0 {
mmixers.push(m);
}
}
return Ok(mmixers);
}
pub async fn set_osd_config_item(&self, id: u8, item: MspOsdItemPosition) -> Result<(), &str> {
let payload = MspSetOsdLayout {
item_index: id,
item: item,
};
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_OSD_CONFIG as u16,
direction: MspPacketDirection::ToFlightController,
data: payload.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_osd_config_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set osd item")
};
}
pub async fn set_osd_config(&self, config: MspOsdConfig) -> Result<(), &str> {
let payload = MspSetGetOsdConfig {
item_index: 0xffu8,
config: config,
};
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_OSD_CONFIG as u16,
direction: MspPacketDirection::ToFlightController,
data: payload.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_osd_config_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set osd config")
};
}
pub async fn get_osd_settings(&self) -> Result<MspOsdSettings, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_OSD_CONFIG as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.osd_configs.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get osd config")
};
let header_len = MspSetGetOsdConfig::packed_bytes();
let osd_set_get_reply = MspSetGetOsdConfig::unpack_from_slice(&payload[..header_len]).unwrap();
let mut item_positions = vec![];
let len = MspOsdItemPosition::packed_bytes();
for i in (header_len..payload.len()).step_by(len) {
let item_pos = MspOsdItemPosition::unpack_from_slice(&payload[i..i+len]).unwrap();
item_positions.push(item_pos);
}
return Ok(MspOsdSettings {
osd_support: osd_set_get_reply.item_index,
config: osd_set_get_reply.config,
item_positions: item_positions,
});
}
pub async fn set_serial_settings(&self, serials: Vec<MspSerialSetting>) -> Result<(), &str> {
let payload = serials.iter().flat_map(|s| s.pack().to_vec()).collect();
let packet = MspPacket {
cmd: MspCommandCode::MSP2_SET_SERIAL_CONFIG as u16,
direction: MspPacketDirection::ToFlightController,
data: payload,
};
self.core.write(packet).await;
return match self.set_serial_settings_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set serial settings")
};
}
pub async fn get_serial_settings(&self) -> Result<Vec<MspSerialSetting>, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP2_SERIAL_CONFIG as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.serial_settings.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get serial settings")
};
let mut serials = vec![];
let len = MspSerialSetting::packed_bytes();
for i in (0..payload.len()).step_by(len) {
let serial_setting = MspSerialSetting::unpack_from_slice(&payload[i..i+len]).unwrap();
if serial_setting.identifier != SerialIdentifier::None {
serials.push(serial_setting);
}
}
return Ok(serials);
}
pub async fn set_features(&self, feat: MspFeatures) -> Result<(), &str> {
let mut clone = feat.clone();
clone.features[0..8].reverse();
clone.features[8..16].reverse();
clone.features[16..24].reverse();
clone.features[24..32].reverse();
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_FEATURE as u16,
direction: MspPacketDirection::ToFlightController,
data: clone.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_features_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set features")
};
}
pub async fn get_features(&self) -> Result<MspFeatures, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_FEATURE as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.features.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get features")
};
let mut feat = MspFeatures::unpack_from_slice(&payload).unwrap();
feat.features[0..8].reverse();
feat.features[8..16].reverse();
feat.features[16..24].reverse();
feat.features[24..32].reverse();
return Ok(feat);
}
pub async fn set_servo_mix_rule(&self, index: u8, servo_rule: MspServoMixRule) -> Result<(), &str> {
let payload = MspSetServoMixRule {
index: index,
servo_rule: servo_rule,
};
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_SERVO_MIX_RULE as u16,
direction: MspPacketDirection::ToFlightController,
data: payload.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_servo_mix_rules_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set servo mix rule")
};
}
pub async fn get_servo_mix_rules(&self) -> Result<Vec<MspServoMixRule>, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_SERVO_MIX_RULES as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.servo_mix_rules.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get servo mix rule")
};
let mut rules = vec![];
let len = MspServoMixRule::packed_bytes();
for i in (0..payload.len()).step_by(len) {
let serial_setting = MspServoMixRule::unpack_from_slice(&payload[i..i+len]).unwrap();
if serial_setting.rate != 0 {
rules.push(serial_setting);
}
}
return Ok(rules);
}
pub async fn set_rx_map(&self, rx_map: MspRxMap) -> Result<(), &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_RX_MAP as u16,
direction: MspPacketDirection::ToFlightController,
data: rx_map.pack().to_vec(),
};
self.core.write(packet).await;
return match self.set_rx_map_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed set rx map rules")
};
}
pub async fn get_rx_map(&self) -> Result<MspRxMap, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_RX_MAP as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.rx_map.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get rx map rules")
};
return Ok(MspRxMap::unpack_from_slice(&payload).unwrap());
}
pub fn str_from_u8_nul_utf8(utf8_src: &[u8]) -> Result<&str, std::str::Utf8Error> {
let nul_range_end = utf8_src.iter()
.position(|&c| c == b'\0')
.unwrap_or(utf8_src.len());
::std::str::from_utf8(&utf8_src[0..nul_range_end])
}
pub async fn set_setting_by_id<'a>(&self, id: &'a u16, value: &[u8]) -> Result<&'a u16, &str> {
let payload = MspSettingInfoRequest {
null: 0,
id: *id
};
self.set_setting(&payload.pack(), value).await?;
Ok(id)
}
pub async fn set_setting_by_name(&self, name: &str, value: &[u8]) -> Result<(), &str> {
let mut payload = name.as_bytes().to_vec();
payload.push(b'\0');
return self.set_setting(&payload, value).await;
}
pub async fn set_setting(&self, id: &[u8], value: &[u8]) -> Result<(), &str> {
let mut payload = id.to_vec();
payload.extend(value);
let packet = MspPacket {
cmd: MspCommandCode::MSP2_COMMON_SET_SETTING as u16,
direction: MspPacketDirection::ToFlightController,
data: payload,
};
self.core.write(packet).await;
return match self.set_setting_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to set setting")
};
}
pub async fn get_setting_info_by_name(&self, name: &str) -> Result<SettingInfo, &str> {
let mut payload = name.as_bytes().to_vec();
payload.push(b'\0');
return self.get_setting_info(payload).await;
}
pub async fn get_setting_info_by_id(&self, id: &u16) -> Result<SettingInfo, &str> {
let payload = MspSettingInfoRequest {
null: 0,
id: *id
};
return self.get_setting_info(payload.pack().to_vec()).await;
}
pub async fn get_setting_info(&self, id: Vec<u8>) -> Result<SettingInfo, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP2_COMMON_SETTING_INFO as u16,
direction: MspPacketDirection::ToFlightController,
data: id,
};
self.core.write(packet).await;
let payload = match self.setting_info.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get setting info"),
};
let name = INavMsp::str_from_u8_nul_utf8(&payload).unwrap();
let len = MspSettingInfo::packed_bytes();
let mut index = name.len() + 1;
let setting_info = MspSettingInfo::unpack_from_slice(&payload[index..index + len]).expect("Failed to parse inavlid msp setting");
index += len;
let mut enum_values = vec![];
if setting_info.setting_mode == SettingMode::ModeLookup {
for _ in setting_info.min..setting_info.max + 1 {
let enum_value = INavMsp::str_from_u8_nul_utf8(&payload[index..]).unwrap();
index += enum_value.len() + 1;
enum_values.push(enum_value);
}
}
let value = &payload[index..];
return Ok(SettingInfo {
name: String::from(name),
value: value.to_vec(),
info: setting_info,
enum_names: enum_values.iter().map(|&s| String::from(s)).collect(),
});
}
pub async fn get_pg_settings(&self) -> Result<Vec<MspSettingGroup>, &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP2_COMMON_PG_LIST as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
let payload = match self.pg_settings.1.recv().await.unwrap() {
Ok(r) => r,
Err(_) => return Err("failed to get pg settings")
};
let mut setting_ids = vec![];
let len = MspSettingGroup::packed_bytes();
for i in (0..payload.len()).step_by(len) {
let setting_id = MspSettingGroup::unpack_from_slice(&payload[i..i+len]).unwrap();
setting_ids.push(setting_id);
}
return Ok(setting_ids);
}
pub async fn save_to_eeprom(&self) -> Result<(), &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_EEPROM_WRITE as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
return match self.write_eeprom_ack.1.recv().await.unwrap() {
Ok(_) => Ok(()),
Err(_) => Err("failed to write to eeprom")
};
}
pub async fn reboot(&self) -> Result<(), &str> {
let packet = MspPacket {
cmd: MspCommandCode::MSP_SET_REBOOT as u16,
direction: MspPacketDirection::ToFlightController,
data: vec![],
};
self.core.write(packet).await;
Ok(())
}
}