Mixer restruct

Cargo.toml

@@ -18,6 +18,8 @@ crsf = "1.0.1"
 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"

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<std::path::PathBuf, Box<dyn std::error::Error>> {
-    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<std::path::PathBuf, Box<dyn std::error::Error>> {
+//     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());

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\
+

docs/param.md

@@ -0,0 +1,12 @@
+# 参数子系统核心需求
+
+## 参数定义和存储
+
+## 访问/更新接口
+- 线程安全
+
+## 参数更新回调
+- 参数更新后，要有机制通知关心的线程
+
+## 元数据与可维护性
+

docs/sim.md

@@ -0,0 +1,27 @@
+# 仿真
+
+## Overview
+本章介绍如何RustPilot如何与Gazebo进行飞行器软件仿真。
+
+当前仅支持四旋翼x3的仿真。
+
+要支持在gazebo中仿真，其中要解决坐标系的差异问题。二者的坐标系定义可见坐标系章节。
+
+首先明确，什么是姿态？
+
+如果一个飞行器从初始状态开始，进行了一定的旋转运动 到另一个确定状态。那么此时我们说的姿态是什么？
+
+有人说是此时飞行器的欧拉角：俯仰角、横滚角、偏航角。但这其实不够直接，真正直接其实是这些欧拉角表达的 旋转
+
+也就是，从一个状态，到另一个状态，我们所说的姿态，其实是在说：它是怎么旋转过去的。我们关心的其实只是这个旋转。
+
+既然关心的是旋转，那么我们就可以有多种方式来表达旋转，比如欧拉角、旋转矩阵、四元数、轴角等。关于这些表达方式的优劣，这里不再赘述，我们这里统一用四元数+ 轴角来描述。
+
+那么此时，在仿真世界中有一架飞行器，此时至少有两个机体坐标系，分别是RustPilot的坐标系和仿真器的坐标系。
+
+处于简单起见，我们定义世界坐标系与飞行器初始状态下在RustPilot中的坐标系重合。
+
+
+
+
+

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
+          }
+        }
+      ]
+    }
+  ]
+}

mixers/gz_mixer.json

@@ -0,0 +1,164 @@
+{
+    "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
+            }
+          },
+          {
+            "ctrl_group_id": 0,
+            "ctrl_channel": "ThrustZ",
+            "scaler": {
+              "scale_p": 1000,
+              "scale_n": 1000,
+              "offset": 0,
+              "min": 0,
+              "max": 1000
+            }
+          }
+        ]
+      },
+      {
+        "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
+            }
+          },
+          {
+            "ctrl_group_id": 0,
+            "ctrl_channel": "ThrustZ",
+            "scaler": {
+              "scale_p": 1000,
+              "scale_n": 1000,
+              "offset": 0,
+              "min": 0,
+              "max": 1000
+            }
+          }
+        ]
+      },
+      {
+        "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
+            }
+          },
+          {
+            "ctrl_group_id": 0,
+            "ctrl_channel": "ThrustZ",
+            "scaler": {
+              "scale_p": 1000,
+              "scale_n": 1000,
+              "offset": 0,
+              "min": 0,
+              "max": 1000
+            }
+          }
+        ]
+      },
+      {
+        "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
+            }
+          },
+          {
+            "ctrl_group_id": 0,
+            "ctrl_channel": "ThrustZ",
+            "scaler": {
+              "scale_p": 1000,
+              "scale_n": 1000,
+              "offset": 0,
+              "min": 0,
+              "max": 1000
+            }
+          }
+        ]
+      }
+    ]
+  }