From ae9c72b86aefdd8427683881b812f5d084024639 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Wed, 28 Jun 2023 20:29:28 +0200 Subject: [PATCH 1/7] implement ReadParamsExt --- simplebgc/src/commands/mod.rs | 1 + simplebgc/src/commands/read_params.rs | 180 ++++++++++++++++++++++++++ simplebgc/src/message.rs | 3 + 3 files changed, 184 insertions(+) diff --git a/simplebgc/src/commands/mod.rs b/simplebgc/src/commands/mod.rs index 6adefc0..598f0dd 100644 --- a/simplebgc/src/commands/mod.rs +++ b/simplebgc/src/commands/mod.rs @@ -35,6 +35,7 @@ pub enum IncomingCommand { GetAngles(RollPitchYaw), ReadParams(Params3Data), ReadParams3(Params3Data), + ReadParamsExt(ParamsExtData), RealtimeData3(RealtimeData3), } diff --git a/simplebgc/src/commands/read_params.rs b/simplebgc/src/commands/read_params.rs index 115c79e..4d2990e 100644 --- a/simplebgc/src/commands/read_params.rs +++ b/simplebgc/src/commands/read_params.rs @@ -396,6 +396,43 @@ pub enum ImuType { Frame, } +#[bitflags] +#[derive(Copy, Clone, Debug, PartialEq)] +#[repr(u8)] +pub enum FiltersEnFlags { + EnableNotch1 = 1, + EnableNotch2 = 2, + EnableNotch3 = 4, + EnableLPF = 8, +} + + +#[derive(FromPrimitive, ToPrimitive, Copy, Clone, Debug, PartialEq)] +#[repr(u8)] +#[allow(non_camel_case_types)] +pub enum EncoderType { + AS5048A = 1, + AS5048B = 2, + AS5048_PWM = 3, + AMT203 = 4, + MA3_10BIT = 5, + MA3_12BIT = 6, + ANALOG = 7, + I2C_DRV1 = 8, + I2C_DRV2 = 9, + I2C_DRV3 = 10, + I2C_DRV4 = 11, + AS5600_PWM = 12, + AS5600_I2C = 13, + RLS_ORBIS = 14, + ORBIS_PWM = 15, + + SkipDetectionFlag = 1 << 4, + EncoderIsGeared = 1 << 7, +} + + + #[derive(BgcPayload, Copy, Clone, Debug, PartialEq)] pub struct RcMixes { #[kind(payload)] @@ -658,3 +695,146 @@ pub struct Params3Data { #[kind(raw)] pub cur_profile_id: u8, } + +#[derive(BgcPayload, Clone, Debug, PartialEq)] +pub struct ParamsExtData { + /// profile ID to read or write. To access current (active) profile, + /// specify 255. Possible values: 0..4 + #[kind(raw)] + pub profile_id: u8, + + #[kind(payload)] + #[size(6)] + pub notch1: NotchParams, + #[kind(payload)] + #[size(6)] + pub notch3: NotchParams, + #[kind(payload)] + #[size(6)] + pub notch2: NotchParams, + + #[kind(payload)] + #[size(6)] + pub lpf_freq: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub filters_en: RollPitchYaw, //RollPitchYaw>, but it does not work, not sure how to implement + + ///units: 0.02197265625 deg, aka 2^14 units per turn + #[kind(payload)] + #[size(6)] + pub encoder_offset: RollPitchYaw, + + #[kind(payload)] + #[size(6)] + pub encoder_field_offset: RollPitchYaw, + + /// unit: 10ms + #[kind(payload)] + #[size(3)] + pub encoder_manual_set_time: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub motor_heating_factor: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub motor_cooling_factor: RollPitchYaw, + + #[kind(raw)] + pub reserved: [u8; 2], + + #[kind(raw)] + pub follow_inside_deadband: u8, + + ///deprecated + #[kind(payload)] + #[size(3)] + pub motor_mag_link: RollPitchYaw, + + ///8.8 fixed-point (1.0f <-> 256) + #[kind(payload)] + #[size(6)] + pub motor_gearing: RollPitchYaw, + + ///deprecated + #[kind(payload)] + #[size(3)] + pub motor_coolingencoder_limit_min: RollPitchYaw, + + ///deprecated + #[kind(payload)] + #[size(3)] + pub motor_coolingencoder_limit_max: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub notch1_gain: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub notch2_gain: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub notch3_gain: RollPitchYaw, + + #[kind(raw)] + pub beeper_volume: u8, + + ///unit: 0.001 + #[kind(payload)] + #[size(6)] + pub encoder_gear_ratio: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub encoder_type: RollPitchYaw, //#FIXME: EncoderType enum + bitflags, not sure how to implement + + #[kind(payload)] + #[size(3)] + pub encoder_cfg: RollPitchYaw, //#FIXME: enum or "internal encoder type", not sure how to implement + + #[kind(payload)] + #[size(3)] + pub outer_p: RollPitchYaw, + + #[kind(payload)] + #[size(3)] + pub outer_i: RollPitchYaw, + + #[kind(enumeration)] + #[format(i8)] + pub mag_axis_top: Orientation, + + #[kind(enumeration)] + #[format(i8)] + pub mag_axis_right: Orientation, + + #[kind(raw)] + pub mag_trust: u8, + + ///unit: 1 degree + #[kind(raw)] + pub mag_declination: i8, + + #[kind(raw)] + pub acc_lpf_freq: u16, + + #[kind(payload)] + #[size(3)] + pub d_term_lpf_freq: RollPitchYaw, + +} + +#[derive(BgcPayload, Copy, Clone, Debug, PartialEq)] +pub struct NotchParams { + #[kind(payload)] + #[size(3)] + pub freq: RollPitchYaw, + #[kind(payload)] + #[size(3)] + pub bw: RollPitchYaw, +} diff --git a/simplebgc/src/message.rs b/simplebgc/src/message.rs index f0a26ae..d69e089 100644 --- a/simplebgc/src/message.rs +++ b/simplebgc/src/message.rs @@ -297,6 +297,7 @@ impl Message for IncomingCommand { IncomingCommand::GetAngles(_) => CMD_GET_ANGLES, IncomingCommand::ReadParams(_) => CMD_READ_PARAMS, IncomingCommand::ReadParams3(_) => CMD_READ_PARAMS_3, + IncomingCommand::ReadParamsExt(_) => CMD_READ_PARAMS_EXT, IncomingCommand::RealtimeData3(_) => CMD_REALTIME_DATA_3, } } @@ -311,6 +312,7 @@ impl Message for IncomingCommand { GetAngles(angles) => Payload::to_bytes(angles), ReadParams(params) => Payload::to_bytes(params), ReadParams3(params) => Payload::to_bytes(params), + ReadParamsExt(params) => Payload::to_bytes(params), RealtimeData3(data) => Payload::to_bytes(data), } } @@ -329,6 +331,7 @@ impl Message for IncomingCommand { CMD_GET_ANGLES => GetAngles(Payload::from_bytes(bytes)?), CMD_READ_PARAMS => ReadParams(Payload::from_bytes(bytes)?), CMD_READ_PARAMS_3 => ReadParams3(Payload::from_bytes(bytes)?), + CMD_READ_PARAMS_EXT => ReadParamsExt(Payload::from_bytes(bytes)?), CMD_REALTIME_DATA_3 => RealtimeData3(Payload::from_bytes(bytes)?), _ => return Err(MessageParseError::BadCommandId { id }), }) From c430c2434260f167f5b8c221be8054210fa8b977 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Thu, 29 Jun 2023 13:17:12 +0200 Subject: [PATCH 2/7] Fix #7 wrong byte order for RollPitchYaw --- simplebgc/src/payload.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplebgc/src/payload.rs b/simplebgc/src/payload.rs index 41589a3..59966cb 100644 --- a/simplebgc/src/payload.rs +++ b/simplebgc/src/payload.rs @@ -74,7 +74,7 @@ impl Payload for i16 { where Self: Sized, { - Ok(b.get_i16()) + Ok(b.get_i16_le()) } fn to_bytes(&self) -> Bytes From 325f3c2d178793a37bc091f130b60af419657cb4 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Thu, 29 Jun 2023 16:10:17 +0200 Subject: [PATCH 3/7] fixup: byte order for u16 too --- simplebgc/src/payload.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplebgc/src/payload.rs b/simplebgc/src/payload.rs index 59966cb..f3db9a6 100644 --- a/simplebgc/src/payload.rs +++ b/simplebgc/src/payload.rs @@ -58,7 +58,7 @@ impl Payload for u16 { where Self: Sized, { - Ok(b.get_u16()) + Ok(b.get_u16_le()) } fn to_bytes(&self) -> Bytes From 868d83fddf5d545f66b2d917e923bfc5e4aae7f6 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Thu, 29 Jun 2023 18:27:33 +0200 Subject: [PATCH 4/7] Support unsupported messages ... by emitting/accepting a RawMessage when the message type is not implemented. This makes it possible to extend this library from within a project that uses it, which is particularly important for handling commands with variable footprint, such as CMD_REALTIME_DATA_CUSTOM --- simplebgc/src/commands/constants.rs | 194 +++++++++++++------------- simplebgc/src/commands/mod.rs | 8 +- simplebgc/src/commands/raw_message.rs | 26 ++++ simplebgc/src/message.rs | 6 +- 4 files changed, 134 insertions(+), 100 deletions(-) create mode 100644 simplebgc/src/commands/raw_message.rs diff --git a/simplebgc/src/commands/constants.rs b/simplebgc/src/commands/constants.rs index 5fc9034..41495fd 100644 --- a/simplebgc/src/commands/constants.rs +++ b/simplebgc/src/commands/constants.rs @@ -1,100 +1,100 @@ //! copied from spec pdf and then edited using //! regex replace #![allow(unused)] -pub(crate) const CMD_READ_PARAMS: u8 = 82; -pub(crate) const CMD_WRITE_PARAMS: u8 = 87; -pub(crate) const CMD_REALTIME_DATA: u8 = 68; -pub(crate) const CMD_BOARD_INFO: u8 = 86; -pub(crate) const CMD_CALIB_ACC: u8 = 65; -pub(crate) const CMD_CALIB_GYRO: u8 = 103; -pub(crate) const CMD_CALIB_EXT_GAIN: u8 = 71; -pub(crate) const CMD_USE_DEFAULTS: u8 = 70; -pub(crate) const CMD_CALIB_POLES: u8 = 80; -pub(crate) const CMD_RESET: u8 = 114; -pub(crate) const CMD_HELPER_DATA: u8 = 72; -pub(crate) const CMD_CALIB_OFFSET: u8 = 79; -pub(crate) const CMD_CALIB_BAT: u8 = 66; -pub(crate) const CMD_MOTORS_ON: u8 = 77; -pub(crate) const CMD_MOTORS_OFF: u8 = 109; -pub(crate) const CMD_CONTROL: u8 = 67; -pub(crate) const CMD_TRIGGER_PIN: u8 = 84; -pub(crate) const CMD_EXECUTE_MENU: u8 = 69; -pub(crate) const CMD_GET_ANGLES: u8 = 73; -pub(crate) const CMD_CONFIRM: u8 = 67; -pub(crate) const CMD_BOARD_INFO_3: u8 = 20; -pub(crate) const CMD_READ_PARAMS_3: u8 = 21; -pub(crate) const CMD_WRITE_PARAMS_3: u8 = 22; -pub(crate) const CMD_REALTIME_DATA_3: u8 = 23; -pub(crate) const CMD_REALTIME_DATA_4: u8 = 25; -pub(crate) const CMD_SELECT_IMU_3: u8 = 24; -pub(crate) const CMD_READ_PROFILE_NAMES: u8 = 28; -pub(crate) const CMD_WRITE_PROFILE_NAMES: u8 = 29; -pub(crate) const CMD_QUEUE_PARAMS_INFO_3: u8 = 30; -pub(crate) const CMD_SET_ADJ_VARS_VAL: u8 = 31; -pub(crate) const CMD_SAVE_PARAMS_3: u8 = 32; -pub(crate) const CMD_READ_PARAMS_EXT: u8 = 33; -pub(crate) const CMD_WRITE_PARAMS_EXT: u8 = 34; -pub(crate) const CMD_AUTO_PID: u8 = 35; -pub(crate) const CMD_SERVO_OUT: u8 = 36; -pub(crate) const CMD_I2C_WRITE_REG_BUF: u8 = 39; -pub(crate) const CMD_I2C_READ_REG_BUF: u8 = 40; -pub(crate) const CMD_WRITE_EXTERNAL_DATA: u8 = 41; -pub(crate) const CMD_READ_EXTERNAL_DATA: u8 = 42; -pub(crate) const CMD_READ_ADJ_VARS_CFG: u8 = 43; -pub(crate) const CMD_WRITE_ADJ_VARS_CFG: u8 = 44; -pub(crate) const CMD_API_VIRT_CH_CONTROL: u8 = 45; -pub(crate) const CMD_ADJ_VARS_STATE: u8 = 46; -pub(crate) const CMD_EEPROM_WRITE: u8 = 47; -pub(crate) const CMD_EEPROM_READ: u8 = 48; -pub(crate) const CMD_CALIB_INFO: u8 = 49; -pub(crate) const CMD_SIGN_MESSAGE: u8 = 50; -pub(crate) const CMD_BOOT_MODE_3: u8 = 51; -pub(crate) const CMD_SYSTEM_STATE: u8 = 52; -pub(crate) const CMD_READ_FILE: u8 = 53; -pub(crate) const CMD_WRITE_FILE: u8 = 54; -pub(crate) const CMD_FS_CLEAR_ALL: u8 = 55; -pub(crate) const CMD_AHRS_HELPER: u8 = 56; -pub(crate) const CMD_RUN_SCRIPT: u8 = 57; -pub(crate) const CMD_SCRIPT_DEBUG: u8 = 58; -pub(crate) const CMD_CALIB_MAG: u8 = 59; -pub(crate) const CMD_GET_ANGLES_EXT: u8 = 61; -pub(crate) const CMD_READ_PARAMS_EXT2: u8 = 62; -pub(crate) const CMD_WRITE_PARAMS_EXT2: u8 = 63; -pub(crate) const CMD_GET_ADJ_VARS_VAL: u8 = 64; -pub(crate) const CMD_CALIB_MOTOR_MAG_LINK: u8 = 74; -pub(crate) const CMD_GYRO_CORRECTION: u8 = 75; -pub(crate) const CMD_DATA_STREAM_INTERVAL: u8 = 85; -pub(crate) const CMD_REALTIME_DATA_CUSTOM: u8 = 88; -pub(crate) const CMD_BEEP_SOUND: u8 = 89; -pub(crate) const CMD_ENCODERS_CALIB_OFFSET_4: u8 = 26; -pub(crate) const CMD_ENCODERS_CALIB_FLD_OFFSET_4: u8 = 27; -pub(crate) const CMD_CONTROL_CONFIG: u8 = 90; -pub(crate) const CMD_CALIB_ORIENT_CORR: u8 = 91; -pub(crate) const CMD_COGGING_CALIB_INFO: u8 = 92; -pub(crate) const CMD_CALIB_COGGING: u8 = 93; -pub(crate) const CMD_CALIB_ACC_EXT_REF: u8 = 94; -pub(crate) const CMD_PROFILE_SET: u8 = 95; -pub(crate) const CMD_CAN_DEVICE_SCAN: u8 = 96; -pub(crate) const CMD_CAN_DRV_HARD_PARAMS: u8 = 97; -pub(crate) const CMD_CAN_DRV_STATE: u8 = 98; -pub(crate) const CMD_CAN_DRV_CALIBRATE: u8 = 99; -pub(crate) const CMD_READ_RC_INPUTS: u8 = 100; -pub(crate) const CMD_REALTIME_DATA_CAN_DRV: u8 = 101; -pub(crate) const CMD_EVENT: u8 = 102; -pub(crate) const CMD_READ_PARAMS_EXT3: u8 = 104; -pub(crate) const CMD_WRITE_PARAMS_EXT3: u8 = 105; -pub(crate) const CMD_EXT_IMU_DEBUG_INFO: u8 = 106; -pub(crate) const CMD_SET_DEVICE_ADDR: u8 = 107; -pub(crate) const CMD_AUTO_PID2: u8 = 108; -pub(crate) const CMD_EXT_IMU_CMD: u8 = 110; -pub(crate) const CMD_READ_STATE_VARS: u8 = 111; -pub(crate) const CMD_WRITE_STATE_VARS: u8 = 112; -pub(crate) const CMD_SERIAL_PROXY: u8 = 113; -pub(crate) const CMD_IMU_ADVANCED_CALIB: u8 = 115; -pub(crate) const CMD_API_VIRT_CH_HIGH_RES: u8 = 116; -pub(crate) const CMD_SET_DEBUG_PORT: u8 = 249; -pub(crate) const CMD_MAVLINK_INFO: u8 = 250; -pub(crate) const CMD_MAVLINK_DEBUG: u8 = 251; -pub(crate) const CMD_DEBUG_VARS_INFO_3: u8 = 253; -pub(crate) const CMD_DEBUG_VARS_3: u8 = 254; -pub(crate) const CMD_ERROR: u8 = 255; +pub const CMD_READ_PARAMS: u8 = 82; +pub const CMD_WRITE_PARAMS: u8 = 87; +pub const CMD_REALTIME_DATA: u8 = 68; +pub const CMD_BOARD_INFO: u8 = 86; +pub const CMD_CALIB_ACC: u8 = 65; +pub const CMD_CALIB_GYRO: u8 = 103; +pub const CMD_CALIB_EXT_GAIN: u8 = 71; +pub const CMD_USE_DEFAULTS: u8 = 70; +pub const CMD_CALIB_POLES: u8 = 80; +pub const CMD_RESET: u8 = 114; +pub const CMD_HELPER_DATA: u8 = 72; +pub const CMD_CALIB_OFFSET: u8 = 79; +pub const CMD_CALIB_BAT: u8 = 66; +pub const CMD_MOTORS_ON: u8 = 77; +pub const CMD_MOTORS_OFF: u8 = 109; +pub const CMD_CONTROL: u8 = 67; +pub const CMD_TRIGGER_PIN: u8 = 84; +pub const CMD_EXECUTE_MENU: u8 = 69; +pub const CMD_GET_ANGLES: u8 = 73; +pub const CMD_CONFIRM: u8 = 67; +pub const CMD_BOARD_INFO_3: u8 = 20; +pub const CMD_READ_PARAMS_3: u8 = 21; +pub const CMD_WRITE_PARAMS_3: u8 = 22; +pub const CMD_REALTIME_DATA_3: u8 = 23; +pub const CMD_REALTIME_DATA_4: u8 = 25; +pub const CMD_SELECT_IMU_3: u8 = 24; +pub const CMD_READ_PROFILE_NAMES: u8 = 28; +pub const CMD_WRITE_PROFILE_NAMES: u8 = 29; +pub const CMD_QUEUE_PARAMS_INFO_3: u8 = 30; +pub const CMD_SET_ADJ_VARS_VAL: u8 = 31; +pub const CMD_SAVE_PARAMS_3: u8 = 32; +pub const CMD_READ_PARAMS_EXT: u8 = 33; +pub const CMD_WRITE_PARAMS_EXT: u8 = 34; +pub const CMD_AUTO_PID: u8 = 35; +pub const CMD_SERVO_OUT: u8 = 36; +pub const CMD_I2C_WRITE_REG_BUF: u8 = 39; +pub const CMD_I2C_READ_REG_BUF: u8 = 40; +pub const CMD_WRITE_EXTERNAL_DATA: u8 = 41; +pub const CMD_READ_EXTERNAL_DATA: u8 = 42; +pub const CMD_READ_ADJ_VARS_CFG: u8 = 43; +pub const CMD_WRITE_ADJ_VARS_CFG: u8 = 44; +pub const CMD_API_VIRT_CH_CONTROL: u8 = 45; +pub const CMD_ADJ_VARS_STATE: u8 = 46; +pub const CMD_EEPROM_WRITE: u8 = 47; +pub const CMD_EEPROM_READ: u8 = 48; +pub const CMD_CALIB_INFO: u8 = 49; +pub const CMD_SIGN_MESSAGE: u8 = 50; +pub const CMD_BOOT_MODE_3: u8 = 51; +pub const CMD_SYSTEM_STATE: u8 = 52; +pub const CMD_READ_FILE: u8 = 53; +pub const CMD_WRITE_FILE: u8 = 54; +pub const CMD_FS_CLEAR_ALL: u8 = 55; +pub const CMD_AHRS_HELPER: u8 = 56; +pub const CMD_RUN_SCRIPT: u8 = 57; +pub const CMD_SCRIPT_DEBUG: u8 = 58; +pub const CMD_CALIB_MAG: u8 = 59; +pub const CMD_GET_ANGLES_EXT: u8 = 61; +pub const CMD_READ_PARAMS_EXT2: u8 = 62; +pub const CMD_WRITE_PARAMS_EXT2: u8 = 63; +pub const CMD_GET_ADJ_VARS_VAL: u8 = 64; +pub const CMD_CALIB_MOTOR_MAG_LINK: u8 = 74; +pub const CMD_GYRO_CORRECTION: u8 = 75; +pub const CMD_DATA_STREAM_INTERVAL: u8 = 85; +pub const CMD_REALTIME_DATA_CUSTOM: u8 = 88; +pub const CMD_BEEP_SOUND: u8 = 89; +pub const CMD_ENCODERS_CALIB_OFFSET_4: u8 = 26; +pub const CMD_ENCODERS_CALIB_FLD_OFFSET_4: u8 = 27; +pub const CMD_CONTROL_CONFIG: u8 = 90; +pub const CMD_CALIB_ORIENT_CORR: u8 = 91; +pub const CMD_COGGING_CALIB_INFO: u8 = 92; +pub const CMD_CALIB_COGGING: u8 = 93; +pub const CMD_CALIB_ACC_EXT_REF: u8 = 94; +pub const CMD_PROFILE_SET: u8 = 95; +pub const CMD_CAN_DEVICE_SCAN: u8 = 96; +pub const CMD_CAN_DRV_HARD_PARAMS: u8 = 97; +pub const CMD_CAN_DRV_STATE: u8 = 98; +pub const CMD_CAN_DRV_CALIBRATE: u8 = 99; +pub const CMD_READ_RC_INPUTS: u8 = 100; +pub const CMD_REALTIME_DATA_CAN_DRV: u8 = 101; +pub const CMD_EVENT: u8 = 102; +pub const CMD_READ_PARAMS_EXT3: u8 = 104; +pub const CMD_WRITE_PARAMS_EXT3: u8 = 105; +pub const CMD_EXT_IMU_DEBUG_INFO: u8 = 106; +pub const CMD_SET_DEVICE_ADDR: u8 = 107; +pub const CMD_AUTO_PID2: u8 = 108; +pub const CMD_EXT_IMU_CMD: u8 = 110; +pub const CMD_READ_STATE_VARS: u8 = 111; +pub const CMD_WRITE_STATE_VARS: u8 = 112; +pub const CMD_SERIAL_PROXY: u8 = 113; +pub const CMD_IMU_ADVANCED_CALIB: u8 = 115; +pub const CMD_API_VIRT_CH_HIGH_RES: u8 = 116; +pub const CMD_SET_DEBUG_PORT: u8 = 249; +pub const CMD_MAVLINK_INFO: u8 = 250; +pub const CMD_MAVLINK_DEBUG: u8 = 251; +pub const CMD_DEBUG_VARS_INFO_3: u8 = 253; +pub const CMD_DEBUG_VARS_3: u8 = 254; +pub const CMD_ERROR: u8 = 255; diff --git a/simplebgc/src/commands/mod.rs b/simplebgc/src/commands/mod.rs index 598f0dd..81dcd5d 100644 --- a/simplebgc/src/commands/mod.rs +++ b/simplebgc/src/commands/mod.rs @@ -1,6 +1,6 @@ #[macro_use] pub(crate) mod macros; -pub(crate) mod constants; +pub mod constants; mod board_info; mod cmd_response; @@ -9,6 +9,7 @@ mod get_angles; mod motors_off; mod read_params; mod realtime; +mod raw_message; pub use self::board_info::*; pub use self::cmd_response::*; @@ -17,6 +18,7 @@ pub use self::get_angles::*; pub use self::motors_off::*; pub use self::read_params::*; pub use self::realtime::*; +pub use self::raw_message::*; use crate::{Payload, PayloadParseError, RollPitchYaw}; use bytes::{BufMut, Bytes, BytesMut}; @@ -28,6 +30,7 @@ payload_rpy!(i16, 2); #[derive(Clone, Debug, PartialEq)] pub enum IncomingCommand { + RawMessage(raw_message::RawMessage), CommandConfirm(ConfirmData), CommandError(ErrorData), BoardInfo(BoardInfo), @@ -57,5 +60,6 @@ pub enum OutgoingCommand { RealtimeData3, GetAngles, GetAnglesExt, - Other { id: u8 }, + Other { id: u8 }, //arbitrary message with empty payload? + RawMessage(raw_message::RawMessage), } diff --git a/simplebgc/src/commands/raw_message.rs b/simplebgc/src/commands/raw_message.rs new file mode 100644 index 0000000..08e26f2 --- /dev/null +++ b/simplebgc/src/commands/raw_message.rs @@ -0,0 +1,26 @@ +use crate::{Payload, PayloadParseError}; + +use bytes::Bytes; + +#[derive(Clone, Debug, PartialEq)] +pub struct RawMessage { + pub typ: u8, + pub payload: Bytes, +} + +impl Payload for RawMessage { + ///leaves typ unfilled! + fn from_bytes(b: Bytes) -> Result + where + Self: Sized, + { + Ok(RawMessage{typ: 0, payload: b}) + } + + fn to_bytes(&self) -> Bytes + where + Self: Sized, + { + self.payload.clone() + } +} \ No newline at end of file diff --git a/simplebgc/src/message.rs b/simplebgc/src/message.rs index d69e089..1d6977d 100644 --- a/simplebgc/src/message.rs +++ b/simplebgc/src/message.rs @@ -236,6 +236,7 @@ impl Message for OutgoingCommand { RealtimeData3 => CMD_REALTIME_DATA_3, GetAngles => CMD_GET_ANGLES, GetAnglesExt => CMD_GET_ANGLES, + RawMessage(msg) => msg.typ, _ => unimplemented!(), } } @@ -260,6 +261,7 @@ impl Message for OutgoingCommand { GetAngles => Bytes::default(), GetAnglesExt => Bytes::default(), Other { id: _ } => Bytes::default(), + RawMessage(data) => Payload::to_bytes(data), } } @@ -299,6 +301,7 @@ impl Message for IncomingCommand { IncomingCommand::ReadParams3(_) => CMD_READ_PARAMS_3, IncomingCommand::ReadParamsExt(_) => CMD_READ_PARAMS_EXT, IncomingCommand::RealtimeData3(_) => CMD_REALTIME_DATA_3, + IncomingCommand::RawMessage(msg) => msg.typ, } } @@ -314,6 +317,7 @@ impl Message for IncomingCommand { ReadParams3(params) => Payload::to_bytes(params), ReadParamsExt(params) => Payload::to_bytes(params), RealtimeData3(data) => Payload::to_bytes(data), + RawMessage(msg) => Payload::to_bytes(msg), } } @@ -333,7 +337,7 @@ impl Message for IncomingCommand { CMD_READ_PARAMS_3 => ReadParams3(Payload::from_bytes(bytes)?), CMD_READ_PARAMS_EXT => ReadParamsExt(Payload::from_bytes(bytes)?), CMD_REALTIME_DATA_3 => RealtimeData3(Payload::from_bytes(bytes)?), - _ => return Err(MessageParseError::BadCommandId { id }), + _ => IncomingCommand::RawMessage(crate::RawMessage{typ: id, payload: bytes}), }) } } From 2f8cc83a008c009c30f6132fe3f61cc1b37a0c75 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Fri, 30 Jun 2023 13:43:58 +0200 Subject: [PATCH 5/7] bump version number --- simplebgc/Cargo.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simplebgc/Cargo.toml b/simplebgc/Cargo.toml index 3c5330e..9e1a647 100644 --- a/simplebgc/Cargo.toml +++ b/simplebgc/Cargo.toml @@ -1,6 +1,6 @@ [package] name = "simplebgc" -version = "0.1.0" +version = "0.2.0" authors = ["Ibiyemi Abiodun "] edition = "2018" From 45e1d65ca9fd28ffe2acedc61ec950a029759078 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Fri, 30 Jun 2023 13:44:58 +0200 Subject: [PATCH 6/7] codec: support starting on a running stream --- simplebgc/src/message.rs | 43 +++++++++++++++++++++++++++++++++++----- 1 file changed, 38 insertions(+), 5 deletions(-) diff --git a/simplebgc/src/message.rs b/simplebgc/src/message.rs index 1d6977d..00cb202 100644 --- a/simplebgc/src/message.rs +++ b/simplebgc/src/message.rs @@ -7,8 +7,13 @@ use tokio_util::codec::{Encoder, Decoder}; pub trait SbgcCodec {} +#[derive(Default)] pub struct V1Codec; -pub struct V2Codec; +#[derive(Default)] +pub struct V2Codec{ + in_sync: bool, +} + impl SbgcCodec for V1Codec {} impl SbgcCodec for V2Codec {} @@ -367,17 +372,45 @@ impl Decoder for V2Codec { type Error = MessageParseError; fn decode(&mut self, src: &mut BytesMut) -> Result, Self::Error> { - if src.len() < 6 { + //println!("codec {}", src.remaining()); + if ! self.in_sync { + let mut found = src.len(); + for i in 0..src.len() { + if src[i] == 0x24 { + found = i; + println!("got start"); + break; + } + } + src.advance(found); + } + if src.remaining() < 6 { // not enough data to read length marker + //println!("not enough data to read length marker"); return Ok(None); } - match IncomingCommand::from_bytes(&src[..]) { + match IncomingCommand::from_bytes(src.chunk()) { Ok((m, num_bytes)) => { + //println!("message!"); + self.in_sync = true; src.advance(num_bytes); Ok(Some(m)) }, - Err(MessageParseError::InsufficientData) => Ok(None), - Err(e) => Err(e), + Err(MessageParseError::InsufficientData) => { + //println!("MessageParseError::InsufficientData"); + Ok(None) + }, + Err(e) => { + src.advance(1); //to not get stuck + if self.in_sync {//lost sync, error + println!("lost sync: {e:?}"); + self.in_sync = false; + Err(e) + } else { //just keep on looking for sync + println!("failed to sync {e:?}, keep looking... "); + Ok(None) + } + }, } } } From 440919b95abab39e55f7c09804272cc306c3acf0 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Mon, 3 Jul 2023 17:08:37 +0200 Subject: [PATCH 7/7] add TargetPrecise and MixFollow AxisControlFlags --- simplebgc/src/commands/control.rs | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/simplebgc/src/commands/control.rs b/simplebgc/src/commands/control.rs index 26ffa70..2285bca 100644 --- a/simplebgc/src/commands/control.rs +++ b/simplebgc/src/commands/control.rs @@ -103,6 +103,23 @@ pub enum AxisControlFlags { /// slow motion (like timelapse shooting). /// (frw. ver. 2.60+) HighResSpeed = 1 << 7, + + /// Applicable for: MODE_ANGLE, MODE_ANGLE_SHORTEST, + ///MODE_ANGLE_REL_FRAME + ///If this flag is set, the speed is not decreased in a + ///vicinity of target. It allows to get more predictive speed + ///profile for the motion trajectory. If not set, actual speed + ///is decreased near target to smooth over the jerks when + ///distance to target is small and target is updated + ///frequently by small steps + TargetPrecise = 1 << 5, + + ///If this flag is set, the follow mode is not overridden, but + ///is mixed with the commanded motion, like it happens + ///for the regular RC control in SPEED or ANGLE mode. + ///If this flag is not set, the commanded motion + ///completely overrides the follow control for this axis. + MixFollow = 1 << 4, } impl FromPrimitive for AxisControlState {