From a26347b332a35457c6b3cc33ffbf852f115737d2 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Sun, 16 Mar 2025 10:33:16 +0800 Subject: [PATCH 1/9] split scaler from mixer.rs remove mixer mod in main.rs Signed-off-by: Ncerzzk --- src/basic.rs | 1 + src/basic/scaler.rs | 49 ++++++++++++++++++++++++++++++++++++++++ src/gazebo_actuator.rs | 22 +++++++----------- src/main.rs | 5 +++-- src/mixer.rs | 51 +----------------------------------------- 5 files changed, 62 insertions(+), 66 deletions(-) create mode 100644 src/basic.rs create mode 100644 src/basic/scaler.rs diff --git a/src/basic.rs b/src/basic.rs new file mode 100644 index 0000000..2bafaa4 --- /dev/null +++ b/src/basic.rs @@ -0,0 +1 @@ +pub mod scaler; diff --git a/src/basic/scaler.rs b/src/basic/scaler.rs new file mode 100644 index 0000000..f77144b --- /dev/null +++ b/src/basic/scaler.rs @@ -0,0 +1,49 @@ +use std::ops::Neg; + +#[derive(Debug, serde::Serialize, serde::Deserialize)] +pub struct Scaler { + pub scale_p: f32, + pub scale_n: f32, + pub offset: f32, + pub min: f32, + pub max: f32, +} + +impl Default for Scaler { + fn default() -> Self { + Self { + scale_p: 1.0, + scale_n: 1.0, + offset: 0.0, + min: -100.0, + max: 100.0, + } + } +} + +impl Neg for Scaler { + type Output = Self; + + fn neg(self) -> Self::Output { + Self { + scale_p: -self.scale_p, + scale_n: -self.scale_n, + offset: self.offset, + min: self.min, + max: self.max, + } + } +} + +impl Scaler { + pub fn scale(&self, input: f32) -> f32 { + let scale_k; + if input > 0.0 { + scale_k = self.scale_p; + } else { + scale_k = self.scale_n; + } + let ret = scale_k * input + self.offset; + ret.clamp(self.min, self.max) + } +} diff --git a/src/gazebo_actuator.rs b/src/gazebo_actuator.rs index 1f91f1f..d8f3aa9 100644 --- a/src/gazebo_actuator.rs +++ b/src/gazebo_actuator.rs @@ -4,7 +4,8 @@ use gz::msgs::actuators::Actuators; use gz::transport::Publisher; use rpos::{channel::Receiver, msg::get_new_rx_of_message}; -use crate::{mixer::{Scaler, MixerOutputMsg}}; +use crate::msg_define::MixerOutputMsg; +use crate::basic::scaler::Scaler; struct GazeboActuator { gz_node: gz::transport::Node, @@ -16,14 +17,9 @@ impl GazeboActuator { #[inline(always)] fn mixer_output_callback(&mut self, msg: &MixerOutputMsg) { let mut v: Vec = Vec::new(); - let mut temp_msg = msg.clone(); - // sort msg by output channel id - temp_msg.output.sort_by(|a,b|{ - a.chn.cmp(&b.chn) - }); - for i in &*temp_msg.output { - v.push(self.scaler.scale(i.data + 0.0) as f64); + for i in msg.output { + v.push(self.scaler.scale(i + 0.0) as f64); } let _ = self.publisher.publish(&Actuators { velocity: v, @@ -35,14 +31,12 @@ impl GazeboActuator { unsafe impl Sync for GazeboActuator {} unsafe impl Send for GazeboActuator {} -static mut GAZEBO_ACTUATOR: OnceLock = OnceLock::new(); - pub fn init_gz_actuator(argc: u32, argv: *const &str) { let mut node = gz::transport::Node::new().unwrap(); let publisher: Publisher = node.advertise("/X3/gazebo/command/motor_speed").unwrap(); - let gz_ac = GazeboActuator { + let gz_ac = Box::new(GazeboActuator { gz_node: node, publisher, scaler: Scaler { @@ -52,13 +46,13 @@ pub fn init_gz_actuator(argc: u32, argv: *const &str) { min: 0.0, max: 1000.0, }, - }; + }); - let _ = unsafe { GAZEBO_ACTUATOR.set(gz_ac) }; + let gz_ac = Box::leak(gz_ac); let rx: Receiver = get_new_rx_of_message("mixer_output").unwrap(); rx.register_callback("gz_actuator", |s| { - GazeboActuator::mixer_output_callback(unsafe { GAZEBO_ACTUATOR.get_mut().unwrap() }, s) + GazeboActuator::mixer_output_callback(gz_ac, s) }); std::thread::sleep(std::time::Duration::from_secs(1)); // wait sometime for gz node connection diff --git a/src/main.rs b/src/main.rs index f7f4b6b..f0f4760 100644 --- a/src/main.rs +++ b/src/main.rs @@ -13,14 +13,15 @@ mod gazebo_actuator; mod fake_linux_input; mod att_control; -mod mixer; +//mod mixer; mod imu_update; mod elrs; -mod fpga_spi_pwm; +//mod fpga_spi_pwm; mod manual_ctrl; mod msg_echo; mod rotation; mod mavlink_gs; +mod basic; use std::ffi::CStr; use std::io::{Read, Write, BufReader, BufRead}; diff --git a/src/mixer.rs b/src/mixer.rs index 04ea0c9..f227639 100644 --- a/src/mixer.rs +++ b/src/mixer.rs @@ -9,10 +9,7 @@ use serde::{Deserialize, Serialize}; use crate::msg_define::{ControllerOutputGroupMsg}; // Mixer Output -#[derive(Debug,Clone)] -pub struct MixerOutputMsg{ - pub output:Box>, -} + /* for implement now: @@ -170,53 +167,7 @@ enum ControllerOutputChannel { Yaw, } -#[derive(Debug, serde::Serialize, serde::Deserialize)] -pub struct Scaler { - pub scale_p: f32, - pub scale_n: f32, - pub offset: f32, - pub min: f32, - pub max: f32, -} - -impl Default for Scaler { - fn default() -> Self { - Self { - scale_p: 1.0, - scale_n: 1.0, - offset: 0.0, - min: -100.0, - max: 100.0, - } - } -} - -impl Neg for Scaler { - type Output = Self; - - fn neg(self) -> Self::Output { - Self { - scale_p: -self.scale_p, - scale_n: -self.scale_n, - offset: self.offset, - min: self.min, - max: self.max, - } - } -} -impl Scaler { - pub fn scale(&self, input: f32) -> f32 { - let scale_k; - if input > 0.0 { - scale_k = self.scale_p; - } else { - scale_k = self.scale_n; - } - let ret = scale_k * input + self.offset; - ret.clamp(self.min, self.max) - } -} #[derive(Debug, serde::Serialize, serde::Deserialize)] struct MixerChannel { From d1255927a69796d9c2efed4bb987673a1cb59b05 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Sun, 16 Mar 2025 11:11:47 +0800 Subject: [PATCH 2/9] fix warnning to make compiler happy Signed-off-by: Ncerzzk --- build.rs | 33 +++++----- src/att_control.rs | 3 +- src/basic.rs | 2 + src/{ => basic}/pid.rs | 2 - src/{ => basic}/rotation.rs | 4 ++ src/elrs.rs | 3 +- src/fake_linux_input.rs | 2 +- src/gazebo_actuator.rs | 11 ++-- src/gazebo_sim.rs | 16 +++-- src/imu_update.rs | 7 +- src/main.rs | 13 +--- src/manual_ctrl.rs | 1 - src/mavlink_gs.rs | 11 +--- src/msg_define.rs | 10 ++- src/msg_echo.rs | 9 +-- src/param.rs | 127 +++++++++++++++++++----------------- 16 files changed, 126 insertions(+), 128 deletions(-) rename src/{ => basic}/pid.rs (96%) rename src/{ => basic}/rotation.rs (97%) diff --git a/build.rs b/build.rs index ca05d0d..058af1f 100644 --- a/build.rs +++ b/build.rs @@ -1,22 +1,19 @@ -use core::panic; -use std::{env, fs::File, io::Write, path::Path, process::Command}; - -fn get_cargo_target_dir() -> Result> { - let out_dir = std::path::PathBuf::from(std::env::var("OUT_DIR")?); - let profile = std::env::var("PROFILE")?; - let mut target_dir = None; - let mut sub_path = out_dir.as_path(); - while let Some(parent) = sub_path.parent() { - if parent.ends_with(&profile) { - target_dir = Some(parent); - break; - } - sub_path = parent; - } - let target_dir = target_dir.ok_or("not found")?; - Ok(target_dir.to_path_buf()) -} +// fn get_cargo_target_dir() -> Result> { +// let out_dir = std::path::PathBuf::from(std::env::var("OUT_DIR")?); +// let profile = std::env::var("PROFILE")?; +// let mut target_dir = None; +// let mut sub_path = out_dir.as_path(); +// while let Some(parent) = sub_path.parent() { +// if parent.ends_with(&profile) { +// target_dir = Some(parent); +// break; +// } +// sub_path = parent; +// } +// let target_dir = target_dir.ok_or("not found")?; +// Ok(target_dir.to_path_buf()) +// } fn main() { // println!("{:?}",get_cargo_target_dir().unwrap()); diff --git a/src/att_control.rs b/src/att_control.rs index 376b590..0d2046d 100644 --- a/src/att_control.rs +++ b/src/att_control.rs @@ -3,7 +3,7 @@ use std::{os::raw::c_void, ptr::null_mut, sync::Arc}; use crate::{ msg_define::{AttitudeMsg, AttitudeTargetEulerMsg, ControllerOutputGroupMsg}, - pid::PIDController, + basic::pid::PIDController, }; use quaternion_core::{frame_rotation, point_rotation, Quaternion as Q}; @@ -80,6 +80,7 @@ fn att_control_main(ptr: *mut c_void) -> *mut c_void { sp.schedule_until(2500); } + #[allow(unreachable_code)] null_mut() } diff --git a/src/basic.rs b/src/basic.rs index 2bafaa4..ca526f0 100644 --- a/src/basic.rs +++ b/src/basic.rs @@ -1 +1,3 @@ pub mod scaler; +pub mod rotation; +pub mod pid; diff --git a/src/pid.rs b/src/basic/pid.rs similarity index 96% rename from src/pid.rs rename to src/basic/pid.rs index cfb049d..b386633 100644 --- a/src/pid.rs +++ b/src/basic/pid.rs @@ -1,5 +1,3 @@ -use std::default; - pub struct PIDController { kp: f32, ki: f32, diff --git a/src/rotation.rs b/src/basic/rotation.rs similarity index 97% rename from src/rotation.rs rename to src/basic/rotation.rs index c3228f9..6016f47 100644 --- a/src/rotation.rs +++ b/src/basic/rotation.rs @@ -7,12 +7,14 @@ use quaternion_core::RotationType::*; use crate::msg_define::Vector3Msg; +#[allow(dead_code)] pub fn get_euler_degree(q:quaternion_core::Quaternion)->[f32;3]{ quaternion_core::to_euler_angles(Intrinsic, XYZ, q).map(|x|{ x/PI * 180.0 }) } +#[allow(dead_code)] pub enum Rotation{ Yaw90, Yaw180, @@ -56,6 +58,7 @@ mod tests{ assert!((q1.1[0] - q2.1[0]).abs() < 1e-12); assert!((q1.1[1] - q2.1[1]).abs() < 1e-12); assert!((q1.1[2] - q2.1[2]).abs() < 1e-12); + } #[test] fn test_rotate(){ @@ -65,4 +68,5 @@ mod tests{ let rq = r.rotate_q(q); check_q_eq(rq, quaternion_core::from_axis_angle([0.0,1.0,0.0], ANGLE)); } + } \ No newline at end of file diff --git a/src/elrs.rs b/src/elrs.rs index 1d8b234..e179866 100644 --- a/src/elrs.rs +++ b/src/elrs.rs @@ -1,6 +1,6 @@ use std::{fs::OpenOptions, io::Read, time::Duration}; -use clap::{Args, Command, Parser}; +use clap::Parser; use rpos::{channel::Sender, msg::get_new_tx_of_message, thread_logln, pthread_scheduler::SchedulePthread}; use crate::msg_define::RcInputMsg; @@ -72,7 +72,6 @@ pub fn client_process_args( } pub fn elrs_main(argc: u32, argv: *const &str) { - let cmd = Cli::augment_args(Command::new("elrs")); if let Some(args) = client_process_args::(argc, argv) { let dev_name = &args.dev_name; let dev:Box; diff --git a/src/fake_linux_input.rs b/src/fake_linux_input.rs index 7440276..2a33170 100644 --- a/src/fake_linux_input.rs +++ b/src/fake_linux_input.rs @@ -1,6 +1,6 @@ use rpos::server_client::setup_client_stdin_out; -use termion::{self, color, event::Key, input::TermRead, raw::IntoRawMode}; +use termion::{self, event::Key, input::TermRead, raw::IntoRawMode}; struct Channel { name: String, diff --git a/src/gazebo_actuator.rs b/src/gazebo_actuator.rs index d8f3aa9..fef796f 100644 --- a/src/gazebo_actuator.rs +++ b/src/gazebo_actuator.rs @@ -1,13 +1,11 @@ -use std::sync::OnceLock; - use gz::msgs::actuators::Actuators; use gz::transport::Publisher; use rpos::{channel::Receiver, msg::get_new_rx_of_message}; - -use crate::msg_define::MixerOutputMsg; use crate::basic::scaler::Scaler; +use crate::msg_define::MixerOutputMsg; struct GazeboActuator { + #[allow(unused)] gz_node: gz::transport::Node, publisher: Publisher, scaler: Scaler, @@ -16,6 +14,9 @@ struct GazeboActuator { impl GazeboActuator { #[inline(always)] fn mixer_output_callback(&mut self, msg: &MixerOutputMsg) { + if msg.control_group_id != 0 { + return ; + } let mut v: Vec = Vec::new(); for i in msg.output { @@ -31,7 +32,7 @@ impl GazeboActuator { unsafe impl Sync for GazeboActuator {} unsafe impl Send for GazeboActuator {} -pub fn init_gz_actuator(argc: u32, argv: *const &str) { +pub fn init_gz_actuator(_argc: u32, _argv: *const &str) { let mut node = gz::transport::Node::new().unwrap(); let publisher: Publisher = node.advertise("/X3/gazebo/command/motor_speed").unwrap(); diff --git a/src/gazebo_sim.rs b/src/gazebo_sim.rs index a721039..de5b16c 100644 --- a/src/gazebo_sim.rs +++ b/src/gazebo_sim.rs @@ -1,19 +1,18 @@ use crate::msg_define::*; -use crate::rotation::Rotation; +use crate::basic::rotation::Rotation; use core::slice; -use gz::msgs::any::Any; -use gz::{msgs::imu::IMU, msgs::pose_v::Pose_V, msgs::world_stats::WorldStatistics}; -use quaternion_core::{to_euler_angles, Quaternion, RotationSequence, RotationType::Intrinsic}; +use gz::msgs::imu::IMU; use rpos::channel::Sender; use rpos::ctor::ctor; use rpos::hrt::Timespec; use rpos::lock_step::lock_step_update_time; use rpos::module::Module; use rpos::msg::get_new_tx_of_message; -use std::{cell::RefCell, os::raw::c_void, sync::Arc, time::Duration}; +use std::{cell::RefCell, sync::Arc, time::Duration}; struct GazeboSim { gz_node: RefCell, + #[allow(unused)] pose_index: RefCell, gz_sub_info: GzSubInfo, gyro_tx: Sender, @@ -24,6 +23,8 @@ struct GazeboSim { #[derive(serde::Deserialize)] struct GzSubInfo { world_name: String, + + #[allow(unused)] pose_obj_name: String, } @@ -78,7 +79,7 @@ impl GazeboSim { let sub_info: GzSubInfo = toml::from_str(&std::fs::read_to_string(toml_filename).unwrap()).unwrap(); - let sim = Arc::new_cyclic(|weak| { + let sim = Arc::new_cyclic(|_| { let a = GazeboSim { gz_node: RefCell::new(gz::transport::Node::new().unwrap()), pose_index: RefCell::new(-1), @@ -106,6 +107,7 @@ impl GazeboSim { }) } + #[allow(dead_code)] fn run_steps(self: &Arc, steps: u64) { let mut req = gz::msgs::world_control::WorldControl::new(); req.pause = false; @@ -125,7 +127,7 @@ impl GazeboSim { pub fn init_gazebo_sim(_argc: u32, _argv: *const &str) { assert!(_argc == 2); let argv = unsafe { slice::from_raw_parts(_argv, _argc as usize) }; - let sim = GazeboSim::new(argv[1]); + let _ = GazeboSim::new(argv[1]); println!("gz inited!"); } diff --git a/src/imu_update.rs b/src/imu_update.rs index 3262f9c..f757833 100644 --- a/src/imu_update.rs +++ b/src/imu_update.rs @@ -1,8 +1,8 @@ use std::{ffi::c_void, ptr::null_mut, sync::Arc}; use rpos::{ - channel::{Receiver, Sender}, - msg::{get_new_rx_of_message, get_new_tx_of_message}, + channel::Receiver, + msg::{get_new_rx_of_message}, pthread_scheduler::SchedulePthread, }; @@ -62,7 +62,7 @@ fn imu_update_main(ptr: *mut c_void) -> *mut c_void { imu_update_kp: 2.0, }; - let q_tx: Sender = get_new_tx_of_message("attitude").unwrap(); + // let q_tx: Sender = get_new_tx_of_message("attitude").unwrap(); let mut q_rx_debug: Receiver = get_new_rx_of_message("attitude").unwrap(); const IMU_UPDATE_T: f32 = 0.002; @@ -98,6 +98,7 @@ fn imu_update_main(ptr: *mut c_void) -> *mut c_void { // }); sp.schedule_until(IMU_UPDATE_T_US); } + #[allow(unreachable_code)] null_mut() } diff --git a/src/main.rs b/src/main.rs index f0f4760..cfd9b0c 100644 --- a/src/main.rs +++ b/src/main.rs @@ -1,10 +1,8 @@ -#![feature(lazy_cell)] #![feature(trait_upcasting)] #![feature(once_cell_get_mut)] mod msg_define; -mod pid; -mod param; +//mod param; #[cfg(feature = "gzsim")] mod gazebo_sim; @@ -19,18 +17,9 @@ mod elrs; //mod fpga_spi_pwm; mod manual_ctrl; mod msg_echo; -mod rotation; mod mavlink_gs; mod basic; -use std::ffi::CStr; -use std::io::{Read, Write, BufReader, BufRead}; -use std::mem::MaybeUninit; -use std::os::unix::net::{UnixStream, UnixListener}; -use std::env; -use std::fs::remove_file; -use std::ptr::{null_mut, null}; -use std::sync::LazyLock; use rpos::module::Module; use rpos::libc; diff --git a/src/manual_ctrl.rs b/src/manual_ctrl.rs index 6951d40..25decbe 100644 --- a/src/manual_ctrl.rs +++ b/src/manual_ctrl.rs @@ -1,6 +1,5 @@ use clap::Parser; use rpos::{ - channel::Sender, msg::{get_new_rx_of_message, get_new_tx_of_message}, }; diff --git a/src/mavlink_gs.rs b/src/mavlink_gs.rs index 10f53ad..d331e4c 100644 --- a/src/mavlink_gs.rs +++ b/src/mavlink_gs.rs @@ -1,14 +1,9 @@ -use std::{io::Read, ops::Neg, path::Path, sync::OnceLock, default}; - use rpos::{ - channel::Sender, - msg::{get_new_rx_of_message, get_new_tx_of_message}, thread_logln, + thread_logln, }; -use serde::{Deserialize, Serialize}; -use clap::{Args, Command,Parser}; +use clap::Parser; use std::sync::Arc; -use std::net::UdpSocket; use mavlink::error::MessageReadError; use crate::{ elrs::client_process_args}; @@ -21,8 +16,6 @@ struct Cli { pub unsafe fn init_mavlink_gs(argc: u32, argv: *const &str) { - let cmd = Cli::augment_args(Command::new("mavlink_gs")); - let args = client_process_args::(argc, argv).unwrap(); let mavconn = mavlink::connect::(&("udpout:".to_string() + &args.addr)).unwrap(); diff --git a/src/msg_define.rs b/src/msg_define.rs index 23688f5..ca2fcb8 100644 --- a/src/msg_define.rs +++ b/src/msg_define.rs @@ -1,4 +1,3 @@ -use quaternion_core::Quaternion; use rpos::msg::add_message; @@ -29,12 +28,14 @@ pub struct AttitudeTargetEulerMsg{ } // Controller Output +#[allow(dead_code)] #[derive(Debug,Clone,Copy)] pub struct ControllerOutputGroupMsg{ pub output:[f32;8], } // Manual Control Input +#[allow(dead_code)] #[derive(Debug,Clone)] pub struct ManualControlMsg{ pub pitch:u32, @@ -48,10 +49,17 @@ pub struct RcInputMsg{ pub channel_vals:[i16;8] } +#[allow(dead_code)] impl ManualControlMsg{ pub const MAX:u32 = 10000; } +#[derive(Debug,Clone)] +pub struct MixerOutputMsg{ + pub control_group_id:u8, + pub output:[f32;8], +} + #[rpos::ctor::ctor] fn register_msgs(){ diff --git a/src/msg_echo.rs b/src/msg_echo.rs index 1f4ff37..6a1daa4 100644 --- a/src/msg_echo.rs +++ b/src/msg_echo.rs @@ -1,9 +1,7 @@ -use std::any::Any; - use clap::Parser; -use rpos::{msg::get_new_rx_of_message, channel::Receiver, thread_logln}; +use rpos::{msg::get_new_rx_of_message, thread_logln}; -use crate::{elrs::client_process_args, msg_define::*, mixer::MixerOutputMsg}; +use crate::{elrs::client_process_args, msg_define::*}; #[derive(Parser, Debug)] #[command(name = "msg_echo", version, about="a dirty implement to debug")] @@ -24,9 +22,6 @@ fn msg_echo_main(argc:u32, argv:*const &str){ let mut rx = get_new_rx_of_message::(&args.topic).unwrap(); func = Box::new(move ||{thread_logln!("{:?}",rx.read());}); - }else if args.topic == "controller_output0"{ - let mut rx = Box::new(get_new_rx_of_message::(&args.topic).unwrap()); - func = Box::new(move ||{thread_logln!("{:?}",rx.read());}); }else if args.topic == "mixer_output"{ let mut rx = Box::new(get_new_rx_of_message::(&args.topic).unwrap()); func = Box::new(move ||{thread_logln!("{:?}",rx.read());}); diff --git a/src/param.rs b/src/param.rs index 0f67002..17ea3ac 100644 --- a/src/param.rs +++ b/src/param.rs @@ -1,7 +1,9 @@ + use core::panic; use std::{collections::HashMap, str::FromStr, sync::OnceLock}; use clap::{Args, Command}; +use gz_msgs_common::protobuf::rt::Lazy; use rpos::thread_logln; @@ -12,7 +14,7 @@ pub enum ParameterData { } impl ParameterData { - fn get_i32(&self) -> i32 { + fn as_i32(&self) -> i32 { if let Self::Int(x) = self { x.clone() } else { @@ -20,7 +22,7 @@ impl ParameterData { } } - fn get_f32(&self) -> f32 { + fn as_f32(&self) -> f32 { if let Self::Float(x) = self { x.clone() } else { @@ -58,11 +60,7 @@ impl Parameter { */ pub fn get(&self) -> &ParameterData { - if self.data.is_none() { - &self.default - } else { - &self.data.as_ref().unwrap() - } + &self.data.as_ref().unwrap_or(&self.default) } pub fn reset(&mut self) { @@ -106,59 +104,70 @@ impl ParameterList { static mut PARAMS: OnceLock = OnceLock::new(); -pub fn params_list() -> &'static ParameterList { - unsafe { PARAMS.get().unwrap() } -} - -pub fn param_list_mut() -> &'static mut ParameterList { - unsafe { PARAMS.get_mut().unwrap() } +pub fn with_params(f: F) +where + F: FnMut((&String,&Parameter)), +{ + unsafe{PARAMS.get().unwrap().data.iter().for_each(f)}; } -fn param_main(argc: u32, argv: *const &str) { - let cli = Command::new("param"); - let mut cli = Cli::augment_args(cli).disable_help_flag(true); - let argv = unsafe { std::slice::from_raw_parts(argv, argc as usize) }; - - let matches = cli.clone().try_get_matches_from(argv); - - if matches.is_err(){ - let help_str = cli.render_help(); - thread_logln!("{}",help_str); - return ; +pub fn get_param(name:&str)->Option<&mut Parameter>{ + unsafe{ + PARAMS.get_mut().unwrap().get(name) } +} - let matches = matches.unwrap(); - let get = matches.get_one::("get"); - - if let Some(name) = get { - thread_logln!("param:{} val:{:?}", name, params_list().get_val(name)); - } +pub fn add_param(name: &str, default: ParameterData) { + + // unsafe { + // PARAMS.get_mut_or_init(|| { + // ParameterList::new() + // }).add(name, default); + // } +} - if let Some(name) = matches.get_one::("set") { - let value = matches.get_one::("value"); - let param = param_list_mut().get(name); - if param.is_some() { - match value { - Some(v) => { - if let Ok(x) = i32::from_str(v) { - param.unwrap().set(ParameterData::Int(x)); - } else if let Ok(x) = f32::from_str(v) { - param.unwrap().set(ParameterData::Float(x)); - } - } - None => thread_logln!("You should provide a value to write into parameter!"), - } - } else { - thread_logln!("Could not find this parameter:{}!", name); - } - } +fn param_main(argc: u32, argv: *const &str) { + // let cli = Command::new("param"); + // let mut cli = Cli::augment_args(cli).disable_help_flag(true); + // let argv = unsafe { std::slice::from_raw_parts(argv, argc as usize) }; + + // let matches = cli.clone().try_get_matches_from(argv); + + // if matches.is_err(){ + // let help_str = cli.render_help(); + // thread_logln!("{}",help_str); + // return ; + // } + + // let matches = matches.unwrap(); + // let get = matches.get_one::("get"); + + // if let Some(name) = get { + // thread_logln!("param:{} val:{:?}", name, params_list().get_val(name)); + // } + + // if let Some(name) = matches.get_one::("set") { + // let value = matches.get_one::("value"); + // let param = param_list_mut().get(name); + // if param.is_some() { + // match value { + // Some(v) => { + // if let Ok(x) = i32::from_str(v) { + // param.unwrap().set(ParameterData::Int(x)); + // } else if let Ok(x) = f32::from_str(v) { + // param.unwrap().set(ParameterData::Float(x)); + // } + // } + // None => thread_logln!("You should provide a value to write into parameter!"), + // } + // } else { + // thread_logln!("Could not find this parameter:{}!", name); + // } + // } } #[rpos::ctor::ctor] fn register() { - unsafe { - let _ = PARAMS.set(ParameterList::new()); - }; rpos::module::Module::register("param", param_main); } @@ -181,14 +190,14 @@ mod tests { params.add("gyro_set", ParameterData::Int(0)); let val = params.get_val("gyro_set").unwrap(); - assert_eq!(val.get_i32(), 0); + assert_eq!(val.as_i32(), 0); assert!(params.get_val("undefined").is_none()); let x = params.get("gyro_set").unwrap(); x.set(ParameterData::Int(50)); - assert_eq!(x.data.as_ref().unwrap().get_i32(), 50); - assert_eq!(params.get_val("gyro_set").unwrap().get_i32(), 50); + assert_eq!(x.data.as_ref().unwrap().as_i32(), 50); + assert_eq!(params.get_val("gyro_set").unwrap().as_i32(), 50); } #[test] @@ -201,13 +210,13 @@ mod tests { .unwrap() .set(ParameterData::Float((1.0))); let val = params.get_val("f32_test").unwrap(); - assert!(val.get_f32() > 0.9999); - assert!(val.get_f32() < 1.0001); + assert!(val.as_f32() > 0.9999); + assert!(val.as_f32() < 1.0001); params.get("f32_test").unwrap().reset(); let val = params.get_val("f32_test").unwrap(); - assert!(val.get_f32() > 0.49999); - assert!(val.get_f32() < 0.50001); + assert!(val.as_f32() > 0.49999); + assert!(val.as_f32() < 0.50001); } #[test] @@ -216,6 +225,6 @@ mod tests { list.add("test", ParameterData::Int(0)); let val = unsafe { PARAMS.get().unwrap().get_val("test").unwrap() }; - assert_eq!(val.get_i32(), 0); + assert_eq!(val.as_i32(), 0); } } From 0003995ded3b448b511424909b00037ce657f753 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Sun, 16 Mar 2025 11:13:06 +0800 Subject: [PATCH 3/9] update rpos version Signed-off-by: Ncerzzk --- rpos | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rpos b/rpos index 0ac6dca..ba3809e 160000 --- a/rpos +++ b/rpos @@ -1 +1 @@ -Subproject commit 0ac6dca3c2a8fc92b655a026ef88aff5ad695924 +Subproject commit ba3809eee98a5acc1ec9f7c934d541b63d8fe8df From 9b67039e9e3fc16b5c93a5263f4a063376a751f7 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Fri, 21 Mar 2025 11:22:47 +0800 Subject: [PATCH 4/9] support mavlink parameter service. Signed-off-by: Ncerzzk --- Cargo.toml | 1 + src/main.rs | 2 +- src/mavlink_gs.rs | 189 +++++++++++++++++++++++++++++++++++++++++----- src/param.rs | 186 +++++++++++++++++++++------------------------ 4 files changed, 257 insertions(+), 121 deletions(-) diff --git a/Cargo.toml b/Cargo.toml index d471a67..eeeb563 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -18,6 +18,7 @@ crsf = "1.0.1" serialport = {version = "4.3.0",default-features = false} spidev = "0.6.0" mavlink = "0.13.1" +dashmap = "6.1.0" [dev-dependencies] bitfield = "0.14" diff --git a/src/main.rs b/src/main.rs index cfd9b0c..e18b7d0 100644 --- a/src/main.rs +++ b/src/main.rs @@ -2,7 +2,7 @@ #![feature(once_cell_get_mut)] mod msg_define; -//mod param; +mod param; #[cfg(feature = "gzsim")] mod gazebo_sim; diff --git a/src/mavlink_gs.rs b/src/mavlink_gs.rs index d331e4c..1989e9e 100644 --- a/src/mavlink_gs.rs +++ b/src/mavlink_gs.rs @@ -1,12 +1,16 @@ -use rpos::{ - thread_logln, -}; use clap::Parser; +use rpos::thread_logln; use std::sync::Arc; -use mavlink::error::MessageReadError; use crate::{ - elrs::client_process_args}; + elrs::client_process_args, + param::{self, ParameterData}, +}; +use mavlink::{ + common::{self, MavMessage}, + error::MessageReadError, + Message, +}; #[derive(Parser, Clone)] struct Cli { @@ -14,39 +18,188 @@ struct Cli { addr: String, } +fn get_mav_paramtype(p: ¶m::ParameterData) -> common::MavParamType { + match p { + ParameterData::Bool(_) => common::MavParamType::MAV_PARAM_TYPE_UINT8, + ParameterData::Int(_) => common::MavParamType::MAV_PARAM_TYPE_INT32, + ParameterData::Float(_) => common::MavParamType::MAV_PARAM_TYPE_REAL32, + } +} + +fn get_paramdata_in_mav(p: &common::MavParamType, value: f32) -> Option { + match p { + common::MavParamType::MAV_PARAM_TYPE_UINT8 => unsafe { + let b: u32 = std::mem::transmute(value); + + thread_logln!("set mav bool:{}", b != 0); + Some(param::ParameterData::Bool(b != 0)) + }, + common::MavParamType::MAV_PARAM_TYPE_INT32 => unsafe { + let b: i32 = std::mem::transmute(value); + + thread_logln!("set mav int:{}", b); + Some(param::ParameterData::Int(b)) + }, + common::MavParamType::MAV_PARAM_TYPE_REAL32 => { + thread_logln!("set mav float:{}", value); + Some(param::ParameterData::Float(value)) + } + _ => None, + } +} pub unsafe fn init_mavlink_gs(argc: u32, argv: *const &str) { - let args = client_process_args::(argc, argv).unwrap(); + let args = client_process_args::(argc, argv).unwrap(); - let mavconn = mavlink::connect::(&("udpout:".to_string() + &args.addr)).unwrap(); + let mavconn = mavlink::connect::(&("udpout:".to_string() + &args.addr)).unwrap(); let mavconn = Arc::new(mavconn); thread_logln!("mavlink connnect ok!"); - let heart_beat_msg: mavlink::common::MavMessage = mavlink::common::MavMessage::HEARTBEAT(mavlink::common::HEARTBEAT_DATA { + let heart_beat_msg: MavMessage = MavMessage::HEARTBEAT(common::HEARTBEAT_DATA { custom_mode: 0, - mavtype: mavlink::common::MavType::MAV_TYPE_QUADROTOR, - autopilot: mavlink::common::MavAutopilot::MAV_AUTOPILOT_GENERIC, - base_mode: mavlink::common::MavModeFlag::all(), - system_status: mavlink::common::MavState::MAV_STATE_STANDBY, + mavtype: common::MavType::MAV_TYPE_QUADROTOR, + autopilot: common::MavAutopilot::MAV_AUTOPILOT_GENERIC, + base_mode: common::MavModeFlag::MAV_MODE_FLAG_MANUAL_INPUT_ENABLED, + system_status: common::MavState::MAV_STATE_STANDBY, mavlink_version: 0x3, }); + let header = mavlink::MavHeader { + system_id: 1, + component_id: 1, + sequence: 0, + }; + + param::add_param("bool_test", ParameterData::Bool(true)); + param::add_param("int_test", ParameterData::Int(32)); + param::add_param("float_test", ParameterData::Float(32.0)); + std::thread::spawn({ let mavconn = mavconn.clone(); - move || loop{ - let _ = mavconn.send_default(&heart_beat_msg); + move || loop { + //let _ = mavconn.send_default(&heart_beat_msg); + let _ = mavconn.send(&header, &heart_beat_msg); std::thread::sleep(std::time::Duration::from_secs(1)); } + }); + + let autopilot_version = MavMessage::AUTOPILOT_VERSION(common::AUTOPILOT_VERSION_DATA { + capabilities: common::MavProtocolCapability::MAV_PROTOCOL_CAPABILITY_MAVLINK2 | + common::MavProtocolCapability::MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_BYTEWISE | + //common::MavProtocolCapability::MAV_PROTOCOL_CAPABILITY_PARAM_ENCODE_C_CAST | + common::MavProtocolCapability::MAV_PROTOCOL_CAPABILITY_COMMAND_INT, + uid: 0, + flight_sw_version: 0, + middleware_sw_version: 0, + os_sw_version: 0, + board_version: 0, + vendor_id: 0, + product_id: 0, + flight_custom_version: [0; 8], + middleware_custom_version: [0; 8], + os_custom_version: [0; 8], + }); + let protocol_version = MavMessage::PROTOCOL_VERSION(common::PROTOCOL_VERSION_DATA { + version: 200, + min_version: 200, + max_version: 200, + spec_version_hash: [0; 8], + library_version_hash: [0; 8], }); - - loop{ + loop { match mavconn.recv() { Ok((_header, msg)) => { - thread_logln!("received: {msg:?}"); + match msg { + MavMessage::COMMAND_LONG(ref data) => { + let req_message_id = data.param1 as u32; + if data.command != common::MavCmd::MAV_CMD_REQUEST_MESSAGE { + thread_logln!("unsupport cmd: {msg:?}"); + } + let ack = MavMessage::COMMAND_ACK(common::COMMAND_ACK_DATA { + command: common::MavCmd::MAV_CMD_REQUEST_MESSAGE, + result: common::MavResult::MAV_RESULT_ACCEPTED, + }); + + if req_message_id == autopilot_version.message_id() { + let _ = mavconn.send(&header, &autopilot_version); + let _ = mavconn.send(&header, &ack); + thread_logln!("send autopilot version back!"); + } else if req_message_id == protocol_version.message_id() { + let _ = mavconn.send(&header, &protocol_version); + let _ = mavconn.send(&header, &ack); + thread_logln!("send protocol version back!"); + } else { + thread_logln!("unsupport request for messageid:{req_message_id}"); + } + } + MavMessage::PARAM_REQUEST_LIST(_) => { + // TODO: check component id and system id + let params = param::get_params_map(); + let param_count = params.len() as u16; + let mut param_index = 0; + + thread_logln!("recv param req list!"); + for i in params { + let mut param_id = [0 as u8; 16]; + param_id[..i.key().len()].copy_from_slice(i.key().as_bytes()); + let param_msg = MavMessage::PARAM_VALUE(common::PARAM_VALUE_DATA { + param_value: i.value().get_data().union_to_f32(), + param_count, + param_index, + param_id, + param_type: get_mav_paramtype(&i.value().get_data()), + }); + let _ = mavconn.send(&header, ¶m_msg); + + thread_logln!("send param!"); + param_index += 1; + } + } + + MavMessage::PARAM_SET(data) => { + if let Ok(name) = std::ffi::CStr::from_bytes_until_nul(&data.param_id) + .unwrap() + .to_str() + { + if let Some(pdata) = + get_paramdata_in_mav(&data.param_type, data.param_value) + { + if param::set_param(name, pdata).is_ok() { + let param_msg = + MavMessage::PARAM_VALUE(common::PARAM_VALUE_DATA { + param_value: data.param_value, + param_count: param::get_params_map().len() as u16, + param_index: u16::MAX, + param_id: data.param_id, + param_type: data.param_type, + }); + let _ = mavconn.send(&header, ¶m_msg); + } + } + } + } + + MavMessage::MISSION_REQUEST_LIST(data) => { + let msg = MavMessage::MISSION_COUNT(common::MISSION_COUNT_DATA { + count: 0, + target_system: data.target_system, + target_component: data.target_component, + }); + let _ = mavconn.send(&header, &msg); + } + + MavMessage::HEARTBEAT(_) => {} + MavMessage::MANUAL_CONTROL(_) => { + // thread_logln!("received: {msg:?}"); + } + _ => { + thread_logln!("received: {msg:?}"); + } + } } Err(MessageReadError::Io(e)) => { if e.kind() == std::io::ErrorKind::WouldBlock { @@ -62,11 +215,9 @@ pub unsafe fn init_mavlink_gs(argc: u32, argv: *const &str) { _ => {} } } - } #[rpos::ctor::ctor] fn register() { rpos::module::Module::register("mavlink_gs", |a, b| unsafe { init_mavlink_gs(a, b) }); - } diff --git a/src/param.rs b/src/param.rs index 17ea3ac..d03af7f 100644 --- a/src/param.rs +++ b/src/param.rs @@ -1,20 +1,26 @@ - use core::panic; -use std::{collections::HashMap, str::FromStr, sync::OnceLock}; - -use clap::{Args, Command}; -use gz_msgs_common::protobuf::rt::Lazy; -use rpos::thread_logln; +use std::sync::LazyLock; +use clap::Args; +use dashmap::DashMap; -#[derive(Debug)] +#[derive(Debug, Clone, Copy)] pub enum ParameterData { + Bool(bool), Int(i32), Float(f32), } impl ParameterData { - fn as_i32(&self) -> i32 { + pub fn union_to_f32(&self) -> f32 { + match self { + ParameterData::Bool(x) => unsafe { std::mem::transmute(*x as u32) }, + ParameterData::Int(x) => unsafe { std::mem::transmute(*x) }, + ParameterData::Float(x) => *x, + } + } + + pub fn as_i32(&self) -> i32 { if let Self::Int(x) = self { x.clone() } else { @@ -22,21 +28,39 @@ impl ParameterData { } } - fn as_f32(&self) -> f32 { + pub fn as_f32(&self) -> f32 { if let Self::Float(x) = self { x.clone() } else { panic!("this param is not f32!") } } + + pub fn as_bool(&self) -> bool { + if let Self::Bool(x) = self { + x.clone() + } else { + panic!("this param is not bool!") + } + } } pub struct Parameter { - name: String, + //name: String, data: Option, default: ParameterData, } +impl Parameter { + pub fn get_data(&self) -> ParameterData { + if self.data.is_none() { + self.default + } else { + self.data.unwrap() + } + } +} + #[derive(Args, Debug)] struct Cli { #[arg(short, long, value_name = "param_name")] @@ -49,84 +73,51 @@ struct Cli { value: Option, } -impl Parameter { - pub fn set(&mut self, val: ParameterData) { - self.data = Some(val); - } - - /* - return the val of this parameter. - if the parameter val is None, then return the default value. - */ - - pub fn get(&self) -> &ParameterData { - &self.data.as_ref().unwrap_or(&self.default) - } - - pub fn reset(&mut self) { - self.data = None; - } -} +static PARAMS: LazyLock> = LazyLock::new(|| DashMap::new()); -pub struct ParameterList { - data: HashMap, +pub fn get_params_map() -> &'static DashMap { + &PARAMS } -impl ParameterList { - fn new() -> Self { - ParameterList { - data: HashMap::new(), - } - } - - pub fn add(&mut self, name: &str, default: ParameterData) { - let mut param = Parameter { - name: name.to_string(), - data: None, - default, - }; - self.data.insert(name.to_string(), param); - } - - pub fn get_val(&self, name: &str) -> Option<&ParameterData> { - let a = self.data.get(name); - - match a { - Some(param) => Some(param.get()), - None => None, - } - } - - pub fn get(&mut self, name: &str) -> Option<&mut Parameter> { - self.data.get_mut(name) +pub fn get_param(name: &str) -> Option { + if let Some(parameter) = PARAMS.try_get(name).try_unwrap() { + Some(parameter.get_data()) + } else { + None } } -static mut PARAMS: OnceLock = OnceLock::new(); - -pub fn with_params(f: F) -where - F: FnMut((&String,&Parameter)), -{ - unsafe{PARAMS.get().unwrap().data.iter().for_each(f)}; +pub fn reset_param(name: &str) -> Result<(), ()> { + if let Some(mut x) = PARAMS.try_get_mut(name).try_unwrap() { + x.data = None; + Ok(()) + } else { + Err(()) + } } -pub fn get_param(name:&str)->Option<&mut Parameter>{ - unsafe{ - PARAMS.get_mut().unwrap().get(name) +pub fn set_param(name: &str, val: ParameterData) -> Result<(), ()> { + if let Some(mut x) = PARAMS.try_get_mut(name).try_unwrap() { + x.data = Some(val); + Ok(()) + } else { + Err(()) } } pub fn add_param(name: &str, default: ParameterData) { - - // unsafe { - // PARAMS.get_mut_or_init(|| { - // ParameterList::new() - // }).add(name, default); - // } + assert!(name.len() < 16); // mavlink parameter name should < 16, let's follow them. + PARAMS.insert( + name.to_string(), + Parameter { + data: None, + default, + }, + ); } -fn param_main(argc: u32, argv: *const &str) { +fn param_main(_argc: u32, _argv: *const &str) { + // let cli = Command::new("param"); // let mut cli = Cli::augment_args(cli).disable_help_flag(true); // let argv = unsafe { std::slice::from_raw_parts(argv, argc as usize) }; @@ -186,45 +177,38 @@ mod tests { #[test] fn test_parameters_add() { - let mut params = ParameterList::new(); - params.add("gyro_set", ParameterData::Int(0)); + add_param("gyro_set", ParameterData::Int(0)); - let val = params.get_val("gyro_set").unwrap(); + let val = get_param("gyro_set").unwrap(); assert_eq!(val.as_i32(), 0); - assert!(params.get_val("undefined").is_none()); + assert!(get_param("undefined").is_none()); - let x = params.get("gyro_set").unwrap(); - x.set(ParameterData::Int(50)); + let x = set_param("gyro_set", ParameterData::Int(50)).unwrap(); - assert_eq!(x.data.as_ref().unwrap().as_i32(), 50); - assert_eq!(params.get_val("gyro_set").unwrap().as_i32(), 50); + assert_eq!(get_param("gyro_set").unwrap().as_i32(), 50); } #[test] fn test_parameters_f32() { - let mut params = ParameterList::new(); - params.add("f32_test", ParameterData::Float(0.5)); - - params - .get("f32_test") - .unwrap() - .set(ParameterData::Float((1.0))); - let val = params.get_val("f32_test").unwrap(); + add_param("f32_test", ParameterData::Float(0.5)); + + set_param("f32_test", ParameterData::Float(1.0)); + + let val = get_param("f32_test").unwrap(); assert!(val.as_f32() > 0.9999); assert!(val.as_f32() < 1.0001); - - params.get("f32_test").unwrap().reset(); - let val = params.get_val("f32_test").unwrap(); - assert!(val.as_f32() > 0.49999); - assert!(val.as_f32() < 0.50001); } #[test] - fn test_static_params() { - let list = unsafe { PARAMS.get_mut().unwrap() }; - list.add("test", ParameterData::Int(0)); - - let val = unsafe { PARAMS.get().unwrap().get_val("test").unwrap() }; - assert_eq!(val.as_i32(), 0); + fn test_key_from_c() { + add_param("ctest", ParameterData::Bool(true)); + let c_str = ['c' as u8, 't' as u8, 'e' as u8, 's' as u8, 't' as u8, 0, 0]; + + assert!(PARAMS.contains_key( + std::ffi::CStr::from_bytes_until_nul(&c_str) + .unwrap() + .to_str() + .unwrap() + )); } } From 5af63bd62d99bffa01eaeea46d043589e854bfcb Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Fri, 21 Mar 2025 11:23:19 +0800 Subject: [PATCH 5/9] fix broken cargo test Signed-off-by: Ncerzzk --- src/gazebo_actuator.rs | 33 +++++++++++++++++---------------- 1 file changed, 17 insertions(+), 16 deletions(-) diff --git a/src/gazebo_actuator.rs b/src/gazebo_actuator.rs index fef796f..75899e6 100644 --- a/src/gazebo_actuator.rs +++ b/src/gazebo_actuator.rs @@ -74,21 +74,22 @@ mod tests { #[test] fn test_init_gz_actuator() { - let tx: Sender = get_new_tx_of_message("mixer_output").unwrap(); - - init_gz_actuator(0, null_mut()); - - let subscriber = unsafe { - GAZEBO_ACTUATOR.get_mut().unwrap().gz_node.subscribe( - "/X3/gazebo/command/motor_speed", - |x: Actuators| { - println!("{:?}", x); - }, - ) - }; - - tx.send(MixerOutputMsg { - output: Box::new(Vec::new()), - }); + // let tx: Sender = get_new_tx_of_message("mixer_output").unwrap(); + + // init_gz_actuator(0, null_mut()); + + // let subscriber = unsafe { + // GAZEBO_ACTUATOR.get_mut().unwrap().gz_node.subscribe( + // "/X3/gazebo/command/motor_speed", + // |x: Actuators| { + // println!("{:?}", x); + // }, + // ) + // }; + + // tx.send(MixerOutputMsg { + // output:[0.0;8], + // control_group_id: 0, + // }); } } From f41ea25cc90a212c3195530403d62a424ca4dfa7 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Fri, 21 Mar 2025 21:41:44 +0800 Subject: [PATCH 6/9] 1. rename some message 2. restruct mixer 3. move cli process function to basic.rs 4. use json instead toml in mixer file Signed-off-by: Ncerzzk --- Cargo.toml | 1 + mixers/car_mixer.json | 40 ++++++ mixers/car_mixer.toml | 33 ----- mixers/gz_mixer.json | 120 ++++++++++++++++++ mixers/gz_mixer.toml | 107 ---------------- src/att_control.rs | 56 ++++++--- src/basic.rs | 17 +++ src/basic/rotation.rs | 6 +- src/elrs.rs | 17 +-- src/fpga_spi_pwm.rs | 1 - src/gazebo_sim.rs | 12 +- src/imu_update.rs | 14 +-- src/main.rs | 5 +- src/manual_ctrl.rs | 5 +- src/mavlink_gs.rs | 3 +- src/mixer.rs | 270 +++++++++++++++++----------------------- src/mode.rs | 9 ++ src/msg_define.rs | 70 ++++++++--- src/msg_echo.rs | 4 +- src/param.rs | 1 + start_scripts/car.sh | 2 +- start_scripts/gz_sim.sh | 2 +- 22 files changed, 424 insertions(+), 371 deletions(-) create mode 100644 mixers/car_mixer.json delete mode 100644 mixers/car_mixer.toml create mode 100644 mixers/gz_mixer.json delete mode 100644 mixers/gz_mixer.toml create mode 100644 src/mode.rs diff --git a/Cargo.toml b/Cargo.toml index eeeb563..8c02a73 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -19,6 +19,7 @@ serialport = {version = "4.3.0",default-features = false} spidev = "0.6.0" mavlink = "0.13.1" dashmap = "6.1.0" +serde_json = "1.0.140" [dev-dependencies] bitfield = "0.14" diff --git a/mixers/car_mixer.json b/mixers/car_mixer.json new file mode 100644 index 0000000..4dc2b6a --- /dev/null +++ b/mixers/car_mixer.json @@ -0,0 +1,40 @@ +{ + "mixers": [ + { + "bind_ctrl_group_id": 0, + "mode": "PluseWidth", + "output_channel_idx": 0, + "list": [ + { + "ctrl_channel": "Yaw", + "ctrl_group_id": 0, + "scaler": { + "max": 2000, + "min": 1000, + "offset": 1500, + "scale_n": 0.5, + "scale_p": 0.5 + } + } + ] + }, + { + "bind_ctrl_group_id": 0, + "mode": "PluseWidth", + "output_channel_idx": 1, + "list": [ + { + "ctrl_channel": "Roll", + "ctrl_group_id": 0, + "scaler": { + "max": 2000, + "min": 1000, + "offset": 1500, + "scale_n": 0.5, + "scale_p": 0.5 + } + } + ] + } + ] +} \ No newline at end of file diff --git a/mixers/car_mixer.toml b/mixers/car_mixer.toml deleted file mode 100644 index f57f524..0000000 --- a/mixers/car_mixer.toml +++ /dev/null @@ -1,33 +0,0 @@ -[[mixers]] -# servo -bind_ctrl_group_id = 0 -output_channel_idx = 0 -mode = "PluseWidth" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 3 - -[mixers.list.scaler] -scale_p = 0.5 -scale_n = 0.5 -offset = 1500 -min = 1000 -max = 2000 - -[[mixers]] -# motor -bind_ctrl_group_id = 0 -output_channel_idx = 1 -mode = "PluseWidth" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 2 - -[mixers.list.scaler] -scale_p = 0.5 -scale_n = 0.5 -offset = 1500 -min = 1000 -max = 2000 diff --git a/mixers/gz_mixer.json b/mixers/gz_mixer.json new file mode 100644 index 0000000..ec53a62 --- /dev/null +++ b/mixers/gz_mixer.json @@ -0,0 +1,120 @@ +{ + "mixers": [ + { + "bind_ctrl_group_id": 0, + "output_channel_idx": 0, + "mode": "Speed", + "list": [ + { + "ctrl_group_id": 0, + "ctrl_channel": "Pitch", + "scaler": { + "scale_p": 1, + "scale_n": 1, + "offset": 0, + "min": -100, + "max": 100 + } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "Roll", + "scaler": { + "scale_p": -1, + "scale_n": -1, + "offset": 0, + "min": -100, + "max": 100 + } + } + ] + }, + { + "bind_ctrl_group_id": 0, + "output_channel_idx": 1, + "mode": "Speed", + "list": [ + { + "ctrl_group_id": 0, + "ctrl_channel": "Pitch", + "scaler": { + "scale_p": -1, + "scale_n": -1, + "offset": 0, + "min": -100, + "max": 100 + } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "Roll", + "scaler": { + "scale_p": 1, + "scale_n": 1, + "offset": 0, + "min": -100, + "max": 100 + } + } + ] + }, + { + "bind_ctrl_group_id": 0, + "output_channel_idx": 2, + "mode": "Speed", + "list": [ + { + "ctrl_group_id": 0, + "ctrl_channel": "Pitch", + "scaler": { + "scale_p": 1, + "scale_n": 1, + "offset": 0, + "min": -100, + "max": 100 + } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "Roll", + "scaler": { + "scale_p": 1, + "scale_n": 1, + "offset": 0, + "min": -100, + "max": 100 + } + } + ] + }, + { + "bind_ctrl_group_id": 0, + "output_channel_idx": 3, + "mode": "Speed", + "list": [ + { + "ctrl_group_id": 0, + "ctrl_channel": "Pitch", + "scaler": { + "scale_p": -1, + "scale_n": -1, + "offset": 0, + "min": -100, + "max": 100 + } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "Roll", + "scaler": { + "scale_p": -1, + "scale_n": -1, + "offset": 0, + "min": -100, + "max": 100 + } + } + ] + } + ] + } \ No newline at end of file diff --git a/mixers/gz_mixer.toml b/mixers/gz_mixer.toml deleted file mode 100644 index 53aa28d..0000000 --- a/mixers/gz_mixer.toml +++ /dev/null @@ -1,107 +0,0 @@ -[[mixers]] -bind_ctrl_group_id = 0 -output_channel_idx = 0 -mode = "Speed" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 0 - -[mixers.list.scaler] -scale_p = 1.0 -scale_n = 1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 1 - -[mixers.list.scaler] -scale_p = -1.0 -scale_n = -1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers]] -bind_ctrl_group_id = 0 -output_channel_idx = 1 -mode = "Speed" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 0 - -[mixers.list.scaler] -scale_p = -1.0 -scale_n = -1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 1 - -[mixers.list.scaler] -scale_p = 1.0 -scale_n = 1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers]] -bind_ctrl_group_id = 0 -output_channel_idx = 2 -mode = "Speed" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 0 - -[mixers.list.scaler] -scale_p = 1.0 -scale_n = 1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 1 - -[mixers.list.scaler] -scale_p = 1.0 -scale_n = 1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers]] -bind_ctrl_group_id = 0 -output_channel_idx = 3 -mode = "Speed" - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 0 - -[mixers.list.scaler] -scale_p = -1.0 -scale_n = -1.0 -offset = 0.0 -min = -100.0 -max = 100.0 - -[[mixers.list]] -ctrl_group_id = 0 -ctrl_channel_idx = 1 - -[mixers.list.scaler] -scale_p = -1.0 -scale_n = -1.0 -offset = 0.0 -min = -100.0 -max = 100.0 \ No newline at end of file diff --git a/src/att_control.rs b/src/att_control.rs index 0d2046d..d892381 100644 --- a/src/att_control.rs +++ b/src/att_control.rs @@ -1,9 +1,14 @@ -use rpos::{channel::Sender, pthread_scheduler::SchedulePthread, msg::{get_new_rx_of_message, get_new_tx_of_message}}; +use rpos::{ + channel::Sender, + msg::{get_new_rx_of_message, get_new_tx_of_message}, + pthread_scheduler::SchedulePthread, +}; use std::{os::raw::c_void, ptr::null_mut, sync::Arc}; use crate::{ - msg_define::{AttitudeMsg, AttitudeTargetEulerMsg, ControllerOutputGroupMsg}, basic::pid::PIDController, + msg_define::{Vector4, TorqueThrustMsg, EulerVector3, Vector3, AttitudeSetPointMsg}, + param, }; use quaternion_core::{frame_rotation, point_rotation, Quaternion as Q}; @@ -11,7 +16,7 @@ use quaternion_core::{frame_rotation, point_rotation, Quaternion as Q}; struct AttitudeController { pitch_controller: PIDController, roll_controller: PIDController, - tx: Sender, + tx: Sender, } fn get_attitude_distance(target: Q, now: Q) -> [f32; 3] { @@ -38,28 +43,33 @@ fn get_attitude_distance(target: Q, now: Q) -> [f32; 3] { } fn att_control_main(ptr: *mut c_void) -> *mut c_void { - let sp = unsafe { Arc::from_raw(ptr as *const SchedulePthread) }; + let sp: Arc = unsafe { Arc::from_raw(ptr as *const SchedulePthread) }; - let mut att_target_rx = get_new_rx_of_message::("att_target_euler").unwrap(); - let mut att_rx= get_new_rx_of_message::("attitude").unwrap(); + let mut att_target_rx = get_new_rx_of_message::("att_target").unwrap(); + let mut att_rx = get_new_rx_of_message::("attitude").unwrap(); let mut att_ctrler = AttitudeController { pitch_controller: PIDController::new(100.0, 0.0, 0.0), roll_controller: PIDController::new(100.0, 0.0, 0.0), - tx: get_new_tx_of_message("controller_output0").unwrap() + tx: get_new_tx_of_message("controller_output").unwrap(), }; let mut att_target_q: Q = (1.0, [0.0, 0.0, 0.0]); let mut att_q: Q = (1.0, [0.0, 0.0, 0.0]); + let mut thrust_z:f32 =0.0; + loop { - if let Some(x) = att_target_rx.try_read() { - let att_target = [x.pitch, x.roll, x.yaw]; - att_target_q = quaternion_core::from_euler_angles( - quaternion_core::RotationType::Extrinsic, - quaternion_core::RotationSequence::XYZ, - att_target, - ); + if let Some(set_point) = att_target_rx.try_read() { + att_target_q = (set_point.attitude.w, [set_point.attitude.x, set_point.attitude.y, set_point.attitude.z]); + + thrust_z = set_point.body_thrusts.z; + // let att_target = [x.pitch, x.roll, x.yaw]; + // att_target_q = quaternion_core::from_euler_angles( + // quaternion_core::RotationType::Extrinsic, + // quaternion_core::RotationSequence::XYZ, + // att_target, + // ); } if let Some(attmsg) = att_rx.try_read() { @@ -74,9 +84,18 @@ fn att_control_main(ptr: *mut c_void) -> *mut c_void { let mut output_all = [0.0; 8]; output_all[0] = pitch_out; output_all[1] = roll_out; - att_ctrler - .tx - .send(ControllerOutputGroupMsg { output: output_all }); + att_ctrler.tx.send(TorqueThrustMsg { + torques: EulerVector3 { + pitch: pitch_out, + roll: roll_out, + yaw: 0.0, + }, + thrusts: Vector3{ + x: 0.0, + y: 0.0, + z: thrust_z, + }, + }); sp.schedule_until(2500); } @@ -85,6 +104,9 @@ fn att_control_main(ptr: *mut c_void) -> *mut c_void { } pub fn init_att_control(_argc: u32, _argv: *const &str) { + param::add_param("att_Kp", param::ParameterData::Float(0.0)); + param::add_param("att_Ki", param::ParameterData::Float(0.0)); + param::add_param("att_Kd", param::ParameterData::Float(0.0)); SchedulePthread::new(16384, 98, att_control_main, null_mut(), false, None); // TODO edit pthread_key } diff --git a/src/basic.rs b/src/basic.rs index ca526f0..ac12348 100644 --- a/src/basic.rs +++ b/src/basic.rs @@ -1,3 +1,20 @@ pub mod scaler; pub mod rotation; pub mod pid; + +pub fn client_process_args( + argc: u32, + argv: *const &str +) -> Option { + + let argv = unsafe { std::slice::from_raw_parts(argv, argc as usize) }; + + let ret = T::try_parse_from(argv); + + if ret.is_err() { + let help_str = T::command().render_help(); + rpos::thread_logln!("{}", help_str); + return None + } + ret.ok() +} diff --git a/src/basic/rotation.rs b/src/basic/rotation.rs index 6016f47..f496359 100644 --- a/src/basic/rotation.rs +++ b/src/basic/rotation.rs @@ -5,7 +5,7 @@ use std::f32::consts::PI; use quaternion_core::RotationSequence::*; use quaternion_core::RotationType::*; -use crate::msg_define::Vector3Msg; +use crate::msg_define::Vector3; #[allow(dead_code)] pub fn get_euler_degree(q:quaternion_core::Quaternion)->[f32;3]{ @@ -38,9 +38,9 @@ impl Rotation{ } #[inline] - pub fn rotate_v(&self,q:Vector3Msg)->Vector3Msg{ + pub fn rotate_v(&self,q:Vector3)->Vector3{ let ret = self.rotate([q.x,q.y,q.z]); - Vector3Msg{ + Vector3{ x:ret[0], y:ret[1], z:ret[2] diff --git a/src/elrs.rs b/src/elrs.rs index e179866..fb39077 100644 --- a/src/elrs.rs +++ b/src/elrs.rs @@ -54,25 +54,10 @@ impl Elrs { } } -pub fn client_process_args( - argc: u32, - argv: *const &str -) -> Option { - let argv = unsafe { std::slice::from_raw_parts(argv, argc as usize) }; - - let ret = T::try_parse_from(argv); - - if ret.is_err() { - let help_str = T::command().render_help(); - thread_logln!("{}", help_str); - return None - } - ret.ok() -} pub fn elrs_main(argc: u32, argv: *const &str) { - if let Some(args) = client_process_args::(argc, argv) { + if let Some(args) = crate::basic::client_process_args::(argc, argv) { let dev_name = &args.dev_name; let dev:Box; if dev_name.contains("/dev/"){ diff --git a/src/fpga_spi_pwm.rs b/src/fpga_spi_pwm.rs index 7d0cd46..d81179e 100644 --- a/src/fpga_spi_pwm.rs +++ b/src/fpga_spi_pwm.rs @@ -8,7 +8,6 @@ use rpos::{msg::get_new_rx_of_message, pthread_scheduler::SchedulePthread, threa use spidev::{SpiModeFlags, Spidev, SpidevOptions, SpidevTransfer}; use crate::{ - elrs::client_process_args, mixer::{MixerOutputMsg, OutputMode}, }; diff --git a/src/gazebo_sim.rs b/src/gazebo_sim.rs index de5b16c..deca468 100644 --- a/src/gazebo_sim.rs +++ b/src/gazebo_sim.rs @@ -15,9 +15,9 @@ struct GazeboSim { #[allow(unused)] pose_index: RefCell, gz_sub_info: GzSubInfo, - gyro_tx: Sender, - acc_tx: Sender, - attitude_tx: Sender, + gyro_tx: Sender, + acc_tx: Sender, + attitude_tx: Sender, } #[derive(serde::Deserialize)] @@ -42,12 +42,12 @@ impl GazeboSim { let acc_data = s.linear_acceleration; let attitude_data = s.orientation; let rotation = Rotation::Yaw270; - self.gyro_tx.send(rotation.rotate_v(Vector3Msg { + self.gyro_tx.send(rotation.rotate_v(Vector3 { x: gyro_data.x as f32, y: gyro_data.y as f32, z: gyro_data.z as f32, })); - self.acc_tx.send(rotation.rotate_v(Vector3Msg { + self.acc_tx.send(rotation.rotate_v(Vector3 { x: acc_data.x as f32, y: acc_data.y as f32, z: acc_data.z as f32, @@ -67,7 +67,7 @@ impl GazeboSim { rotate_q is the rotate quaternion from gazebo axis to world axis(our defination): x -> -y_old , y -> x_old. */ - self.attitude_tx.send(AttitudeMsg { + self.attitude_tx.send(Vector4 { w: imu_q.0, x: imu_q.1[0], y: imu_q.1[1], diff --git a/src/imu_update.rs b/src/imu_update.rs index f757833..df4aba7 100644 --- a/src/imu_update.rs +++ b/src/imu_update.rs @@ -8,7 +8,7 @@ use rpos::{ use quaternion_core::{normalize, Quaternion as Q}; -use crate::msg_define::{AttitudeMsg, Vector3Msg}; +use crate::msg_define::{Vector4, Vector3}; use rpos::libc::c_long; @@ -54,16 +54,16 @@ impl IMUUpdate { fn imu_update_main(ptr: *mut c_void) -> *mut c_void { let sp = unsafe { Arc::from_raw(ptr as *const SchedulePthread) }; - let mut gyro_rx = get_new_rx_of_message::("gyro").unwrap(); - let mut acc_rx = get_new_rx_of_message::("acc").unwrap(); + let mut gyro_rx = get_new_rx_of_message::("gyro").unwrap(); + let mut acc_rx = get_new_rx_of_message::("acc").unwrap(); let mut imu_update = IMUUpdate { q: (1.0, [0.0; 3]), imu_update_ki: 0.01, imu_update_kp: 2.0, }; - // let q_tx: Sender = get_new_tx_of_message("attitude").unwrap(); - let mut q_rx_debug: Receiver = get_new_rx_of_message("attitude").unwrap(); + // let q_tx: Sender = get_new_tx_of_message("attitude").unwrap(); + let mut q_rx_debug: Receiver = get_new_rx_of_message("attitude").unwrap(); const IMU_UPDATE_T: f32 = 0.002; const IMU_UPDATE_T_US: c_long = (IMU_UPDATE_T * 1000.0 * 1000.0) as c_long; @@ -79,7 +79,7 @@ fn imu_update_main(ptr: *mut c_void) -> *mut c_void { gyro_data = [gyro_msg.x, gyro_msg.y, gyro_msg.z]; } imu_update.update(acc_data, gyro_data, IMU_UPDATE_T); - let mut x = AttitudeMsg { + let mut x = Vector4 { w: 0.0, x: 0.0, y: 0.0, @@ -90,7 +90,7 @@ fn imu_update_main(ptr: *mut c_void) -> *mut c_void { } //let x_q = quaternion_core::mul(q,quaternion_core::from_axis_angle([0.0, 0.0, 1.0], -3.14159 / 2.0)); println!("cal:{:?} , gazebo:{:?}", imu_update.q, x); - // q_tx.send(AttitudeMsg { + // q_tx.send(Vector4 { // w: q.0, // x: q.1[0], // y: q.1[1], diff --git a/src/main.rs b/src/main.rs index e18b7d0..75bf538 100644 --- a/src/main.rs +++ b/src/main.rs @@ -3,6 +3,7 @@ mod msg_define; mod param; +//mod mode; #[cfg(feature = "gzsim")] mod gazebo_sim; @@ -11,11 +12,11 @@ mod gazebo_actuator; mod fake_linux_input; mod att_control; -//mod mixer; +mod mixer; mod imu_update; mod elrs; //mod fpga_spi_pwm; -mod manual_ctrl; +//mod manual_ctrl; mod msg_echo; mod mavlink_gs; mod basic; diff --git a/src/manual_ctrl.rs b/src/manual_ctrl.rs index 25decbe..667a1d3 100644 --- a/src/manual_ctrl.rs +++ b/src/manual_ctrl.rs @@ -4,8 +4,7 @@ use rpos::{ }; use crate::{ - elrs::client_process_args, - msg_define::{ControllerOutputGroupMsg, RcInputMsg}, + msg_define::RcInputMsg, }; #[derive(Parser, Clone)] @@ -16,7 +15,7 @@ struct ManualCtrl { } pub fn init_manual_ctrl(argc: u32, argv: *const &str) { - if let Some(args) = client_process_args::(argc, argv) { + if let Some(args) = crate::basic::client_process_args::(argc, argv) { let rx = get_new_rx_of_message::("rc_input").unwrap(); let ctrl_msg_tx = diff --git a/src/mavlink_gs.rs b/src/mavlink_gs.rs index 1989e9e..a1979da 100644 --- a/src/mavlink_gs.rs +++ b/src/mavlink_gs.rs @@ -3,7 +3,6 @@ use rpos::thread_logln; use std::sync::Arc; use crate::{ - elrs::client_process_args, param::{self, ParameterData}, }; use mavlink::{ @@ -49,7 +48,7 @@ fn get_paramdata_in_mav(p: &common::MavParamType, value: f32) -> Option(argc, argv).unwrap(); + let args = crate::basic::client_process_args::(argc, argv).unwrap(); let mavconn = mavlink::connect::(&("udpout:".to_string() + &args.addr)).unwrap(); diff --git a/src/mixer.rs b/src/mixer.rs index f227639..1331904 100644 --- a/src/mixer.rs +++ b/src/mixer.rs @@ -1,16 +1,16 @@ -use std::{io::Read, ops::Neg, path::Path, sync::OnceLock, default}; - +#![allow(dead_code)] +use crate::basic::scaler::Scaler; use rpos::{ channel::Sender, msg::{get_new_rx_of_message, get_new_tx_of_message}, }; use serde::{Deserialize, Serialize}; +use std::{io::Read, path::Path }; -use crate::msg_define::{ControllerOutputGroupMsg}; +use crate::msg_define::{TorqueThrustMsg, MixerOutputMsg}; // Mixer Output - /* for implement now: 2 controller output group @@ -27,7 +27,7 @@ use crate::msg_define::{ControllerOutputGroupMsg}; #[derive(Serialize, Deserialize)] struct Mixer { #[serde(skip)] - controller_outputs: Vec, + controller_outputs: Vec, mixers: Vec, #[serde(skip)] tx: Sender, @@ -35,21 +35,17 @@ struct Mixer { impl Mixer { #[inline(always)] - fn update_ctrl_outputs(&mut self, ctrl_group_id: u8, msg: &ControllerOutputGroupMsg) { - self.controller_outputs[ctrl_group_id as usize] = *msg; - let mut publish = Vec::::new(); - + fn update_ctrl_outputs(&self, msg: &TorqueThrustMsg) { + let mut publish: [f32; 8] = [0.0; 8]; for i in &self.mixers { - if i.bind_ctrl_group_id == ctrl_group_id { - publish.push(MixerOutData{ - chn: i.output_channel_idx, - mode: i.mode.clone(), - data: i.calcuate(&self.controller_outputs), - }); + if i.bind_ctrl_group_id == 0 { + // TODO: remove this + publish[i.output_channel_idx as usize] = i.calcuate(&msg) } } self.tx.send(MixerOutputMsg { - output: Box::new(publish), + output: publish, + control_group_id: 0, }); } @@ -57,10 +53,10 @@ impl Mixer { where P: AsRef, { - let mut toml_str = String::new(); + let mut s = String::new(); if let Ok(mut file) = std::fs::File::open(filepath) { - file.read_to_string(&mut toml_str).unwrap(); - if let Ok(temp) = toml::from_str::(&toml_str) { + file.read_to_string(&mut s).unwrap(); + if let Ok(temp) = serde_json::from_str::(&s) { self.mixers = temp.mixers; } else { return Err(()); @@ -85,17 +81,17 @@ impl Mixer { MixerChannel { scaler: Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Pitch as u8, + ctrl_channel: ControlChannel::Pitch, }, MixerChannel { scaler: -Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Roll as u8, + ctrl_channel: ControlChannel::Roll, }, ], bind_ctrl_group_id: 0, output_channel_idx: 0, - mode:OutputMode::Speed + mode: OutputMode::Speed, }; let motor_1 = SumMixer { @@ -103,17 +99,17 @@ impl Mixer { MixerChannel { scaler: -Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Pitch as u8, + ctrl_channel: ControlChannel::Pitch, }, MixerChannel { scaler: Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Roll as u8, + ctrl_channel: ControlChannel::Roll, }, ], bind_ctrl_group_id: 0, output_channel_idx: 1, - mode:OutputMode::Speed + mode: OutputMode::Speed, }; let motor_2 = SumMixer { @@ -121,17 +117,17 @@ impl Mixer { MixerChannel { scaler: Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Pitch as u8, + ctrl_channel: ControlChannel::Pitch, }, MixerChannel { scaler: Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Roll as u8, + ctrl_channel: ControlChannel::Roll, }, ], bind_ctrl_group_id: 0, output_channel_idx: 2, - mode:OutputMode::Speed + mode: OutputMode::Speed, }; let motor_3 = SumMixer { @@ -139,17 +135,17 @@ impl Mixer { MixerChannel { scaler: -Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Pitch as u8, + ctrl_channel: ControlChannel::Pitch, }, MixerChannel { scaler: -Scaler::default(), ctrl_group_id: 0, - ctrl_channel_idx: ControllerOutputChannel::Roll as u8, + ctrl_channel: ControlChannel::Roll, }, ], bind_ctrl_group_id: 0, output_channel_idx: 3, - mode:OutputMode::Speed + mode: OutputMode::Speed, }; self.mixers.push(motor_0); @@ -157,23 +153,13 @@ impl Mixer { self.mixers.push(motor_2); self.mixers.push(motor_3); } - } -enum ControllerOutputChannel { - Pitch = 0x0, - Roll, - Thrust, - Yaw, -} - - - #[derive(Debug, serde::Serialize, serde::Deserialize)] struct MixerChannel { scaler: Scaler, ctrl_group_id: u8, - ctrl_channel_idx: u8, + ctrl_channel: ControlChannel, } #[derive(Debug, serde::Serialize, serde::Deserialize)] @@ -182,152 +168,128 @@ struct SumMixer { bind_ctrl_group_id: u8, output_channel_idx: u8, #[serde(default)] - mode:OutputMode + mode: OutputMode, } -#[derive(Debug, Default, serde::Serialize, serde::Deserialize,Clone)] -pub enum OutputMode{ +#[derive(Debug, serde::Serialize, serde::Deserialize, Clone,Copy)] +enum ControlChannel{ + Pitch=0, + Roll, + Yaw, + ThrustX, + ThrustY, + ThrustZ, +} + +#[derive(Debug, Default, serde::Serialize, serde::Deserialize, Clone)] +pub enum OutputMode { #[default] Duty, // 0 ~ 1.0 PluseWidth, // 0 ~ 2000 us (actually we can get a larger value, but no significance) - DShot, // 0 ~ 1.0 - Speed, // only used in simulation now -} - -#[derive(Debug,Clone)] -pub struct MixerOutData{ - pub chn:u8, - pub mode:OutputMode, - pub data:f32 + DShot, // 0 ~ 1.0 + Speed, // only used in simulation now } impl SumMixer { - fn calcuate(&self, ctrl_output_groups: &Vec) -> f32 { - let ret: f32 = self.list.iter().fold(0.0, |acc, i| { - let group = ctrl_output_groups.get(i.ctrl_group_id as usize).unwrap(); - let input = group.output.get(i.ctrl_channel_idx as usize).unwrap(); - acc + i.scaler.scale(*input) + fn calcuate(&self, ctrl_out: &TorqueThrustMsg) -> f32 { + let mixer: f32 = self.list.iter().fold(0.0, |acc, i| { + let input; + if (i.ctrl_channel as u8) < 3 { + input = ctrl_out.torques[i.ctrl_channel as usize]; + } else if (i.ctrl_channel as u8) < 6 { + input = ctrl_out.thrusts[i.ctrl_channel as usize]; + } else { + panic!("error index!"); + } + acc + i.scaler.scale(input) }); - ret + mixer } } -static mut MIXER: OnceLock = OnceLock::new(); -const CONTROLLER_OUTPUT_COUNT: u8 = 2; - pub unsafe fn init_mixer(argc: u32, argv: *const &str) { - let _ = MIXER.get_or_init(|| { - let mut ret = Mixer { - controller_outputs: Vec::new(), - mixers: Vec::new(), - tx: get_new_tx_of_message("mixer_output").unwrap(), - }; - - if argc == 2 { - let path = std::slice::from_raw_parts(argv, argc as usize); - println!("read mixer from {}.", path[1]); - ret.read_mixers_info_from_file(path[1]).unwrap(); - } else if argc == 1 { - println!("use default x quadcopter mixer!"); - ret.init_x_quadcopter_mixers(); - } else { - panic!("error arg num of mixer!"); - } + let mut mixer = Mixer { + controller_outputs: Vec::new(), + mixers: Vec::new(), + tx: get_new_tx_of_message("mixer_output").unwrap(), + }; + + if argc == 2 { + let path = std::slice::from_raw_parts(argv, argc as usize); + println!("read mixer from {}.", path[1]); + mixer.read_mixers_info_from_file(path[1]).unwrap(); + } else if argc == 1 { + println!("use default x quadcopter mixer!"); + mixer.init_x_quadcopter_mixers(); + } else { + panic!("error arg num of mixer!"); + } - for i in 0..CONTROLLER_OUTPUT_COUNT { - let rx = get_new_rx_of_message::( - format!("controller_output{}", i).as_str(), - ) - .unwrap(); - ret.controller_outputs - .push(ControllerOutputGroupMsg { output: [0.0; 8] }); - rx.register_callback( - format!("mixer_listener{}", i).as_str(), - move |x: &ControllerOutputGroupMsg| { - MIXER.get_mut().unwrap().update_ctrl_outputs(i, x); - }, - ); - } - ret + let rx = get_new_rx_of_message::("controller_output").unwrap(); + rx.register_callback("mixer_listner", move |x: &TorqueThrustMsg| { + mixer.update_ctrl_outputs(x); }); } #[rpos::ctor::ctor] fn register() { - rpos::msg::add_message:: ("mixer_output"); - // send a empty message, to fill the "data" field of channel - // or it would panic if the rx try to read - get_new_tx_of_message("mixer_output").unwrap().send(MixerOutputMsg{ output: Box::new(Vec::new()) }); rpos::module::Module::register("mixer", |a, b| unsafe { init_mixer(a, b) }); - } #[cfg(test)] mod tests { use std::{default, ptr::null_mut}; - use crate::mixer; + use crate::{mixer, msg_define::{EulerVector3, Vector3}}; use super::*; - fn get_fake_controller_output_tx(group_id: u8) -> Sender { - let tx = get_new_tx_of_message::( - format!("controller_output{}", group_id).as_str(), - ) - .unwrap(); - tx - } #[test] fn test_init_mixer() { - let tx = get_fake_controller_output_tx(0); - unsafe { - init_mixer(1, null_mut()); - assert_eq!(MIXER.get().unwrap().controller_outputs.len(), 2); - - tx.send(ControllerOutputGroupMsg { output: [1.0; 8] }); - for i in 0..8 { - assert!(MIXER.get().unwrap().controller_outputs[0].output[i as usize] > 0.0); - assert!(MIXER.get().unwrap().controller_outputs[0].output[i as usize] < 2.0); - } - } - } - - #[test] - fn test_x_quadcoptermixer_calcute() { - let ctrl_tx = get_fake_controller_output_tx(0); - let mut mixer_rx = get_new_rx_of_message::("mixer_output").unwrap(); - unsafe { - init_mixer(1, null_mut()); - MIXER.get_mut().unwrap().init_x_quadcopter_mixers(); - } - - ctrl_tx.send(ControllerOutputGroupMsg { output: [50.0; 8] }); - println!("{:?}", mixer_rx.read()) - } - - #[test] - fn test_mixer2toml() { + let tx = get_new_tx_of_message::("controller_output").unwrap(); + let mut rx =get_new_rx_of_message::("mixer_output").unwrap(); unsafe { init_mixer(1, null_mut()); + assert!(rx.try_read().is_none()); + tx.send(TorqueThrustMsg { + torques: EulerVector3{ + pitch: 1.0, + roll: 0.0, + yaw: 0.0, + }, + thrusts: Vector3{ + x: 0.0, + y: 0.0, + z: 0.0, + }, + }); + rx.try_read().unwrap(); } - println!( - "{}", - toml::to_string_pretty(unsafe { &(MIXER.get().unwrap()) }).unwrap() - ); } - #[test] - fn test_toml2mixer() { - unsafe { - init_mixer(1, null_mut()); - let origin = format!("{:?}", MIXER.get().unwrap().mixers); - MIXER - .get_mut() - .unwrap() - .read_mixers_info_from_file("mixers/gz_mixer.toml") - .unwrap(); - let new = format!("{:?}", MIXER.get().unwrap().mixers); - assert_eq!(origin, new); - } - } + // #[test] + // fn test_mixer2toml() { + // unsafe { + // init_mixer(1, null_mut()); + // } + // println!( + // "{}", + // toml::to_string_pretty(unsafe { &(MIXER.get().unwrap()) }).unwrap() + // ); + // } + + // #[test] + // fn test_toml2mixer() { + // unsafe { + // init_mixer(1, null_mut()); + // let origin = format!("{:?}", MIXER.get().unwrap().mixers); + // MIXER + // .get_mut() + // .unwrap() + // .read_mixers_info_from_file("mixers/gz_mixer.toml") + // .unwrap(); + // let new = format!("{:?}", MIXER.get().unwrap().mixers); + // assert_eq!(origin, new); + // } + // } } diff --git a/src/mode.rs b/src/mode.rs new file mode 100644 index 0000000..039d561 --- /dev/null +++ b/src/mode.rs @@ -0,0 +1,9 @@ + + +pub enum FlightMode{ + Manual, + Stabilize, + Altitude, + Position +} + diff --git a/src/msg_define.rs b/src/msg_define.rs index ca2fcb8..f437a8e 100644 --- a/src/msg_define.rs +++ b/src/msg_define.rs @@ -1,37 +1,73 @@ +#![allow(dead_code)] +use std::ops::Index; + use rpos::msg::add_message; // Gyro/Acc message data, unit:rad/s #[derive(Debug,Clone,Copy,Default)] -pub struct Vector3Msg{ +pub struct Vector3{ pub x:f32, pub y:f32, pub z:f32 } - -// Attitude message data #[derive(Debug,Clone,Copy)] -pub struct AttitudeMsg{ +pub struct Vector4{ pub w:f32, pub x:f32, pub y:f32, pub z:f32 } +#[derive(Debug,Clone,Copy)] +pub struct AttitudeSetPointMsg{ + pub attitude:Vector4, // quaternion + pub body_thrusts:Vector3 // [-1,1] +} + // Attitude target euler #[derive(Debug,Clone,Copy)] -pub struct AttitudeTargetEulerMsg{ +pub struct EulerVector3{ pub pitch:f32, pub roll:f32, pub yaw:f32 } -// Controller Output -#[allow(dead_code)] +impl Index for EulerVector3{ + type Output = f32; + fn index(&self, index:usize) -> &Self::Output { + match index { + 0=>&self.pitch, + 1=>&self.roll, + 2=>&self.yaw, + _=>panic!("over index!") + } + } +} + +impl Index for Vector3{ + type Output = f32; + fn index(&self, index:usize) -> &Self::Output { + match index { + 0=>&self.x, + 1=>&self.y, + 2=>&self.z, + _=>panic!("over index!") + } + } +} + #[derive(Debug,Clone,Copy)] -pub struct ControllerOutputGroupMsg{ - pub output:[f32;8], +pub struct TorqueThrustMsg{ + pub torques:EulerVector3, + pub thrusts:Vector3 +} + +// will used in rate controller +pub struct RateSetPointMsg{ + pub angle_rate:EulerVector3, + pub thrusts:Vector3 } // Manual Control Input @@ -63,13 +99,15 @@ pub struct MixerOutputMsg{ #[rpos::ctor::ctor] fn register_msgs(){ - add_message::("gyro"); - add_message::("acc"); - add_message::("attitude"); - add_message::("att_target_euler"); - add_message::("controller_output0"); - add_message::("controller_output1"); - + add_message::("gyro"); + add_message::("acc"); + add_message::("attitude"); + //add_message::("att_target_euler"); + add_message::("att_target"); + add_message::("controller_output"); + //add_message::("controller_output0"); + //add_message::("controller_output1"); + add_message:: ("mixer_output"); add_message::("manual_control"); add_message::("rc_input"); } diff --git a/src/msg_echo.rs b/src/msg_echo.rs index 6a1daa4..ba940b5 100644 --- a/src/msg_echo.rs +++ b/src/msg_echo.rs @@ -1,7 +1,7 @@ use clap::Parser; use rpos::{msg::get_new_rx_of_message, thread_logln}; -use crate::{elrs::client_process_args, msg_define::*}; +use crate::{ msg_define::*}; #[derive(Parser, Debug)] #[command(name = "msg_echo", version, about="a dirty implement to debug")] @@ -16,7 +16,7 @@ struct Cli { fn msg_echo_main(argc:u32, argv:*const &str){ - if let Some(args) = client_process_args::(argc, argv){ + if let Some(args) = crate::basic::client_process_args::(argc, argv){ let mut func:Box ()>; if args.topic == "rc_input"{ let mut rx = get_new_rx_of_message::(&args.topic).unwrap(); diff --git a/src/param.rs b/src/param.rs index d03af7f..18a2f23 100644 --- a/src/param.rs +++ b/src/param.rs @@ -1,3 +1,4 @@ +#![allow(dead_code)] use core::panic; use std::sync::LazyLock; diff --git a/start_scripts/car.sh b/start_scripts/car.sh index 05cf706..110195c 100644 --- a/start_scripts/car.sh +++ b/start_scripts/car.sh @@ -7,5 +7,5 @@ sleep 1 # I have fix the bug in dshot mode, while pwm mode i have never test yet # so just set spi-mode to 0, in which the module could work ./rust_pilot -- fpga_spi_pwm -d /dev/spidev0.2 --predivider 4 -f ../fpga/spi_pwm.bin --spi-mode 0 -./rust_pilot -- mixer ./car_mixer.toml +./rust_pilot -- mixer ./car_mixer.json ./rust_pilot -- manual_ctrl -d diff --git a/start_scripts/gz_sim.sh b/start_scripts/gz_sim.sh index 98b09dc..c9ad88e 100644 --- a/start_scripts/gz_sim.sh +++ b/start_scripts/gz_sim.sh @@ -3,6 +3,6 @@ ./rust_pilot gazebo_actuator -./rust_pilot mixer /home/ncer/RustPilot/mixers/gz_mixer.toml +./rust_pilot mixer /home/ncer/RustPilot/mixers/gz_mixer.json ./rust_pilot att_control \ No newline at end of file From 4e8e6c631532e32dd652b23aea58da3290b4acd5 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Mon, 21 Apr 2025 00:35:06 +0800 Subject: [PATCH 7/9] add basic docs Signed-off-by: Ncerzzk --- docs/axis.md | 30 ++++++++++++++++++++++++++++++ docs/param.md | 12 ++++++++++++ docs/sim.md | 27 +++++++++++++++++++++++++++ 3 files changed, 69 insertions(+) create mode 100644 docs/axis.md create mode 100644 docs/param.md create mode 100644 docs/sim.md diff --git a/docs/axis.md b/docs/axis.md new file mode 100644 index 0000000..2c9188e --- /dev/null +++ b/docs/axis.md @@ -0,0 +1,30 @@ +# 坐标系定义 + +## RustPilot +机体坐标系使用右(x)、前(y)、上(z) + +世界坐标系使用东、北、天定义。 在初始状态下，机体坐标系与世界坐标系应是重合的。 + +绕X轴旋转为俯仰(Pitch) +绕Y轴旋转为横滚(Roll) +绕Z轴旋转为偏航(Yaw) + +旋转的正方向由右手定则确定。 + +## Gazebo + +Gazebo仿真器的坐标系定义为：前、左、上。 + +旋转定义方式为： +绕X轴旋转为横滚(Roll) +绕Y轴旋转为俯仰(Pitch) +绕Z轴旋转为偏航(Yaw) + +旋转的正方向由右手确定。 + +## Gz坐标系-> 世界坐标系 + +Gz 坐标系绕z轴旋转90°就是世界坐标系，因此其旋转矩阵为： + +`q = cos\theta + k sin\ + diff --git a/docs/param.md b/docs/param.md new file mode 100644 index 0000000..4341aac --- /dev/null +++ b/docs/param.md @@ -0,0 +1,12 @@ +# 参数子系统核心需求 + +## 参数定义和存储 + +## 访问/更新接口 +- 线程安全 + +## 参数更新回调 +- 参数更新后，要有机制通知关心的线程 + +## 元数据与可维护性 + diff --git a/docs/sim.md b/docs/sim.md new file mode 100644 index 0000000..31edc5b --- /dev/null +++ b/docs/sim.md @@ -0,0 +1,27 @@ +# 仿真 + +## Overview +本章介绍如何RustPilot如何与Gazebo进行飞行器软件仿真。 + +当前仅支持四旋翼x3的仿真。 + +要支持在gazebo中仿真，其中要解决坐标系的差异问题。二者的坐标系定义可见坐标系章节。 + +首先明确，什么是姿态？ + +如果一个飞行器从初始状态开始，进行了一定的旋转运动 到另一个确定状态。那么此时我们说的姿态是什么？ + +有人说是此时飞行器的欧拉角：俯仰角、横滚角、偏航角。但这其实不够直接，真正直接其实是这些欧拉角表达的 旋转 + +也就是，从一个状态，到另一个状态，我们所说的姿态，其实是在说：它是怎么旋转过去的。我们关心的其实只是这个旋转。 + +既然关心的是旋转，那么我们就可以有多种方式来表达旋转，比如欧拉角、旋转矩阵、四元数、轴角等。关于这些表达方式的优劣，这里不再赘述，我们这里统一用四元数+ 轴角来描述。 + +那么此时，在仿真世界中有一架飞行器，此时至少有两个机体坐标系，分别是RustPilot的坐标系和仿真器的坐标系。 + +处于简单起见，我们定义世界坐标系与飞行器初始状态下在RustPilot中的坐标系重合。 + + + + + From 9c830f57676dc0e7484df3ec1b4a01c84eddb45f Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Mon, 21 Apr 2025 00:37:17 +0800 Subject: [PATCH 8/9] mavlink joystick support Signed-off-by: Ncerzzk --- src/main.rs | 2 +- src/manual_ctrl.rs | 54 ++++++++++++++++++++++++++++++++--------- src/mavlink_gs.rs | 27 +++++++++++++++++---- src/msg_define.rs | 4 +-- start_scripts/gz_sim.sh | 6 ++++- 5 files changed, 72 insertions(+), 21 deletions(-) diff --git a/src/main.rs b/src/main.rs index 75bf538..f35470b 100644 --- a/src/main.rs +++ b/src/main.rs @@ -16,7 +16,7 @@ mod mixer; mod imu_update; mod elrs; //mod fpga_spi_pwm; -//mod manual_ctrl; +mod manual_ctrl; mod msg_echo; mod mavlink_gs; mod basic; diff --git a/src/manual_ctrl.rs b/src/manual_ctrl.rs index 667a1d3..a17102a 100644 --- a/src/manual_ctrl.rs +++ b/src/manual_ctrl.rs @@ -1,14 +1,16 @@ use clap::Parser; -use rpos::{ - msg::{get_new_rx_of_message, get_new_tx_of_message}, -}; +use rpos::msg::{get_new_rx_of_message, get_new_tx_of_message}; use crate::{ msg_define::RcInputMsg, + msg_define::{AttitudeSetPointMsg, EulerVector3, TorqueThrustMsg, Vector3, Vector4}, }; #[derive(Parser, Clone)] -#[command(name = "manual_ctrl",about = "recv rc input, do channel mapping(if need) and output to mixer or attitude_controller")] +#[command( + name = "manual_ctrl", + about = "recv rc input, do channel mapping(if need) and output to mixer or attitude_controller" +)] struct ManualCtrl { #[arg(short, long, help = "directly send output to mixer")] directly_out: bool, @@ -17,17 +19,45 @@ struct ManualCtrl { pub fn init_manual_ctrl(argc: u32, argv: *const &str) { if let Some(args) = crate::basic::client_process_args::(argc, argv) { let rx = get_new_rx_of_message::("rc_input").unwrap(); - - let ctrl_msg_tx = - get_new_tx_of_message::("controller_output0").unwrap(); - rx.register_callback("manual_ctrl_rx", move |rc_msg| { - if args.directly_out { + if args.directly_out { + let ctrl_msg_tx = + get_new_tx_of_message::("toreque_thrust_setpoint").unwrap(); + rx.register_callback("manual_ctrl_rx", move |rc_msg| { let arr = rc_msg .channel_vals .map(|x| (x as f32).clamp(-1000.0, 1000.0)); - ctrl_msg_tx.send(ControllerOutputGroupMsg { output: arr }); - } - }); + ctrl_msg_tx.send(TorqueThrustMsg { + torques: EulerVector3 { + pitch: arr[1], + roll: arr[0], + yaw: arr[3], + }, + thrusts: Vector3 { + x: 0.0, + y: 0.0, + z: arr[2], + }, + }); + }); + } else { + let att_target_tx = get_new_tx_of_message::("att_target").unwrap(); + + rx.register_callback("manual_ctrl_rx", move |rc_msg| { + att_target_tx.send(AttitudeSetPointMsg { + attitude: Vector4{ + w: 1.0, + x: 0.0, + y: 0.0, + z: 0.0, + }, + body_thrusts: Vector3 { + x: 0.0, + y: 0.0, + z: (rc_msg.channel_vals[2] + 1000) as f32 / 2000.0, + }, // maping -1000~1000 to 0~1 } + }); + }); + } } } diff --git a/src/mavlink_gs.rs b/src/mavlink_gs.rs index a1979da..db5426d 100644 --- a/src/mavlink_gs.rs +++ b/src/mavlink_gs.rs @@ -1,9 +1,9 @@ use clap::Parser; -use rpos::thread_logln; -use std::sync::Arc; +use rpos::{thread_logln, msg::get_new_tx_of_message}; +use std::{sync::Arc}; use crate::{ - param::{self, ParameterData}, + param::{self, ParameterData}, msg_define::RcInputMsg, }; use mavlink::{ common::{self, MavMessage}, @@ -15,6 +15,10 @@ use mavlink::{ struct Cli { #[arg(short, long, value_name = "addr")] addr: String, + + #[arg(long, help = "use joystick provided by ground station")] + joystick: bool, + } fn get_mav_paramtype(p: ¶m::ParameterData) -> common::MavParamType { @@ -109,6 +113,14 @@ pub unsafe fn init_mavlink_gs(argc: u32, argv: *const &str) { library_version_hash: [0; 8], }); + let rc_input_tx; + + if args.joystick{ + rc_input_tx = Some(get_new_tx_of_message::("rc_input").unwrap()); + }else{ + rc_input_tx = None; + } + loop { match mavconn.recv() { Ok((_header, msg)) => { @@ -192,8 +204,13 @@ pub unsafe fn init_mavlink_gs(argc: u32, argv: *const &str) { } MavMessage::HEARTBEAT(_) => {} - MavMessage::MANUAL_CONTROL(_) => { - // thread_logln!("received: {msg:?}"); + MavMessage::MANUAL_CONTROL(data) => { + if let Some(ref tx) = rc_input_tx{ + let mut vals = [0;8]; + vals[2] = (data.z - 500) * 2; // map 0-1000 to -1000 to 1000 + tx.send(RcInputMsg { channel_vals: vals }) + } + //thread_logln!("received: {msg:?}"); } _ => { thread_logln!("received: {msg:?}"); diff --git a/src/msg_define.rs b/src/msg_define.rs index f437a8e..ee7b6b9 100644 --- a/src/msg_define.rs +++ b/src/msg_define.rs @@ -82,7 +82,7 @@ pub struct ManualControlMsg{ #[derive(Debug,Clone)] pub struct RcInputMsg{ - pub channel_vals:[i16;8] + pub channel_vals:[i16;8] // -1000~1000 } #[allow(dead_code)] @@ -104,7 +104,7 @@ fn register_msgs(){ add_message::("attitude"); //add_message::("att_target_euler"); add_message::("att_target"); - add_message::("controller_output"); + add_message::("toreque_thrust_setpoint"); //add_message::("controller_output0"); //add_message::("controller_output1"); add_message:: ("mixer_output"); diff --git a/start_scripts/gz_sim.sh b/start_scripts/gz_sim.sh index c9ad88e..58a73df 100644 --- a/start_scripts/gz_sim.sh +++ b/start_scripts/gz_sim.sh @@ -5,4 +5,8 @@ ./rust_pilot mixer /home/ncer/RustPilot/mixers/gz_mixer.json -./rust_pilot att_control \ No newline at end of file +./rust_pilot att_control + +./rust_pilot -- manual_ctrl + +./rust_pilot -- mavlink_gs --addr localhost:14550 --joystick \ No newline at end of file From 8fe559b67bd668205aabad1d83d536c84e984a54 Mon Sep 17 00:00:00 2001 From: Ncerzzk Date: Mon, 21 Apr 2025 00:38:14 +0800 Subject: [PATCH 9/9] mixer update Signed-off-by: Ncerzzk --- mixers/gz_mixer.json | 44 ++++++++++++++++++++++++++++++++++++++++++++ src/att_control.rs | 2 +- src/mixer.rs | 6 +++--- 3 files changed, 48 insertions(+), 4 deletions(-) diff --git a/mixers/gz_mixer.json b/mixers/gz_mixer.json index ec53a62..6c3e707 100644 --- a/mixers/gz_mixer.json +++ b/mixers/gz_mixer.json @@ -26,6 +26,17 @@ "min": -100, "max": 100 } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "ThrustZ", + "scaler": { + "scale_p": 1000, + "scale_n": 1000, + "offset": 0, + "min": 0, + "max": 1000 + } } ] }, @@ -55,6 +66,17 @@ "min": -100, "max": 100 } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "ThrustZ", + "scaler": { + "scale_p": 1000, + "scale_n": 1000, + "offset": 0, + "min": 0, + "max": 1000 + } } ] }, @@ -84,6 +106,17 @@ "min": -100, "max": 100 } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "ThrustZ", + "scaler": { + "scale_p": 1000, + "scale_n": 1000, + "offset": 0, + "min": 0, + "max": 1000 + } } ] }, @@ -113,6 +146,17 @@ "min": -100, "max": 100 } + }, + { + "ctrl_group_id": 0, + "ctrl_channel": "ThrustZ", + "scaler": { + "scale_p": 1000, + "scale_n": 1000, + "offset": 0, + "min": 0, + "max": 1000 + } } ] } diff --git a/src/att_control.rs b/src/att_control.rs index d892381..194be29 100644 --- a/src/att_control.rs +++ b/src/att_control.rs @@ -51,7 +51,7 @@ fn att_control_main(ptr: *mut c_void) -> *mut c_void { let mut att_ctrler = AttitudeController { pitch_controller: PIDController::new(100.0, 0.0, 0.0), roll_controller: PIDController::new(100.0, 0.0, 0.0), - tx: get_new_tx_of_message("controller_output").unwrap(), + tx: get_new_tx_of_message("toreque_thrust_setpoint").unwrap(), }; let mut att_target_q: Q = (1.0, [0.0, 0.0, 0.0]); diff --git a/src/mixer.rs b/src/mixer.rs index 1331904..75b8f36 100644 --- a/src/mixer.rs +++ b/src/mixer.rs @@ -197,7 +197,7 @@ impl SumMixer { if (i.ctrl_channel as u8) < 3 { input = ctrl_out.torques[i.ctrl_channel as usize]; } else if (i.ctrl_channel as u8) < 6 { - input = ctrl_out.thrusts[i.ctrl_channel as usize]; + input = ctrl_out.thrusts[i.ctrl_channel as usize - 3]; } else { panic!("error index!"); } @@ -225,7 +225,7 @@ pub unsafe fn init_mixer(argc: u32, argv: *const &str) { panic!("error arg num of mixer!"); } - let rx = get_new_rx_of_message::("controller_output").unwrap(); + let rx = get_new_rx_of_message::("toreque_thrust_setpoint").unwrap(); rx.register_callback("mixer_listner", move |x: &TorqueThrustMsg| { mixer.update_ctrl_outputs(x); }); @@ -246,7 +246,7 @@ mod tests { #[test] fn test_init_mixer() { - let tx = get_new_tx_of_message::("controller_output").unwrap(); + let tx = get_new_tx_of_message::("toreque_thrust_setpoint").unwrap(); let mut rx =get_new_rx_of_message::("mixer_output").unwrap(); unsafe { init_mixer(1, null_mut());