432 new mixing system

.github/workflows/pre-release.yml

@@ -16,7 +16,7 @@ jobs:
     - name: checkout submodules
       run: git submodule update --init --recursive
     - name: install toolchain
-      run: sudo apt -y install gcc-arm-none-eabi
+      run: sudo apt -y install gcc-arm-none-eabi libeigen3-dev
     - name: check toolchain
       run: arm-none-eabi-gcc --version
     - name: build varmint

.github/workflows/release.yml

@@ -16,7 +16,7 @@ jobs:
       - name: checkout submodules
         run: git submodule update --init --recursive
       - name: install toolchain
-        run: sudo apt -y install gcc-arm-none-eabi
+        run: sudo apt -y install gcc-arm-none-eabi libeigen3-dev
       - name: check toolchain
         run: arm-none-eabi-gcc --version
       - name: build varmint

.github/workflows/unit_tests.yml

@@ -12,7 +12,7 @@ jobs:
     - name: clone
       run: git submodule update --init --recursive
     - name: apt install
-      run: sudo apt-get install -y build-essential cmake libgtest-dev libeigen3-dev
+      run: sudo apt-get install -y build-essential cmake libgtest-dev
     - name: install gtest
       run: |
         cd /usr/src/gtest

.gitmodules

@@ -4,3 +4,6 @@
 [submodule "boards/varmint_h7"]
 	path = boards/varmint_h7
 	url = https://github.com/rosflight/varmint_h7.git
+[submodule "lib/eigen"]
+	path = lib/eigen
+	url = https://gitlab.com/libeigen/eigen.git

CMakeLists.txt

@@ -29,15 +29,13 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
 endif()
 
 ### source files ###
-
 include_directories(
     include
     include/interface
     lib
     comms/mavlink
     comms/mavlink/v1.0
     comms/mavlink/v1.0/common
-    comms/mavlink/v1.0/rosflight
 )
 
 file(GLOB_RECURSE ROSFLIGHT_SOURCES

comms/mavlink/mavlink.cpp

@@ -487,15 +487,19 @@ void Mavlink::handle_msg_offboard_control(const mavlink_message_t * const msg)
       return;
   }
 
-  control.x.value = ctrl.x;
-  control.y.value = ctrl.y;
-  control.z.value = ctrl.z;
-  control.F.value = ctrl.F;
-
-  control.x.valid = !(ctrl.ignore & IGNORE_VALUE1);
-  control.y.valid = !(ctrl.ignore & IGNORE_VALUE2);
-  control.z.valid = !(ctrl.ignore & IGNORE_VALUE3);
-  control.F.valid = !(ctrl.ignore & IGNORE_VALUE4);
+  control.Qx.value = ctrl.Qx;
+  control.Qy.value = ctrl.Qy;
+  control.Qz.value = ctrl.Qz;
+  control.Fx.value = ctrl.Fx;
+  control.Fy.value = ctrl.Fy;
+  control.Fz.value = ctrl.Fz;
+
+  control.Qx.valid = !(ctrl.ignore & IGNORE_VALUE1);
+  control.Qy.valid = !(ctrl.ignore & IGNORE_VALUE2);
+  control.Qz.valid = !(ctrl.ignore & IGNORE_VALUE3);
+  control.Fx.valid = !(ctrl.ignore & IGNORE_VALUE4);
+  control.Fy.valid = !(ctrl.ignore & IGNORE_VALUE5);
+  control.Fz.valid = !(ctrl.ignore & IGNORE_VALUE6);
 
   if (listener_ != nullptr) { listener_->offboard_control_callback(control); }
 }

include/command_manager.h

@@ -47,7 +47,7 @@ typedef enum
 {
   RATE,        // Channel is is in rate mode (mrad/s)
   ANGLE,       // Channel command is in angle mode (mrad)
-  THROTTLE,    // Channel is direcly controlling throttle max/1000
+  THROTTLE,    // Channel is controlling throttle setting, which will be converted to force
   PASSTHROUGH, // Channel directly passes PWM input to the mixer
 } control_type_t;
 
@@ -58,13 +58,22 @@ typedef struct
   float value;         // The value of the channel
 } control_channel_t;
 
+typedef enum
+{
+  X_AXIS,
+  Y_AXIS,
+  Z_AXIS,
+} rc_f_axis_t;
+
 typedef struct
 {
   uint32_t stamp_ms;
-  control_channel_t x;
-  control_channel_t y;
-  control_channel_t z;
-  control_channel_t F;
+  control_channel_t Qx;
+  control_channel_t Qy;
+  control_channel_t Qz;
+  control_channel_t Fx;
+  control_channel_t Fy;
+  control_channel_t Fz;
 } control_t;
 
 class CommandManager : public ParamListenerInterface
@@ -77,38 +86,50 @@ class CommandManager : public ParamListenerInterface
     control_channel_t * combined;
   } mux_t;
 
-  mux_t muxes[4] = {{&rc_command_.x, &offboard_command_.x, &combined_command_.x},
-                    {&rc_command_.y, &offboard_command_.y, &combined_command_.y},
-                    {&rc_command_.z, &offboard_command_.z, &combined_command_.z},
-                    {&rc_command_.F, &offboard_command_.F, &combined_command_.F}};
+  mux_t muxes[6] = {{&rc_command_.Qx, &offboard_command_.Qx, &combined_command_.Qx},
+                    {&rc_command_.Qy, &offboard_command_.Qy, &combined_command_.Qy},
+                    {&rc_command_.Qz, &offboard_command_.Qz, &combined_command_.Qz},
+                    {&rc_command_.Fx, &offboard_command_.Fx, &combined_command_.Fx},
+                    {&rc_command_.Fy, &offboard_command_.Fy, &combined_command_.Fy},
+                    {&rc_command_.Fz, &offboard_command_.Fz, &combined_command_.Fz}};
 
   // clang-format off
   control_t rc_command_ = {0,
                            {false, ANGLE, 0.0},
                            {false, ANGLE, 0.0},
                            {false, RATE, 0.0},
+                           {false, THROTTLE, 0.0},
+                           {false, THROTTLE, 0.0},
                            {false, THROTTLE, 0.0}};
   control_t offboard_command_ = {0,
                                  {false, ANGLE, 0.0},
                                  {false, ANGLE, 0.0},
                                  {false, RATE, 0.0},
+                                 {false, THROTTLE, 0.0},
+                                 {false, THROTTLE, 0.0},
                                  {false, THROTTLE, 0.0}};
   control_t combined_command_ = {0,
                                  {false, ANGLE, 0.0},
                                  {false, ANGLE, 0.0},
                                  {false, RATE, 0.0},
+                                 {false, THROTTLE, 0.0},
+                                 {false, THROTTLE, 0.0},
                                  {false, THROTTLE, 0.0}};
 
   control_t multirotor_failsafe_command_ = {0,
                                             {true, ANGLE, 0.0},
                                             {true, ANGLE, 0.0},
                                             {true, RATE, 0.0},
+                                            {true, THROTTLE, 0.0},
+                                            {true, THROTTLE, 0.0},
                                             {true, THROTTLE, 0.0}};
   control_t fixedwing_failsafe_command_ = {0,
                                            {true, PASSTHROUGH, 0.0},
                                            {true, PASSTHROUGH, 0.0},
                                            {true, PASSTHROUGH, 0.0},
-                                           {true, THROTTLE, 0.0}};
+                                           {true, PASSTHROUGH, 0.0},
+                                           {true, PASSTHROUGH, 0.0},
+                                           {true, PASSTHROUGH, 0.0}};
   // clang-format on
 
   typedef enum
@@ -119,10 +140,12 @@ class CommandManager : public ParamListenerInterface
 
   enum MuxChannel
   {
-    MUX_X,
-    MUX_Y,
-    MUX_Z,
-    MUX_F,
+    MUX_QX,
+    MUX_QY,
+    MUX_QZ,
+    MUX_FX,
+    MUX_FY,
+    MUX_FZ,
   };
 
   typedef struct
@@ -138,7 +161,8 @@ class CommandManager : public ParamListenerInterface
   ROSflight & RF_;
 
   bool new_command_;
-  bool rc_override_;
+  bool rc_throttle_override_;
+  bool rc_attitude_override_;
 
   control_t & failsafe_command_;
 
@@ -157,6 +181,8 @@ class CommandManager : public ParamListenerInterface
   void init();
   bool run();
   bool rc_override_active();
+  bool rc_throttle_override_active();
+  bool rc_attitude_override_active();
   bool offboard_control_active();
   void set_new_offboard_command(control_t new_offboard_command);
   void set_new_rc_command(control_t new_rc_command);

include/controller.h

@@ -51,21 +51,26 @@ class Controller : public ParamListenerInterface
 public:
   struct Output
   {
-    float F;
-    float x;
-    float y;
-    float z;
+    float Fx;
+    float Fy;
+    float Fz;
+    float Qx;
+    float Qy;
+    float Qz;
   };
 
   Controller(ROSflight & rf);
 
   inline const Output & output() const { return output_; }
+  inline float max_thrust() const { return max_thrust_; }
 
   void init();
   void run();
 
+  void calculate_max_thrust();
   void calculate_equilbrium_torque_from_rc();
   void param_change_callback(uint16_t param_id) override;
+  bool is_throttle_high(float threshold);
 
 private:
   class PID
@@ -92,8 +97,8 @@ class Controller : public ParamListenerInterface
 
   ROSflight & RF_;
 
-  turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state,
-                                  const control_t & command, bool update_integrators);
+  Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
+                                   const control_t & command, bool update_integrators);
 
   Output output_;
 
@@ -103,6 +108,8 @@ class Controller : public ParamListenerInterface
   PID pitch_rate_;
   PID yaw_rate_;
 
+  float max_thrust_;
+
   uint64_t prev_time_us_;
 };
 

include/interface/comm_link.h

@@ -84,10 +84,12 @@ class CommLinkInterface
     };
 
     Mode mode;
-    Channel x;
-    Channel y;
-    Channel z;
-    Channel F;
+    Channel Qx;
+    Channel Qy;
+    Channel Qz;
+    Channel Fx;
+    Channel Fy;
+    Channel Fz;
   };
 
   struct AuxCommand