From a45aa0804aa9035ab7754729a46df5f5f7101e34 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Mon, 5 Aug 2024 14:18:35 -0600
Subject: [PATCH 01/41] Adds option to use mixer based on motor positions and
 motor parameters.

---
 CMakeLists.txt  |  3 +++
 include/mixer.h |  6 +++++
 src/mixer.cpp   | 69 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 78 insertions(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3efea7c0..c2586cfe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,9 @@ include_directories(
     comms/mavlink/v1.0/rosflight
 )
 
+include_directories(${EIGEN3_INCLUDE_DIRS})
+include_directories(/usr/include/eigen3)
+
 file(GLOB_RECURSE ROSFLIGHT_SOURCES
     "src/*.cpp"
     "lib/turbomath/turbomath.cpp"
diff --git a/include/mixer.h b/include/mixer.h
index 8e2efc0c..1725b933 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -34,6 +34,8 @@
 
 #include "interface/param_listener.h"
 
+#include <Eigen/QR>
+
 #include <cstdbool>
 #include <cstdint>
 
@@ -230,6 +232,8 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
 
+  mixer_t correct_mixer_;
+
   const mixer_t * mixer_to_use_;
 
   const mixer_t* array_of_mixers_[NUM_MIXERS] = {
@@ -261,6 +265,8 @@ class Mixer : public ParamListenerInterface
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
   inline const float * get_outputs() const { return raw_outputs_; }
+
+  void calculate_mixer_values();
 };
 
 } // namespace rosflight_firmware
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 14f57821..fe266049 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -76,6 +76,11 @@ void Mixer::init_mixing()
     mixer_to_use_ = nullptr;
   } else {
     mixer_to_use_ = array_of_mixers_[mixer_choice];
+
+    if (mixer_to_use_ == *custom_mixing) {
+      calculate_mixer_values();
+      mixer_to_use_ = *correct_mixer_;
+    }
   }
 
   init_PWM();
@@ -86,6 +91,70 @@ void Mixer::init_mixing()
   }
 }
 
+void Mixer::calculate_mixer_values()
+{
+  // Define parameters for the mixer
+  float C_T = 1.0; // Thrust coefficient
+  float J = 1.0;   // Advance ratio
+  float C_Q0 = 1.0; 
+  float C_Q1 = 1.0; 
+  float C_Q2 = 1.0; 
+  float D = 1.0;   // Propeller diameter
+  float C_Q = 1.0; // Torque coefficient
+  int num_motors = 4; // Number of motors
+
+  // Motor position parameters
+  float l[num_motors] = {1.0, 1.0, 1.0, 1.0}; // Distance from center of mass to motor
+  float psi[num_motors] = {M_PI_2, 3*M_PI_2, 5*M_PI_2, 7*M_PI_2}; // Angle of motor from x-axis
+  float Va = 1.0; // Airspeed
+  float R = 1.0; // Motor resistance
+  float rho = 1.225; // Air density
+  float K_V = 1.0; // Motor velocity constant
+  float K_Q = K_V; // Motor torque constant
+  float i_0 = 1.0; // Motor no-load current
+
+  int roll_dir = 1.0;
+  int pitch_dir = 1.0;
+  int yaw_dir = 1.0;
+
+  // Create the mixing matrix as defined in Ch. 14
+  Eigen::MatrixXf M(4, num_motors);
+  for (int i = 0; i < num_motors; ++i) {
+    // Determine the direction of the roll moment generated by the motor
+    int roll_dir = (cos(psi[i]) < 0.0) ? 1.0 : -1.0;
+    int pitch_dir = (sin(psi[i]) < 0.0) ? -1.0 : 1.0;
+    int yaw_dir = (i % 2 == 0) ? 1.0 : 1.0;
+
+    M(0, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * C_T;
+    M(1, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * roll_dir * C_T * D * l[i] * cos(psi[i]);
+    M(2, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * pitch_dir * C_T * D * l[i] * sin(psi[i]);
+    M(3, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * yaw_dir * C_Q * D;
+  }
+
+  // Take the pseudoinverse of the matrix
+  Eigen::MatrixXf M_pinv = M.completeOrthogonalDecomposition().pseudoInverse();
+  
+  // Fill in the actual mixing matrix used in the mixer
+  for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    if (i < num_motors) {
+      correct_mixer_.default_pwm_rate[i] = 490;
+      correct_mixer_.output_type[i] = M;
+      correct_mixer_.F[i] = M_pinv(0, i);
+      correct_mixer_.x[i] = M_pinv(1, i);
+      correct_mixer_.y[i] = M_pinv(2, i);
+      correct_mixer_.z[i] = M_pinv(3, i);
+    }
+    else {
+      correct_mixer_.default_pwm_rate[i] = 50;
+      correct_mixer_.output_type[i] = NONE;
+      correct_mixer_.F[i] = 0.0;
+      correct_mixer_.x[i] = 0.0;
+      correct_mixer_.y[i] = 0.0;
+      correct_mixer_.z[i] = 0.0;
+    }
+  }
+}
+
 void Mixer::init_PWM()
 {
   if (mixer_to_use_ != nullptr) {

From 8ddf21ea6b9e8d11cabacf0175c5486c151e47ec Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Mon, 5 Aug 2024 14:18:35 -0600
Subject: [PATCH 02/41] Adds option to use mixer based on motor positions and
 motor parameters.

---
 CMakeLists.txt  |  3 +++
 include/mixer.h |  8 +++++-
 src/mixer.cpp   | 69 +++++++++++++++++++++++++++++++++++++++++++++++++
 3 files changed, 79 insertions(+), 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3efea7c0..c2586cfe 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -40,6 +40,9 @@ include_directories(
     comms/mavlink/v1.0/rosflight
 )
 
+include_directories(${EIGEN3_INCLUDE_DIRS})
+include_directories(/usr/include/eigen3)
+
 file(GLOB_RECURSE ROSFLIGHT_SOURCES
     "src/*.cpp"
     "lib/turbomath/turbomath.cpp"
diff --git a/include/mixer.h b/include/mixer.h
index 8e2efc0c..3bb1a8b0 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -34,6 +34,8 @@
 
 #include "interface/param_listener.h"
 
+#include <eigen3/Eigen/QR>
+
 #include <cstdbool>
 #include <cstdint>
 
@@ -230,6 +232,8 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
 
+  mixer_t correct_mixer_;
+
   const mixer_t * mixer_to_use_;
 
   const mixer_t* array_of_mixers_[NUM_MIXERS] = {
@@ -247,7 +251,7 @@ class Mixer : public ParamListenerInterface
     &passthrough_mixing,
     &fixedwing_vtail_mixing,
     &quadplane_mixing,
-    &custom_mixing
+    &custom_mixing,
   };
 
   // clang-format on
@@ -261,6 +265,8 @@ class Mixer : public ParamListenerInterface
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
   inline const float * get_outputs() const { return raw_outputs_; }
+
+  void calculate_mixer_values();
 };
 
 } // namespace rosflight_firmware
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 14f57821..fe266049 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -76,6 +76,11 @@ void Mixer::init_mixing()
     mixer_to_use_ = nullptr;
   } else {
     mixer_to_use_ = array_of_mixers_[mixer_choice];
+
+    if (mixer_to_use_ == *custom_mixing) {
+      calculate_mixer_values();
+      mixer_to_use_ = *correct_mixer_;
+    }
   }
 
   init_PWM();
@@ -86,6 +91,70 @@ void Mixer::init_mixing()
   }
 }
 
+void Mixer::calculate_mixer_values()
+{
+  // Define parameters for the mixer
+  float C_T = 1.0; // Thrust coefficient
+  float J = 1.0;   // Advance ratio
+  float C_Q0 = 1.0; 
+  float C_Q1 = 1.0; 
+  float C_Q2 = 1.0; 
+  float D = 1.0;   // Propeller diameter
+  float C_Q = 1.0; // Torque coefficient
+  int num_motors = 4; // Number of motors
+
+  // Motor position parameters
+  float l[num_motors] = {1.0, 1.0, 1.0, 1.0}; // Distance from center of mass to motor
+  float psi[num_motors] = {M_PI_2, 3*M_PI_2, 5*M_PI_2, 7*M_PI_2}; // Angle of motor from x-axis
+  float Va = 1.0; // Airspeed
+  float R = 1.0; // Motor resistance
+  float rho = 1.225; // Air density
+  float K_V = 1.0; // Motor velocity constant
+  float K_Q = K_V; // Motor torque constant
+  float i_0 = 1.0; // Motor no-load current
+
+  int roll_dir = 1.0;
+  int pitch_dir = 1.0;
+  int yaw_dir = 1.0;
+
+  // Create the mixing matrix as defined in Ch. 14
+  Eigen::MatrixXf M(4, num_motors);
+  for (int i = 0; i < num_motors; ++i) {
+    // Determine the direction of the roll moment generated by the motor
+    int roll_dir = (cos(psi[i]) < 0.0) ? 1.0 : -1.0;
+    int pitch_dir = (sin(psi[i]) < 0.0) ? -1.0 : 1.0;
+    int yaw_dir = (i % 2 == 0) ? 1.0 : 1.0;
+
+    M(0, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * C_T;
+    M(1, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * roll_dir * C_T * D * l[i] * cos(psi[i]);
+    M(2, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * pitch_dir * C_T * D * l[i] * sin(psi[i]);
+    M(3, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * yaw_dir * C_Q * D;
+  }
+
+  // Take the pseudoinverse of the matrix
+  Eigen::MatrixXf M_pinv = M.completeOrthogonalDecomposition().pseudoInverse();
+  
+  // Fill in the actual mixing matrix used in the mixer
+  for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    if (i < num_motors) {
+      correct_mixer_.default_pwm_rate[i] = 490;
+      correct_mixer_.output_type[i] = M;
+      correct_mixer_.F[i] = M_pinv(0, i);
+      correct_mixer_.x[i] = M_pinv(1, i);
+      correct_mixer_.y[i] = M_pinv(2, i);
+      correct_mixer_.z[i] = M_pinv(3, i);
+    }
+    else {
+      correct_mixer_.default_pwm_rate[i] = 50;
+      correct_mixer_.output_type[i] = NONE;
+      correct_mixer_.F[i] = 0.0;
+      correct_mixer_.x[i] = 0.0;
+      correct_mixer_.y[i] = 0.0;
+      correct_mixer_.z[i] = 0.0;
+    }
+  }
+}
+
 void Mixer::init_PWM()
 {
   if (mixer_to_use_ != nullptr) {

From 70e7c9bb34d8f928cc07bac0cad8e8c9b777f465 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 8 Aug 2024 13:33:20 -0600
Subject: [PATCH 03/41] changed controller to output forces and torques

---
 include/controller.h |  5 ++-
 src/controller.cpp   | 77 +++++++++++++++++++++++++++++++++++---------
 2 files changed, 66 insertions(+), 16 deletions(-)

diff --git a/include/controller.h b/include/controller.h
index b8a9e9d1..92d4409c 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -64,6 +64,7 @@ class Controller : public ParamListenerInterface
   void init();
   void run();
 
+  void calculate_max_thrust();
   void calculate_equilbrium_torque_from_rc();
   void param_change_callback(uint16_t param_id) override;
 
@@ -92,7 +93,7 @@ class Controller : public ParamListenerInterface
 
   ROSflight & RF_;
 
-  turbomath::Vector run_pid_loops(uint32_t dt, const Estimator::State & state,
+  Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
                                   const control_t & command, bool update_integrators);
 
   Output output_;
@@ -103,6 +104,8 @@ class Controller : public ParamListenerInterface
   PID pitch_rate_;
   PID yaw_rate_;
 
+  float max_thrust_;
+
   uint64_t prev_time_us_;
 };
 
diff --git a/src/controller.cpp b/src/controller.cpp
index 05e6c16f..16e766e4 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -38,6 +38,7 @@
 
 #include <cstdbool>
 #include <cstdint>
+#include <iostream>
 
 namespace rosflight_firmware
 {
@@ -49,25 +50,56 @@ void Controller::init()
 {
   prev_time_us_ = 0;
 
-  float max = RF_.params_.get_param_float(PARAM_MAX_COMMAND);
-  float min = -max;
+  // Calculate the max thrust to convert from throttle setting to thrust
+  calculate_max_thrust();
+  
+  // Don't saturate the torque values
+  float max_torque = INFINITY;
+  float min = -max_torque;
+
   float tau = RF_.params_.get_param_float(PARAM_PID_TAU);
 
   roll_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_P),
              RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_I),
-             RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max, min, tau);
+             RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max_torque, min, tau);
   roll_rate_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_P),
                   RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_I),
-                  RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max, min, tau);
+                  RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max_torque, min, tau);
   pitch_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_P),
               RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_I),
-              RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max, min, tau);
+              RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max_torque, min, tau);
   pitch_rate_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_P),
                    RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_I),
-                   RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max, min, tau);
+                   RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max_torque, min, tau);
   yaw_rate_.init(RF_.params_.get_param_float(PARAM_PID_YAW_RATE_P),
                  RF_.params_.get_param_float(PARAM_PID_YAW_RATE_I),
-                 RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max, min, tau);
+                 RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max_torque, min, tau);
+}
+
+void Controller::calculate_max_thrust()
+{
+  float R = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE);
+  float D = RF_.params_.get_param_float(PARAM_PROP_DIAMETER);
+  float rho = RF_.params_.get_param_float(PARAM_AIR_DENSITY);
+  float CQ = RF_.params_.get_param_float(PARAM_PROP_CQ);
+  float CT = RF_.params_.get_param_float(PARAM_PROP_CT);
+  float KV = RF_.params_.get_param_float(PARAM_MOTOR_KV);
+  float i0 = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT);
+  float KQ = KV;
+  int num_motors = RF_.params_.get_param_int(PARAM_NUM_MOTORS);
+
+  float V_max = RF_.params_.get_param_float(PARAM_VOLT_MAX);
+  float a = R * rho * pow(D, 5.0) * CQ / (4 * pow(M_PI, 2.0) * KQ);
+  float b = KV;
+  float c = i0 * R - V_max;
+
+  // Using Eq. 4.19 and setting equal to the equation at the beginning of 4.3 in Small Unmanned Aircraft
+  // We only need the positive root. Since a and b positive, sqrt term must be positive
+  float omega = (-b + sqrt(pow(b, 2.0) - 4 * a * c)) / (2 * a); 
+
+  // Calculate the max thrust from Eq in 4.3 of Small Unmanned Aircraft
+  // Note that the equation is for a single motor, so to calculate max thrust, we need to multiply by the number of motors
+  max_thrust_ = rho * pow(D, 4.0) * CT * pow(omega, 2.0) / (4 * pow(M_PI, 2.0)) * num_motors;
 }
 
 void Controller::run()
@@ -91,16 +123,17 @@ void Controller::run()
     && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000;
 
   // Run the PID loops
-  turbomath::Vector pid_output = run_pid_loops(
+  Controller::Output pid_output = run_pid_loops(
     dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
 
   // Add feedforward torques
   output_.x = pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
   output_.y = pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE);
   output_.z = pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE);
-  output_.F = RF_.command_manager_.combined_control().F.value;
+  output_.F = pid_output.F;
 }
 
+// TODO: Fix this to output equilibrium torque (not normalized values)
 void Controller::calculate_equilbrium_torque_from_rc()
 {
   // Make sure we are disarmed
@@ -129,10 +162,11 @@ void Controller::calculate_equilbrium_torque_from_rc()
     // dt is zero, so what this really does is applies the P gain with the settings
     // your RC transmitter, which if it flies level is a really good guess for
     // the static offset torques
-    turbomath::Vector pid_output =
+    Controller::Output pid_output =
       run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false);
 
     // the output from the controller is going to be the static offsets
+    // TODO: Should we be adding the equilibrium torques to the existing ones?
     RF_.params_.set_param_float(PARAM_X_EQ_TORQUE,
                                 pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE));
     RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE,
@@ -168,7 +202,15 @@ void Controller::param_change_callback(uint16_t param_id)
     case PARAM_PID_YAW_RATE_P:
     case PARAM_PID_YAW_RATE_I:
     case PARAM_PID_YAW_RATE_D:
-    case PARAM_MAX_COMMAND:
+    case PARAM_MOTOR_RESISTANCE:
+    case PARAM_PROP_DIAMETER:
+    case PARAM_AIR_DENSITY:
+    case PARAM_PROP_CQ:
+    case PARAM_PROP_CT:
+    case PARAM_MOTOR_KV:
+    case PARAM_NO_LOAD_CURRENT:
+    case PARAM_VOLT_MAX:
+    case PARAM_NUM_MOTORS:
     case PARAM_PID_TAU:
       init();
       break;
@@ -178,11 +220,11 @@ void Controller::param_change_callback(uint16_t param_id)
   }
 }
 
-turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state,
+Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state,
                                             const control_t & command, bool update_integrators)
 {
   // Based on the control types coming from the command manager, run the appropriate PID loops
-  turbomath::Vector out;
+  Controller::Output out;
 
   float dt = 1e-6 * dt_us;
 
@@ -213,6 +255,11 @@ turbomath::Vector Controller::run_pid_loops(uint32_t dt_us, const Estimator::Sta
     out.z = command.z.value;
   }
 
+  // THROTTLE
+  // TODO: This works if the input is a throttle setting, but not if the input is a thrust
+  // TODO: Add something that will saturate input RC command to a MAX_THROTTLE parameter, so you can maintain controllability
+  out.F = command.F.value * max_thrust_;
+
   return out;
 }
 
@@ -244,7 +291,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
   if (dt > 0.0001f) {
     // calculate D term (use dirty derivative if we don't have access to a measurement of the
     // derivative) The dirty derivative is a sort of low-pass filtered version of the derivative.
-    //// (Include reference to Dr. Beard's notes here)
+    //// TODO: (Include reference to Dr. Beard's notes here)
     differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_
       + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_);
     xdot = differentiator_;
@@ -281,7 +328,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator,
   float u = p_term - d_term + i_term;
 
   // Integrator anti-windup
-  //// Include reference to Dr. Beard's notes here
+  //// TODO: Include reference to Dr. Beard's notes here
   float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u;
   if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) {
     integrator_ = (u_sat - p_term + d_term) / ki_;

From f41077f6fa68af2b4ea226a3925946ee292e0ee5 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 8 Aug 2024 13:43:07 -0600
Subject: [PATCH 04/41] added parameters for the new mixing matrix calculations

---
 include/param.h | 35 +++++++++++++++++++++++++++++++++--
 src/param.cpp   | 49 ++++++++++++++++++++++++++++++++++++++++---------
 2 files changed, 73 insertions(+), 11 deletions(-)

diff --git a/include/param.h b/include/param.h
index 03d42067..b0b43a58 100644
--- a/include/param.h
+++ b/include/param.h
@@ -62,6 +62,39 @@ enum : uint16_t
   PARAM_BAUD_RATE = 0,
   PARAM_SERIAL_DEVICE,
 
+  PARAM_AIR_DENSITY,
+
+  PARAM_NUM_MOTORS,
+  PARAM_MOTOR_RESISTANCE,
+  PARAM_MOTOR_KV,
+  PARAM_NO_LOAD_CURRENT,
+  PARAM_PROP_DIAMETER,
+  PARAM_PROP_CT,
+  PARAM_PROP_CQ,
+  PARAM_VOLT_MAX,
+
+  PARAM_MOTOR_0_POS,
+  PARAM_MOTOR_1_POS,
+  PARAM_MOTOR_2_POS,
+  PARAM_MOTOR_3_POS,
+  PARAM_MOTOR_4_POS,
+  PARAM_MOTOR_5_POS,
+  PARAM_MOTOR_6_POS,
+  PARAM_MOTOR_7_POS,
+  PARAM_MOTOR_8_POS,
+  PARAM_MOTOR_9_POS,
+
+  PARAM_MOTOR_0_PSI,
+  PARAM_MOTOR_1_PSI,
+  PARAM_MOTOR_2_PSI,
+  PARAM_MOTOR_3_PSI,
+  PARAM_MOTOR_4_PSI,
+  PARAM_MOTOR_5_PSI,
+  PARAM_MOTOR_6_PSI,
+  PARAM_MOTOR_7_PSI,
+  PARAM_MOTOR_8_PSI,
+  PARAM_MOTOR_9_PSI,
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
@@ -70,8 +103,6 @@ enum : uint16_t
   /********************************/
   /*** CONTROLLER CONFIGURATION ***/
   /********************************/
-  PARAM_MAX_COMMAND,
-
   PARAM_PID_ROLL_RATE_P,
   PARAM_PID_ROLL_RATE_I,
   PARAM_PID_ROLL_RATE_D,
diff --git a/src/param.cpp b/src/param.cpp
index 6bd0eeb3..4bd0ef62 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -108,6 +108,39 @@ void Params::set_defaults(void)
   init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600
   init_param_int(PARAM_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3
 
+  init_param_float(PARAM_AIR_DENSITY, "AIR_DENSITY", 1.225f); // Density of the air (kg/m^3) | 0 | 1000.0
+
+  init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // number of vertical-facing motors on the vehicle | 1 | 8
+  init_param_float(PARAM_MOTOR_RESISTANCE, "MOTOR_RESISTANCE", 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0
+  init_param_float(PARAM_MOTOR_KV, "MOTOR_KV", 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0
+  init_param_float(PARAM_NO_LOAD_CURRENT, "NO_LOAD_CURRENT", 1.5); // No-load current of the motor in amps | 0 | 1000.0
+  init_param_float(PARAM_PROP_DIAMETER, "PROP_DIAMETER", 0.381f); // Diameter of the propeller in meters | 0 | 1.0
+  init_param_float(PARAM_PROP_CT, "PROP_CT", 0.075f); // Thrust coefficient of the propeller | 0 | 100.0
+  init_param_float(PARAM_PROP_CQ, "PROP_CQ", 0.0045f); // Torque coefficient of the propeller | 0 | 100.0
+  init_param_float(PARAM_VOLT_MAX, "VOLT_MAX", 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0
+
+  init_param_float(PARAM_MOTOR_0_POS, "MOTOR_0_POS", 0.25f); // Radial position of motor 0 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_1_POS, "MOTOR_1_POS", 0.25f); // Radial position of motor 1 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_2_POS, "MOTOR_2_POS", 0.25f); // Radial position of motor 2 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_3_POS, "MOTOR_3_POS", 0.25f); // Radial position of motor 3 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_4_POS, "MOTOR_4_POS", 0.0f); // Radial position of motor 4 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_5_POS, "MOTOR_5_POS", 0.0f); // Radial position of motor 5 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_6_POS, "MOTOR_6_POS", 0.0f); // Radial position of motor 6 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_7_POS, "MOTOR_7_POS", 0.0f); // Radial position of motor 7 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_8_POS, "MOTOR_8_POS", 0.0f); // Radial position of motor 8 from the center of mass (m) | 0.0 | 1000.0
+  init_param_float(PARAM_MOTOR_9_POS, "MOTOR_9_POS", 0.0f); // Radial position of motor 9 from the center of mass (m) | 0.0 | 1000.0
+
+  init_param_float(PARAM_MOTOR_0_PSI, "MOTOR_0_PSI", M_PI_4); // Angular position of motor 0 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_1_PSI, "MOTOR_1_PSI", 3 * M_PI_4); // Angular position of motor 1 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_2_PSI, "MOTOR_2_PSI", 5 * M_PI_4); // Angular position of motor 2 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_3_PSI, "MOTOR_3_PSI", 7 * M_PI_4); // Angular position of motor 3 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_4_PSI, "MOTOR_4_PSI", 0.0f); // Angular position of motor 4 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_5_PSI, "MOTOR_5_PSI", 0.0f); // Angular position of motor 5 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_6_PSI, "MOTOR_6_PSI", 0.0f); // Angular position of motor 6 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_7_PSI, "MOTOR_7_PSI", 0.0f); // Angular position of motor 7 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_8_PSI, "MOTOR_8_PSI", 0.0f); // Angular position of motor 8 from the body from x axis (rad) | 0.0 | 6.28319
+  init_param_float(PARAM_MOTOR_9_PSI, "MOTOR_9_PSI", 0.0f); // Angular position of motor 9 from the body from x axis (rad) | 0.0 | 6.28319
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
@@ -116,27 +149,25 @@ void Params::set_defaults(void)
   /********************************/
   /*** CONTROLLER CONFIGURATION ***/
   /********************************/
-  init_param_float(PARAM_MAX_COMMAND, "PARAM_MAX_CMD", 1.0); // saturation point for PID controller output | 0 | 1.0
-
-  init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 0.070f); // Roll Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 1.5f); // Roll Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_RATE_I, "PID_ROLL_RATE_I", 0.000f); // Roll Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_RATE_D, "PID_ROLL_RATE_D", 0.000f); // Roll Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_PITCH_RATE_P, "PID_PITCH_RATE_P", 0.070f);  // Pitch Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_RATE_P, "PID_PITCH_RATE_P", 1.5f);  // Pitch Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_RATE_I, "PID_PITCH_RATE_I", 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_RATE_D, "PID_PITCH_RATE_D", 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f);   // Yaw Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 5.5f);   // Yaw Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_YAW_RATE_I, "PID_YAW_RATE_I", 0.0f);  // Yaw Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_YAW_RATE_D, "PID_YAW_RATE_D", 0.0f);  // Yaw Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f);   // Roll Angle Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 1.2f);   // Roll Angle Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f);   // Roll Angle Integral Gain | 0.0 | 1000.0
-  init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f);  // Roll Angle Derivative Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.4f);  // Roll Angle Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f);  // Pitch Angle Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 1.2f);  // Pitch Angle Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f);  // Pitch Angle Integral Gain | 0.0 | 1000.0
-  init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.4f); // Pitch Angle Derivative Gain | 0.0 | 1000.0
 
   init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0
   init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0

From 2caff0982daa8316aff5d663b0e8e92b2c16432f Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 8 Aug 2024 15:08:33 -0600
Subject: [PATCH 05/41] Added custom mixer that uses Ch. 14 mixer equations.
 Also uses Ch. 4 and 14 equations to calculate the max thrust given a throttle
 setting (since RC commands come in as throttle settings, not forces).

---
 include/mixer.h |  22 ++++++-
 src/mixer.cpp   | 154 +++++++++++++++++++++++++++++++++---------------
 2 files changed, 126 insertions(+), 50 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 3bb1a8b0..abd98a82 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -63,7 +63,7 @@ class Mixer : public ParamListenerInterface
     TRICOPTER = 9,
     FIXEDWING = 10,
     PASSTHROUGH = 11,
-    VTAIL = 12,
+    INVERTED_VTAIL = 12,
     QUADPLANE = 13,
     CUSTOM = 14,
     NUM_MIXERS,
@@ -200,7 +200,7 @@ class Mixer : public ParamListenerInterface
     {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
     {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
 
-  const mixer_t fixedwing_vtail_mixing = {
+  const mixer_t fixedwing_inverted_vtail_mixing = {
     {   S, NONE, NONE,     S,    S,    M, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
     {  50,   50,   50,    50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
     {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
@@ -249,18 +249,34 @@ class Mixer : public ParamListenerInterface
     &tricopter_mixing,
     &fixedwing_mixing,
     &passthrough_mixing,
-    &fixedwing_vtail_mixing,
+    &fixedwing_inverted_vtail_mixing,
     &quadplane_mixing,
     &custom_mixing,
   };
 
   // clang-format on
 
+  // Define parameters for the mixer
+  float R_; // Motor resistance
+  float rho_; // Air density
+  float K_V_; // Motor back-emf constant
+  float K_Q_ = 0.01706; // Motor torque constant
+  float i_0_; // Motor no-load current
+  float D_;   // Propeller diameter
+  float C_T_; // Thrust coefficient
+  float C_Q_; // Torque coefficient
+  int num_motors_; // Number of motors
+  float V_max_; // Maximum battery voltage
+  float l_[NUM_MIXER_OUTPUTS]; // Radial distance from center of mass to motor
+  float psi_[NUM_MIXER_OUTPUTS]; // Angle of motor from body x-axis
+
+
 public:
   Mixer(ROSflight & _rf);
   void init();
   void init_PWM();
   void init_mixing();
+  void update_parameters();
   void mix_output();
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
diff --git a/src/mixer.cpp b/src/mixer.cpp
index fe266049..993cc8eb 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -34,6 +34,8 @@
 #include "rosflight.h"
 
 #include <cstdint>
+#include <iostream>
+// #include "eigen3/Eigen/Dense"
 
 namespace rosflight_firmware
 {
@@ -49,6 +51,35 @@ void Mixer::param_change_callback(uint16_t param_id)
 {
   switch (param_id) {
     case PARAM_MIXER:
+    case PARAM_MOTOR_RESISTANCE:
+    case PARAM_AIR_DENSITY:
+    case PARAM_MOTOR_KV:
+    case PARAM_NO_LOAD_CURRENT:
+    case PARAM_PROP_DIAMETER:
+    case PARAM_PROP_CT:
+    case PARAM_PROP_CQ:
+    case PARAM_NUM_MOTORS:
+    case PARAM_VOLT_MAX:
+    case PARAM_MOTOR_0_POS:
+    case PARAM_MOTOR_1_POS:
+    case PARAM_MOTOR_2_POS:
+    case PARAM_MOTOR_3_POS:
+    case PARAM_MOTOR_4_POS:
+    case PARAM_MOTOR_5_POS:
+    case PARAM_MOTOR_6_POS:
+    case PARAM_MOTOR_7_POS:
+    case PARAM_MOTOR_8_POS:
+    case PARAM_MOTOR_9_POS:
+    case PARAM_MOTOR_0_PSI:
+    case PARAM_MOTOR_1_PSI:
+    case PARAM_MOTOR_2_PSI:
+    case PARAM_MOTOR_3_PSI:
+    case PARAM_MOTOR_4_PSI:
+    case PARAM_MOTOR_5_PSI:
+    case PARAM_MOTOR_6_PSI:
+    case PARAM_MOTOR_7_PSI:
+    case PARAM_MOTOR_8_PSI:
+    case PARAM_MOTOR_9_PSI:
       init_mixing();
       break;
     case PARAM_MOTOR_PWM_SEND_RATE:
@@ -63,6 +94,9 @@ void Mixer::param_change_callback(uint16_t param_id)
 
 void Mixer::init_mixing()
 {
+  // Update parameters
+  update_parameters(); 
+
   // clear the invalid mixer error
   RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER);
 
@@ -77,9 +111,10 @@ void Mixer::init_mixing()
   } else {
     mixer_to_use_ = array_of_mixers_[mixer_choice];
 
-    if (mixer_to_use_ == *custom_mixing) {
+    if (mixer_to_use_ == &custom_mixing) {
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Calculating custom mixer values...");
       calculate_mixer_values();
-      mixer_to_use_ = *correct_mixer_;
+      mixer_to_use_ = &correct_mixer_;
     }
   }
 
@@ -91,60 +126,71 @@ void Mixer::init_mixing()
   }
 }
 
+void Mixer::update_parameters() {
+  R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE);
+  rho_ = RF_.params_.get_param_float(PARAM_AIR_DENSITY);
+  K_V_ = RF_.params_.get_param_float(PARAM_MOTOR_KV);
+  K_Q_ = K_V_;
+  i_0_ = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT);
+  D_ = RF_.params_.get_param_float(PARAM_PROP_DIAMETER);
+  C_T_ = RF_.params_.get_param_float(PARAM_PROP_CT);
+  C_Q_ = RF_.params_.get_param_float(PARAM_PROP_CQ);
+  num_motors_ = RF_.params_.get_param_int(PARAM_NUM_MOTORS);
+  V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX);
+
+  l_[0] = RF_.params_.get_param_float(PARAM_MOTOR_0_POS);
+  l_[1] = RF_.params_.get_param_float(PARAM_MOTOR_1_POS);
+  l_[2] = RF_.params_.get_param_float(PARAM_MOTOR_2_POS);
+  l_[3] = RF_.params_.get_param_float(PARAM_MOTOR_3_POS);
+  l_[4] = RF_.params_.get_param_float(PARAM_MOTOR_4_POS);
+  l_[5] = RF_.params_.get_param_float(PARAM_MOTOR_5_POS);
+  l_[6] = RF_.params_.get_param_float(PARAM_MOTOR_6_POS);
+  l_[7] = RF_.params_.get_param_float(PARAM_MOTOR_7_POS);
+  l_[8] = RF_.params_.get_param_float(PARAM_MOTOR_8_POS);
+  l_[9] = RF_.params_.get_param_float(PARAM_MOTOR_9_POS);
+
+  psi_[0] = RF_.params_.get_param_float(PARAM_MOTOR_0_PSI);
+  psi_[1] = RF_.params_.get_param_float(PARAM_MOTOR_1_PSI);
+  psi_[2] = RF_.params_.get_param_float(PARAM_MOTOR_2_PSI);
+  psi_[3] = RF_.params_.get_param_float(PARAM_MOTOR_3_PSI);
+  psi_[4] = RF_.params_.get_param_float(PARAM_MOTOR_4_PSI);
+  psi_[5] = RF_.params_.get_param_float(PARAM_MOTOR_5_PSI);
+  psi_[6] = RF_.params_.get_param_float(PARAM_MOTOR_6_PSI);
+  psi_[7] = RF_.params_.get_param_float(PARAM_MOTOR_7_PSI);
+  psi_[8] = RF_.params_.get_param_float(PARAM_MOTOR_8_PSI);
+  psi_[9] = RF_.params_.get_param_float(PARAM_MOTOR_9_PSI);
+}
+
 void Mixer::calculate_mixer_values()
 {
-  // Define parameters for the mixer
-  float C_T = 1.0; // Thrust coefficient
-  float J = 1.0;   // Advance ratio
-  float C_Q0 = 1.0; 
-  float C_Q1 = 1.0; 
-  float C_Q2 = 1.0; 
-  float D = 1.0;   // Propeller diameter
-  float C_Q = 1.0; // Torque coefficient
-  int num_motors = 4; // Number of motors
-
-  // Motor position parameters
-  float l[num_motors] = {1.0, 1.0, 1.0, 1.0}; // Distance from center of mass to motor
-  float psi[num_motors] = {M_PI_2, 3*M_PI_2, 5*M_PI_2, 7*M_PI_2}; // Angle of motor from x-axis
-  float Va = 1.0; // Airspeed
-  float R = 1.0; // Motor resistance
-  float rho = 1.225; // Air density
-  float K_V = 1.0; // Motor velocity constant
-  float K_Q = K_V; // Motor torque constant
-  float i_0 = 1.0; // Motor no-load current
-
-  int roll_dir = 1.0;
-  int pitch_dir = 1.0;
-  int yaw_dir = 1.0;
-
   // Create the mixing matrix as defined in Ch. 14
-  Eigen::MatrixXf M(4, num_motors);
-  for (int i = 0; i < num_motors; ++i) {
-    // Determine the direction of the roll moment generated by the motor
-    int roll_dir = (cos(psi[i]) < 0.0) ? 1.0 : -1.0;
-    int pitch_dir = (sin(psi[i]) < 0.0) ? -1.0 : 1.0;
-    int yaw_dir = (i % 2 == 0) ? 1.0 : 1.0;
-
-    M(0, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * C_T;
-    M(1, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * roll_dir * C_T * D * l[i] * cos(psi[i]);
-    M(2, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * pitch_dir * C_T * D * l[i] * sin(psi[i]);
-    M(3, i) = rho * pow(D, 4.0) / (4 * pow(M_PI), 2.0) * yaw_dir * C_Q * D;
+  Eigen::MatrixXf mixer_matrix(4, num_motors_);
+  for (int i = 0; i < num_motors_; ++i) {
+    // Determine the direction of the yaw moment generated by the motor i 
+    // TODO: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
+    int yaw_dir = (i % 2 == 0) ? 1 : -1;
+
+    mixer_matrix(0, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_;
+    mixer_matrix(1, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * (-sin(psi_[i]));
+    mixer_matrix(2, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * cos(psi_[i]);
+    mixer_matrix(3, i) = rho_ * pow(D_, 5.0) / (4 * pow(M_PI, 2.0)) * yaw_dir * C_Q_;
   }
 
   // Take the pseudoinverse of the matrix
-  Eigen::MatrixXf M_pinv = M.completeOrthogonalDecomposition().pseudoInverse();
+  Eigen::MatrixXf M_pinv = mixer_matrix.completeOrthogonalDecomposition().pseudoInverse();
   
-  // Fill in the actual mixing matrix used in the mixer
+  // Fill in the mixing matrix from the inverted matrix above 
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (i < num_motors) {
+    if (i < num_motors_) {
       correct_mixer_.default_pwm_rate[i] = 490;
       correct_mixer_.output_type[i] = M;
-      correct_mixer_.F[i] = M_pinv(0, i);
-      correct_mixer_.x[i] = M_pinv(1, i);
-      correct_mixer_.y[i] = M_pinv(2, i);
-      correct_mixer_.z[i] = M_pinv(3, i);
+      correct_mixer_.F[i] = M_pinv(i, 0);
+      correct_mixer_.x[i] = M_pinv(i, 1);
+      correct_mixer_.y[i] = M_pinv(i, 2);
+      correct_mixer_.z[i] = M_pinv(i, 3);
     }
     else {
+      // Set the rest of the ouput types to NONE with a default PWM rate of 50
       correct_mixer_.default_pwm_rate[i] = 50;
       correct_mixer_.output_type[i] = NONE;
       correct_mixer_.F[i] = 0.0;
@@ -212,6 +258,7 @@ void Mixer::mix_output()
     commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
     commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
   } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
+    // TODO: Fix this since input to the mixer is thrust, not throttle
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
     commands.z = 0.0;
@@ -222,15 +269,28 @@ void Mixer::mix_output()
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
-                     + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
-
+      double omega_squared = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
+                             + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
+      
+      // Ensure that omega_squared is non-negative
+      if (omega_squared < 0.0) { omega_squared = 0.0; }
+      
+      // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
+      // Note that we assume constant airspeed and propeller speed, leading to a constant advance ratio, torque, and thrust constants.
+      double V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
+                    + R_ * i_0_
+                    + K_V_ * sqrt(omega_squared);
+
+      // Convert desired V_in setting to a throttle setting
+      outputs_[i] = V_in / V_max_;
+      
       // Save off the largest control output if it is greater than 1.0 for future scaling
       if (outputs_[i] > max_output) { max_output = outputs_[i]; }
     }
   }
 
   // saturate outputs to maintain controllability even during aggressive maneuvers
+  // TODO: Fix how this saturates to maintain controlability
   float scale_factor = 1.0;
   if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
 

From ab5ec00a2b1126df511ebca25a095da6024f8a8f Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Mon, 12 Aug 2024 10:54:04 -0600
Subject: [PATCH 06/41] added maximum throttle param from RC controller to
 maintain controllability under aggressive maneuvers

---
 include/controller.h | 1 +
 include/param.h      | 1 +
 src/controller.cpp   | 9 +++++++--
 src/param.cpp        | 1 +
 4 files changed, 10 insertions(+), 2 deletions(-)

diff --git a/include/controller.h b/include/controller.h
index 92d4409c..5293be5f 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -105,6 +105,7 @@ class Controller : public ParamListenerInterface
   PID yaw_rate_;
 
   float max_thrust_;
+  float max_throttle_;
 
   uint64_t prev_time_us_;
 };
diff --git a/include/param.h b/include/param.h
index b0b43a58..d24f0cc0 100644
--- a/include/param.h
+++ b/include/param.h
@@ -206,6 +206,7 @@ enum : uint16_t
   PARAM_RC_OVERRIDE_DEVIATION,
   PARAM_OVERRIDE_LAG_TIME,
   PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE,
+  PARAM_RC_MAX_THROTTLE,
 
   PARAM_RC_ATTITUDE_MODE,
   PARAM_RC_MAX_ROLL,
diff --git a/src/controller.cpp b/src/controller.cpp
index 16e766e4..309dc08b 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -52,6 +52,8 @@ void Controller::init()
 
   // Calculate the max thrust to convert from throttle setting to thrust
   calculate_max_thrust();
+
+  max_throttle_ = RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
   
   // Don't saturate the torque values
   float max_torque = INFINITY;
@@ -211,6 +213,7 @@ void Controller::param_change_callback(uint16_t param_id)
     case PARAM_NO_LOAD_CURRENT:
     case PARAM_VOLT_MAX:
     case PARAM_NUM_MOTORS:
+    case PARAM_RC_MAX_THROTTLE:
     case PARAM_PID_TAU:
       init();
       break;
@@ -257,8 +260,10 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
 
   // THROTTLE
   // TODO: This works if the input is a throttle setting, but not if the input is a thrust
-  // TODO: Add something that will saturate input RC command to a MAX_THROTTLE parameter, so you can maintain controllability
-  out.F = command.F.value * max_thrust_;
+  // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability
+  out.F = command.F.value * max_thrust_ * max_throttle_;
+
+  // std::cout << "Throttle: " << out.F << std::endl;
 
   return out;
 }
diff --git a/src/param.cpp b/src/param.cpp
index 4bd0ef62..27af30e8 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -253,6 +253,7 @@ void Params::set_defaults(void)
   init_param_float(PARAM_RC_OVERRIDE_DEVIATION, "RC_OVRD_DEV", 0.1); // RC stick deviation from center for override | 0.0 | 1.0
   init_param_int(PARAM_OVERRIDE_LAG_TIME, "OVRD_LAG_TIME", 1000); // RC stick deviation lag time before returning control (ms) | 0 | 100000
   init_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, "MIN_THROTTLE", true); // Take minimum throttle between RC and computer at all times | 0 | 1
+  init_param_float(PARAM_RC_MAX_THROTTLE, "RC_MAX_THR", 0.7f); // Maximum throttle command sent by full deflection of RC sticks, to maintain controllability during aggressive maneuvers | 0.0 | 1.0
 
   init_param_int(PARAM_RC_ATTITUDE_MODE, "RC_ATT_MODE", 1); // Attitude mode for RC sticks (0: rate, 1: angle). Overridden if RC_ATT_CTRL_CHN is set. | 0 | 1
   init_param_float(PARAM_RC_MAX_ROLL, "RC_MAX_ROLL", 0.786f); // Maximum roll angle command sent by full deflection of RC sticks | 0.0 | 3.14159

From 20e5d639c0784a24e3da2bc88ef1e7debc1151fc Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Wed, 14 Aug 2024 15:23:55 -0600
Subject: [PATCH 07/41] Scales thrust commands by RC_MAX_THROTTLE to maintain
 controllability

---
 include/command_manager.h |  4 ++--
 include/controller.h      |  1 +
 src/comm_manager.cpp      |  2 +-
 src/command_manager.cpp   |  2 +-
 src/controller.cpp        | 14 +++++++-------
 5 files changed, 12 insertions(+), 11 deletions(-)

diff --git a/include/command_manager.h b/include/command_manager.h
index 4f39564e..a22aaaed 100644
--- a/include/command_manager.h
+++ b/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;
 
@@ -108,7 +108,7 @@ class CommandManager : public ParamListenerInterface
                                            {true, PASSTHROUGH, 0.0},
                                            {true, PASSTHROUGH, 0.0},
                                            {true, PASSTHROUGH, 0.0},
-                                           {true, THROTTLE, 0.0}};
+                                           {true, PASSTHROUGH, 0.0}};
   // clang-format on
 
   typedef enum
diff --git a/include/controller.h b/include/controller.h
index 5293be5f..319dea3a 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -60,6 +60,7 @@ class Controller : public ParamListenerInterface
   Controller(ROSflight & rf);
 
   inline const Output & output() const { return output_; }
+  inline const float max_thrust() const { return max_thrust_; }
 
   void init();
   void run();
diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
index 6e872c5e..5a22db42 100644
--- a/src/comm_manager.cpp
+++ b/src/comm_manager.cpp
@@ -254,7 +254,7 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon
       new_offboard_command.x.type = PASSTHROUGH;
       new_offboard_command.y.type = PASSTHROUGH;
       new_offboard_command.z.type = PASSTHROUGH;
-      new_offboard_command.F.type = THROTTLE;
+      new_offboard_command.F.type = PASSTHROUGH;
       break;
     case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE:
       new_offboard_command.x.type = RATE;
diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index a66aaa64..fd54f0c4 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -113,7 +113,7 @@ void CommandManager::interpret_rc(void)
     rc_command_.x.type = PASSTHROUGH;
     rc_command_.y.type = PASSTHROUGH;
     rc_command_.z.type = PASSTHROUGH;
-    rc_command_.F.type = THROTTLE;
+    rc_command_.F.type = PASSTHROUGH;
   } else {
     // roll and pitch
     control_type_t roll_pitch_type;
diff --git a/src/controller.cpp b/src/controller.cpp
index 309dc08b..73511586 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -135,7 +135,6 @@ void Controller::run()
   output_.F = pid_output.F;
 }
 
-// TODO: Fix this to output equilibrium torque (not normalized values)
 void Controller::calculate_equilbrium_torque_from_rc()
 {
   // Make sure we are disarmed
@@ -168,7 +167,6 @@ void Controller::calculate_equilbrium_torque_from_rc()
       run_pid_loops(0, fake_state, RF_.command_manager_.rc_control(), false);
 
     // the output from the controller is going to be the static offsets
-    // TODO: Should we be adding the equilibrium torques to the existing ones?
     RF_.params_.set_param_float(PARAM_X_EQ_TORQUE,
                                 pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE));
     RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE,
@@ -259,11 +257,13 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
   }
 
   // THROTTLE
-  // TODO: This works if the input is a throttle setting, but not if the input is a thrust
-  // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability
-  out.F = command.F.value * max_thrust_ * max_throttle_;
-
-  // std::cout << "Throttle: " << out.F << std::endl;
+  if (command.F.type == THROTTLE) {
+    // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability
+    out.F = command.F.value * max_thrust_ * max_throttle_;
+  } else {
+    // If it is not a throttle setting, then it is passthrough (to the mixer)
+    out.F = command.F.value;
+  }
 
   return out;
 }

From c068882dcd28cbfe47637e270e24604450d5aad1 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Wed, 14 Aug 2024 15:26:10 -0600
Subject: [PATCH 08/41] Computes the mixing matrix using the SVD, which doesn't
 dynamically allocate memory.

---
 include/mixer.h |  5 +++--
 src/mixer.cpp   | 57 ++++++++++++++++++++++++++++---------------------
 2 files changed, 36 insertions(+), 26 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index abd98a82..c3d31bac 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -34,7 +34,8 @@
 
 #include "interface/param_listener.h"
 
-#include <eigen3/Eigen/QR>
+#include <eigen3/Eigen/Dense>
+#include <eigen3/Eigen/SVD>
 
 #include <cstdbool>
 #include <cstdint>
@@ -232,7 +233,7 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
 
-  mixer_t correct_mixer_;
+  mixer_t custom_mixer_;
 
   const mixer_t * mixer_to_use_;
 
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 993cc8eb..abd0ec23 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -34,8 +34,6 @@
 #include "rosflight.h"
 
 #include <cstdint>
-#include <iostream>
-// #include "eigen3/Eigen/Dense"
 
 namespace rosflight_firmware
 {
@@ -114,7 +112,7 @@ void Mixer::init_mixing()
     if (mixer_to_use_ == &custom_mixing) {
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Calculating custom mixer values...");
       calculate_mixer_values();
-      mixer_to_use_ = &correct_mixer_;
+      mixer_to_use_ = &custom_mixer_;
     }
   }
 
@@ -163,8 +161,11 @@ void Mixer::update_parameters() {
 
 void Mixer::calculate_mixer_values()
 {
-  // Create the mixing matrix as defined in Ch. 14
-  Eigen::MatrixXf mixer_matrix(4, num_motors_);
+  // Create the mixing matrix as defined in Ch. 14. Initialize the matrix to zero.
+  Eigen::Matrix<double, 4, 10> mixer_matrix;
+  mixer_matrix.setZero();
+
+  // Fill in the mixing matrix with the correct values according to the number of motors
   for (int i = 0; i < num_motors_; ++i) {
     // Determine the direction of the yaw moment generated by the motor i 
     // TODO: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
@@ -176,27 +177,36 @@ void Mixer::calculate_mixer_values()
     mixer_matrix(3, i) = rho_ * pow(D_, 5.0) / (4 * pow(M_PI, 2.0)) * yaw_dir * C_Q_;
   }
 
-  // Take the pseudoinverse of the matrix
-  Eigen::MatrixXf M_pinv = mixer_matrix.completeOrthogonalDecomposition().pseudoInverse();
-  
+  // Calculate the pseudoinverse of the mixing matrix using the SVD
+  Eigen::JacobiSVD<Eigen::Matrix<double, 4, 10>> svd(mixer_matrix, Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix<double, 10, 4> Sig;
+  Sig.setZero();
+  Sig(0, 0) = 1.0 / svd.singularValues()[0];
+  Sig(1, 1) = 1.0 / svd.singularValues()[1];
+  Sig(2, 2) = 1.0 / svd.singularValues()[2];
+  Sig(3, 3) = 1.0 / svd.singularValues()[3];
+
+  // Pseudoinverse of the mixing matrix
+  Eigen::Matrix<double, 10, 4> mixer_matrix_pinv = svd.matrixV() * Sig * svd.matrixU().transpose();
+
   // Fill in the mixing matrix from the inverted matrix above 
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (i < num_motors_) {
-      correct_mixer_.default_pwm_rate[i] = 490;
-      correct_mixer_.output_type[i] = M;
-      correct_mixer_.F[i] = M_pinv(i, 0);
-      correct_mixer_.x[i] = M_pinv(i, 1);
-      correct_mixer_.y[i] = M_pinv(i, 2);
-      correct_mixer_.z[i] = M_pinv(i, 3);
+      custom_mixer_.default_pwm_rate[i] = 490;
+      custom_mixer_.output_type[i] = M;
+      custom_mixer_.F[i] = mixer_matrix_pinv(i, 0);
+      custom_mixer_.x[i] = mixer_matrix_pinv(i, 1);
+      custom_mixer_.y[i] = mixer_matrix_pinv(i, 2);
+      custom_mixer_.z[i] = mixer_matrix_pinv(i, 3);
     }
     else {
       // Set the rest of the ouput types to NONE with a default PWM rate of 50
-      correct_mixer_.default_pwm_rate[i] = 50;
-      correct_mixer_.output_type[i] = NONE;
-      correct_mixer_.F[i] = 0.0;
-      correct_mixer_.x[i] = 0.0;
-      correct_mixer_.y[i] = 0.0;
-      correct_mixer_.z[i] = 0.0;
+      custom_mixer_.default_pwm_rate[i] = 50;
+      custom_mixer_.output_type[i] = NONE;
+      custom_mixer_.F[i] = 0.0;
+      custom_mixer_.x[i] = 0.0;
+      custom_mixer_.y[i] = 0.0;
+      custom_mixer_.z[i] = 0.0;
     }
   }
 }
@@ -257,8 +267,7 @@ void Mixer::mix_output()
     commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
     commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
     commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
-  } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
-    // TODO: Fix this since input to the mixer is thrust, not throttle
+  } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) * RF_.controller_.max_thrust()) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
     commands.z = 0.0;
@@ -290,9 +299,9 @@ void Mixer::mix_output()
   }
 
   // saturate outputs to maintain controllability even during aggressive maneuvers
-  // TODO: Fix how this saturates to maintain controlability
+  // TODO: Fix how this saturates to maintain controlability. 
   float scale_factor = 1.0;
-  if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
+  // if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
 
   // Perform Motor Output Scaling
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {

From fbc2fe3f46d0fe31fcd239cd074e1b7bef793e23 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Wed, 14 Aug 2024 15:33:12 -0600
Subject: [PATCH 09/41] fixing code style

---
 include/controller.h |  2 +-
 include/mixer.h      | 23 +++++++++++------------
 src/comm_manager.cpp |  3 ++-
 src/controller.cpp   |  6 +++---
 src/mixer.cpp        | 41 +++++++++++++++++++++++------------------
 5 files changed, 40 insertions(+), 35 deletions(-)

diff --git a/include/controller.h b/include/controller.h
index 319dea3a..bceafa41 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -95,7 +95,7 @@ class Controller : public ParamListenerInterface
   ROSflight & RF_;
 
   Controller::Output run_pid_loops(uint32_t dt, const Estimator::State & state,
-                                  const control_t & command, bool update_integrators);
+                                   const control_t & command, bool update_integrators);
 
   Output output_;
 
diff --git a/include/mixer.h b/include/mixer.h
index c3d31bac..270de647 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -258,20 +258,19 @@ class Mixer : public ParamListenerInterface
   // clang-format on
 
   // Define parameters for the mixer
-  float R_; // Motor resistance
-  float rho_; // Air density
-  float K_V_; // Motor back-emf constant
-  float K_Q_ = 0.01706; // Motor torque constant
-  float i_0_; // Motor no-load current
-  float D_;   // Propeller diameter
-  float C_T_; // Thrust coefficient
-  float C_Q_; // Torque coefficient
-  int num_motors_; // Number of motors
-  float V_max_; // Maximum battery voltage
-  float l_[NUM_MIXER_OUTPUTS]; // Radial distance from center of mass to motor
+  float R_;                      // Motor resistance
+  float rho_;                    // Air density
+  float K_V_;                    // Motor back-emf constant
+  float K_Q_ = 0.01706;          // Motor torque constant
+  float i_0_;                    // Motor no-load current
+  float D_;                      // Propeller diameter
+  float C_T_;                    // Thrust coefficient
+  float C_Q_;                    // Torque coefficient
+  int num_motors_;               // Number of motors
+  float V_max_;                  // Maximum battery voltage
+  float l_[NUM_MIXER_OUTPUTS];   // Radial distance from center of mass to motor
   float psi_[NUM_MIXER_OUTPUTS]; // Angle of motor from body x-axis
 
-
 public:
   Mixer(ROSflight & _rf);
   void init();
diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
index 5a22db42..44caa7b0 100644
--- a/src/comm_manager.cpp
+++ b/src/comm_manager.cpp
@@ -350,7 +350,8 @@ void CommManager::send_status(void)
   if (!initialized_) { return; }
 
   uint8_t control_mode = 0;
-  if (RF_.params_.get_param_int(PARAM_FIXED_WING) || RF_.command_manager_.combined_control().x.type == PASSTHROUGH) {
+  if (RF_.params_.get_param_int(PARAM_FIXED_WING)
+      || RF_.command_manager_.combined_control().x.type == PASSTHROUGH) {
     control_mode = MODE_PASS_THROUGH;
   } else if (RF_.command_manager_.combined_control().x.type == ANGLE) {
     control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
diff --git a/src/controller.cpp b/src/controller.cpp
index 73511586..806f46e8 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -54,7 +54,7 @@ void Controller::init()
   calculate_max_thrust();
 
   max_throttle_ = RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
-  
+
   // Don't saturate the torque values
   float max_torque = INFINITY;
   float min = -max_torque;
@@ -97,7 +97,7 @@ void Controller::calculate_max_thrust()
 
   // Using Eq. 4.19 and setting equal to the equation at the beginning of 4.3 in Small Unmanned Aircraft
   // We only need the positive root. Since a and b positive, sqrt term must be positive
-  float omega = (-b + sqrt(pow(b, 2.0) - 4 * a * c)) / (2 * a); 
+  float omega = (-b + sqrt(pow(b, 2.0) - 4 * a * c)) / (2 * a);
 
   // Calculate the max thrust from Eq in 4.3 of Small Unmanned Aircraft
   // Note that the equation is for a single motor, so to calculate max thrust, we need to multiply by the number of motors
@@ -222,7 +222,7 @@ void Controller::param_change_callback(uint16_t param_id)
 }
 
 Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::State & state,
-                                            const control_t & command, bool update_integrators)
+                                             const control_t & command, bool update_integrators)
 {
   // Based on the control types coming from the command manager, run the appropriate PID loops
   Controller::Output out;
diff --git a/src/mixer.cpp b/src/mixer.cpp
index abd0ec23..75f10611 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -93,7 +93,7 @@ void Mixer::param_change_callback(uint16_t param_id)
 void Mixer::init_mixing()
 {
   // Update parameters
-  update_parameters(); 
+  update_parameters();
 
   // clear the invalid mixer error
   RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER);
@@ -110,7 +110,8 @@ void Mixer::init_mixing()
     mixer_to_use_ = array_of_mixers_[mixer_choice];
 
     if (mixer_to_use_ == &custom_mixing) {
-      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO, "Calculating custom mixer values...");
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+                            "Calculating custom mixer values...");
       calculate_mixer_values();
       mixer_to_use_ = &custom_mixer_;
     }
@@ -124,7 +125,8 @@ void Mixer::init_mixing()
   }
 }
 
-void Mixer::update_parameters() {
+void Mixer::update_parameters()
+{
   R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE);
   rho_ = RF_.params_.get_param_float(PARAM_AIR_DENSITY);
   K_V_ = RF_.params_.get_param_float(PARAM_MOTOR_KV);
@@ -167,18 +169,21 @@ void Mixer::calculate_mixer_values()
 
   // Fill in the mixing matrix with the correct values according to the number of motors
   for (int i = 0; i < num_motors_; ++i) {
-    // Determine the direction of the yaw moment generated by the motor i 
+    // Determine the direction of the yaw moment generated by the motor i
     // TODO: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
     int yaw_dir = (i % 2 == 0) ? 1 : -1;
 
     mixer_matrix(0, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_;
-    mixer_matrix(1, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * (-sin(psi_[i]));
+    mixer_matrix(1, i) =
+      rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * (-sin(psi_[i]));
     mixer_matrix(2, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * cos(psi_[i]);
     mixer_matrix(3, i) = rho_ * pow(D_, 5.0) / (4 * pow(M_PI, 2.0)) * yaw_dir * C_Q_;
   }
 
   // Calculate the pseudoinverse of the mixing matrix using the SVD
-  Eigen::JacobiSVD<Eigen::Matrix<double, 4, 10>> svd(mixer_matrix, Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::JacobiSVD<Eigen::Matrix<double, 4, 10>> svd(mixer_matrix,
+                                                     Eigen::FullPivHouseholderQRPreconditioner
+                                                       | Eigen::ComputeFullU | Eigen::ComputeFullV);
   Eigen::Matrix<double, 10, 4> Sig;
   Sig.setZero();
   Sig(0, 0) = 1.0 / svd.singularValues()[0];
@@ -189,7 +194,7 @@ void Mixer::calculate_mixer_values()
   // Pseudoinverse of the mixing matrix
   Eigen::Matrix<double, 10, 4> mixer_matrix_pinv = svd.matrixV() * Sig * svd.matrixU().transpose();
 
-  // Fill in the mixing matrix from the inverted matrix above 
+  // Fill in the mixing matrix from the inverted matrix above
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (i < num_motors_) {
       custom_mixer_.default_pwm_rate[i] = 490;
@@ -198,8 +203,7 @@ void Mixer::calculate_mixer_values()
       custom_mixer_.x[i] = mixer_matrix_pinv(i, 1);
       custom_mixer_.y[i] = mixer_matrix_pinv(i, 2);
       custom_mixer_.z[i] = mixer_matrix_pinv(i, 3);
-    }
-    else {
+    } else {
       // Set the rest of the ouput types to NONE with a default PWM rate of 50
       custom_mixer_.default_pwm_rate[i] = 50;
       custom_mixer_.output_type[i] = NONE;
@@ -267,7 +271,8 @@ void Mixer::mix_output()
     commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
     commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
     commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
-  } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE) * RF_.controller_.max_thrust()) {
+  } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
+               * RF_.controller_.max_thrust()) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
     commands.z = 0.0;
@@ -278,28 +283,28 @@ void Mixer::mix_output()
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      double omega_squared = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
-                             + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
-      
+      double omega_squared =
+        (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
+         + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
+
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
-      
+
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
       // Note that we assume constant airspeed and propeller speed, leading to a constant advance ratio, torque, and thrust constants.
       double V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
-                    + R_ * i_0_
-                    + K_V_ * sqrt(omega_squared);
+        + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
       // Convert desired V_in setting to a throttle setting
       outputs_[i] = V_in / V_max_;
-      
+
       // Save off the largest control output if it is greater than 1.0 for future scaling
       if (outputs_[i] > max_output) { max_output = outputs_[i]; }
     }
   }
 
   // saturate outputs to maintain controllability even during aggressive maneuvers
-  // TODO: Fix how this saturates to maintain controlability. 
+  // TODO: Fix how this saturates to maintain controlability.
   float scale_factor = 1.0;
   // if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
 

From 69dc3afae78d2706d0d732f494e7a18813e115d4 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Wed, 14 Aug 2024 15:45:11 -0600
Subject: [PATCH 10/41] return type qualifier fix

---
 include/controller.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/include/controller.h b/include/controller.h
index bceafa41..7987b63a 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -60,7 +60,7 @@ class Controller : public ParamListenerInterface
   Controller(ROSflight & rf);
 
   inline const Output & output() const { return output_; }
-  inline const float max_thrust() const { return max_thrust_; }
+  inline float max_thrust() const { return max_thrust_; }
 
   void init();
   void run();

From cc75f3201e3106fcaa49f650aeedd3899dc660e4 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 09:18:13 -0600
Subject: [PATCH 11/41] added libeigen3-dev to workflow runs

---
 .github/workflows/pre-release.yml      | 2 +-
 .github/workflows/release.yml          | 2 +-
 .github/workflows/varmint_firmware.yml | 2 +-
 3 files changed, 3 insertions(+), 3 deletions(-)

diff --git a/.github/workflows/pre-release.yml b/.github/workflows/pre-release.yml
index 04676287..cae3bfef 100644
--- a/.github/workflows/pre-release.yml
+++ b/.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
diff --git a/.github/workflows/release.yml b/.github/workflows/release.yml
index 26bdfb0c..65cb84f8 100644
--- a/.github/workflows/release.yml
+++ b/.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
diff --git a/.github/workflows/varmint_firmware.yml b/.github/workflows/varmint_firmware.yml
index 1f894760..9a782e3c 100644
--- a/.github/workflows/varmint_firmware.yml
+++ b/.github/workflows/varmint_firmware.yml
@@ -12,7 +12,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

From 04d14e31701764263a7e2056edf3932a0a00e807 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 15:55:50 -0600
Subject: [PATCH 12/41] Added Eigen dependency to all boards.

---
 CMakeLists.txt | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index c2586cfe..503f848f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,6 +29,7 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
 endif()
 
 ### source files ###
+find_package(Eigen3 REQUIRED)
 
 include_directories(
     include
@@ -40,9 +41,6 @@ include_directories(
     comms/mavlink/v1.0/rosflight
 )
 
-include_directories(${EIGEN3_INCLUDE_DIRS})
-include_directories(/usr/include/eigen3)
-
 file(GLOB_RECURSE ROSFLIGHT_SOURCES
     "src/*.cpp"
     "lib/turbomath/turbomath.cpp"

From b28b9100d9631d60fb4d94ac31465a08004dccd5 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 16:06:11 -0600
Subject: [PATCH 13/41] Moved Eigen package to common/CMakeLists.txt

---
 CMakeLists.txt | 1 -
 1 file changed, 1 deletion(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 503f848f..3efea7c0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,7 +29,6 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
 endif()
 
 ### source files ###
-find_package(Eigen3 REQUIRED)
 
 include_directories(
     include

From b76ede7da0a1c2b5262885650ea569bbc4a6916c Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 16:22:34 -0600
Subject: [PATCH 14/41] Added hardcoded, precomputed mixing matrices for
 standard aircraft

---
 include/mixer.h | 96 ++++++++++++++++++++++++-------------------------
 src/mixer.cpp   | 13 +++----
 2 files changed, 55 insertions(+), 54 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 270de647..70a12f78 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -124,74 +124,74 @@ class Mixer : public ParamListenerInterface
   const mixer_t quadcopter_plus_mixing = {
     {   M,     M,     M,     M, NONE, NONE, NONE, NONE, NONE, NONE},  // output_type
     { 490,   490,   490,   490,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {1.0f,  1.0f,  1.0f,  1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {0.0f, -1.0f,  0.0f,  1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {1.0f,  0.0f, -1.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {1.0f, -1.0f,  1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {5098.031218379775, 5098.0312183797805, 5098.031218379778, 5098.0312183797805, 0, 0, 0, 0, 0, 0},
+    {0.0, -40784.24974703961, 0.0, 40784.24974703687, 0, 0, 0, 0, 0, 0},
+    {40784.249747038244, 0.0, -40784.24974703822, 0.0, 0, 0, 0, 0, 0, 0},
+    {223010.98943043666, -223010.98943043654, 223010.98943043666, -223010.98943043657, 0, 0, 0, 0, 0, 0}};
 
   const mixer_t quadcopter_x_mixing = {
     {    M,     M,     M,     M, NONE, NONE, NONE, NONE, NONE, NONE},  // output_type
     {  490,   490,   490,   490,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    { 1.0f,  1.0f,  1.0f,  1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {-1.0f, -1.0f,  1.0f,  1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    { 1.0f, -1.0f, -1.0f,  1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    { 1.0f, -1.0f,  1.0f, -1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {5098.031218379779, 5098.03121837978, 5098.031218379777, 5098.031218379782, 0, 0, 0, 0, 0, 0},
+    {-28838.81956173643, -28838.819561736505, 28838.81956173648, 28838.819561736465, 0, 0, 0, 0, 0, 0},
+    {28838.819561736495, -28838.819561736498, -28838.819561736433, 28838.81956173645, 0, 0, 0, 0, 0, 0},
+    {223010.9894304366, -223010.9894304366, 223010.98943043666, -223010.9894304366, 0, 0, 0, 0, 0, 0}};
 
   const mixer_t hex_plus_mixing = {
     {   M,          M,          M,     M,         M,         M, NONE, NONE, NONE, NONE},  // output_type
     { 490,        490,        490,   490,       490,       490,   50,   50,   50,   50},  // Rate (Hz)
-    {1.0f,       1.0f,       1.0f,  1.0f,      1.0f,      1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {0.0f, -0.866025f, -0.866025f,  0.0f, 0.866025f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {1.0f,       0.5f,      -0.5f, -1.0f,     -0.5f,      0.5f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {1.0f,      -1.0f,       1.0f, -1.0f,      1.0f,     -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {3398.687478919854, 3398.6874789198537, 3398.6874789198537, 3398.6874789198523, 3398.6874789198537, 3398.6874789198537, 0, 0, 0, 0},
+    {0.0, -23546.797570149454, -23546.797570149447, 0.0, 23546.797570149443, 23546.79757014946, 0, 0, 0, 0},
+    {27189.49983135882, 13594.749915679433, -13594.749915679424, -27189.49983135882, -13594.749915679417, 13594.749915679415, 0, 0, 0, 0},
+    {148673.99295362437, -148673.99295362443, 148673.99295362437, -148673.99295362437, 148673.9929536245, -148673.9929536244, 0, 0, 0, 0}};
 
   const mixer_t hex_x_mixing = {
-    {        M,     M,          M,          M,    M,         M, NONE, NONE, NONE, NONE},  // output_type
-    {      490,   490,        490,        490,  490,       490,   50,   50,   50,   50},  // Rate (Hz)
-    {     1.0f,  1.0f,       1.0f,       1.0f, 1.0f,      1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {    -0.5f, -1.0f,      -0.5f,       0.5f, 1.0f,      0.5f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.866025f,  0.0f, -0.866025f, -0.866025f, 0.0f, 0.866025f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {     1.0f, -1.0f,       1.0f,      -1.0f, 1.0f,     -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {3398.687478919854, 3398.687478919853, 3398.687478919853, 3398.6874789198523, 3398.687478919854, 3398.687478919854, 0, 0, 0, 0},
+    {-13594.749915679407, -27189.49983135883, -13594.74991567941, 13594.749915679384, 27189.49983135884, 13594.74991567942, 0, 0, 0, 0},
+    {23546.797570149465, 0.0, -23546.797570149454, -23546.797570149472, 0.0, 23546.797570149465, 0, 0, 0, 0},
+    {148673.99295362426, -148673.9929536243, 148673.99295362443, -148673.99295362443, 148673.99295362446, -148673.99295362437, 0, 0, 0, 0}};
 
   const mixer_t octocopter_plus_mixing = {
-    {   M,       M,     M,       M,     M,       M,    M,      M, NONE, NONE},  // output_type
-    { 490,     490,   490,     490,   490,     490,  490,    490,   50,   50},  // Rate (Hz)
-    {1.0f,    1.0f,  1.0f,    1.0f,  1.0f,    1.0f, 1.0f,   1.0f, 0.0f, 0.0f},  // F Mix
-    {0.0f, -0.707f, -1.0f, -0.707f,  0.0f,  0.707f, 1.0f, 0.707f, 0.0f, 0.0f},  // X Mix
-    {1.0f,  0.707f,  0.0f, -0.707f, -1.0f, -0.707f, 0.0f, 0.707f, 0.0f, 0.0f},  // Y Mix
-    {1.0f,   -1.0f,  1.0f,   -1.0f,  1.0f,   -1.0f, 1.0f,  -1.0f, 0.0f, 0.0f}}; // Z Mix
+    {M, M, M, M, M, M, M, M, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
+    {2549.0156091898907, 2549.0156091898903, 2549.01560918989, 2549.0156091898893, 2549.015609189889, 2549.0156091898893, 2549.01560918989, 2549.0156091898903, 0, 0},
+    {0.0, -14419.409780868224, -20392.124873519115, -14419.409780868233, 0.0, 14419.409780868235, 20392.124873519115, 14419.409780868242, 0, 0},
+    {20392.124873519126, 14419.409780868244, 0.0, -14419.409780868249, -20392.124873519126, -14419.409780868249, 0.0, 14419.409780868238, 0, 0},
+    {111505.49471521821, -111505.4947152183, 111505.49471521829, -111505.49471521829, 111505.49471521827, -111505.49471521826, 111505.49471521827, -111505.49471521827, 0, 0}};
 
   const mixer_t octocopter_x_mixing = {
-    {      M,      M,       M,       M,      M,       M,      M,     M, NONE, NONE},  // output_type
-    {    490,    490,     490,     490,    490,     490,    490,   490,   50,   50},  // Rate (Hz)
-    {   1.0f,   1.0f,    1.0f,    1.0f,   1.0f,    1.0f,   1.0f,  1.0f, 0.0f, 0.0f},  // F Mix
-    {-0.414f,  -1.0f,   -1.0f, -0.414f, 0.414f,    1.0f,   1.0f, 0.414, 0.0f, 0.0f},  // X Mix
-    {   1.0f, 0.414f, -0.414f,   -1.0f,  -1.0f, -0.414f, 0.414f,   1.0, 0.0f, 0.0f},  // Y Mix
-    {   1.0f,  -1.0f,    1.0f,   -1.0f,   1.0f,   -1.0f,   1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix
+    {M, M, M, M, M, M, M, M, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
+    {2549.0156091898893, 2549.01560918989, 2549.01560918989, 2549.0156091898903, 2549.0156091898903, 2549.01560918989, 2549.01560918989, 2549.0156091898893, 0, 0},
+    {-7803.728339815821, -18839.866795058642, -18839.86679505863, -7803.728339815828, 7803.728339815823, 18839.86679505863, 18839.86679505864, 7803.728339815834, 0, 0},
+    {18839.86679505862, 7803.728339815822, -7803.728339815816, -18839.866795058624, -18839.86679505863, -7803.728339815828, 7803.728339815817, 18839.86679505862, 0, 0},
+    {111505.49471521824, -111505.49471521845, 111505.49471521832, -111505.49471521827, 111505.49471521837, -111505.49471521827, 111505.49471521833, -111505.49471521836, 0, 0}};
 
   const mixer_t Y6_mixing = {
-    {     M,      M,       M,       M,      M,      M, NONE, NONE, NONE, NONE},  // output_type
-    {   490,    490,     490,     490,    490,    490,   50,   50,   50,   50},  // Rate (Hz)
-    {  1.0f,   1.0f,    1.0f,    1.0f,   1.0f,   1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    { -1.0f,  -1.0f,    0.0f,    0.0f,   1.0f,   1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.667f, 0.667f, -1.333f, -1.333f, 0.667f, 0.667f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {  1.0f,  -1.0f,    1.0f,   -1.0f,   1.0f,  -1.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {3398.6874789198546, 3398.6874789198523, 3398.6874789198537, 3398.6874789198546, 3398.687478919853, 3398.6874789198528, 0, 0, 0, 0},
+    {-23546.79757014947, 0.0, 23546.797570149454, -23546.79757014944, 0.0, 23546.79757014947, 0, 0, 0, 0},
+    {13594.749915679397, -27189.499831358822, 13594.749915679413, 13594.749915679437, -27189.499831358844, 13594.749915679426, 0, 0, 0, 0},
+    {148673.9929536246, -148673.9929536245, 148673.9929536245, -148673.99295362452, 148673.99295362446, -148673.9929536245, 0, 0, 0, 0}};
 
   const mixer_t X8_mixing = {
-    {    M,     M,     M,     M,     M,     M,    M,     M, NONE, NONE},  // output_type
-    {  490,   490,   490,   490,   490,   490,  490,  490,   50,   50},  // Rate (Hz)
-    { 1.0f,  1.0f,  1.0f,  1.0f,  1.0f,  1.0f, 1.0f,  1.0f, 0.0f, 0.0f},  // F Mix
-    {-1.0f, -1.0f, -1.0f, -1.0f,  1.0f,  1.0f, 1.0f,  1.0f, 0.0f, 0.0f},  // X Mix
-    { 1.0f,  1.0f, -1.0f, -1.0f, -1.0f, -1.0f, 1.0f,  1.0f, 0.0f, 0.0f},  // Y Mix
-    { 1.0f, -1.0f,  1.0f, -1.0f,  1.0f, -1.0f, 1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix
+    {M, M, M, M, M, M, M, M, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
+    {2549.01560918989, 2549.01560918989, 2549.0156091898893, 2549.0156091898893, 2549.01560918989, 2549.01560918989, 2549.0156091898903, 2549.0156091898903, 0, 0},
+    {-14419.409780868245, -14419.409780868247, -14419.40978086824, -14419.409780868236, 14419.409780868244, 14419.409780868247, 14419.40978086824, 14419.409780868244, 0, 0},
+    {14419.409780868236, 14419.40978086824, -14419.409780868247, -14419.409780868247, -14419.40978086824, -14419.409780868244, 14419.409780868249, 14419.409780868245, 0, 0},
+    {111505.49471521829, -111505.49471521827, 111505.49471521823, -111505.49471521829, 111505.49471521827, -111505.49471521826, 111505.49471521829, -111505.49471521823, 0, 0}};
 
   const mixer_t tricopter_mixing = {
-    {      M,       M,      M,    NONE,        S,   NONE,   NONE,   NONE,   NONE,   NONE},  // output_type
-    {    490,     490,    490,      50,       50,     50,     50,     50,     50,     50},  // Rate (Hz)
-    { 1.000f,  0.000f, 1.000f,  0.000f,   1.000f,   0.0f,   0.0f,   0.0f,   0.0f,   0.0f},  // F Mix
-    {-1.000f,  0.000f, 0.000f,  0.000f,   1.000f,   0.0f,   0.0f,   0.0f,   0.0f,   0.0f},  // X Mix
-    { 0.667f,  0.000f, 0.667f,  0.000f,  -1.333f,   0.0f,   0.0f,   0.0f,   0.0f,   0.0f},  // Y Mix
-    { 0.000f,  1.000f, 0.000f,  0.000f,   0.000f,   0.0f,   0.0f,   0.0f,   0.0f,   0.0f} }; // Z Mix
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
+    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {3398.687478919855, 3398.687478919855, 3398.6874789198528, 3398.687478919852, 3398.6874789198537, 3398.6874789198528, 0, 0, 0, 0},
+    {-23546.797570149458, -23546.797570149447, 0.0, 0.0, 23546.797570149454, 23546.797570149465, 0, 0, 0, 0},
+    {13594.749915679417, 13594.74991567943, -27189.499831358844, -27189.49983135883, 13594.749915679406, 13594.749915679418, 0, 0, 0, 0},
+    {148673.99295362437, -148673.99295362429, 148673.99295362443, -148673.9929536245, 148673.9929536244, -148673.99295362443, 0, 0, 0, 0}};
 
   const mixer_t fixedwing_mixing = {
     {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 75f10611..e635eb00 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -170,7 +170,7 @@ void Mixer::calculate_mixer_values()
   // Fill in the mixing matrix with the correct values according to the number of motors
   for (int i = 0; i < num_motors_; ++i) {
     // Determine the direction of the yaw moment generated by the motor i
-    // TODO: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
+    // Note: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
     int yaw_dir = (i % 2 == 0) ? 1 : -1;
 
     mixer_matrix(0, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_;
@@ -181,10 +181,10 @@ void Mixer::calculate_mixer_values()
   }
 
   // Calculate the pseudoinverse of the mixing matrix using the SVD
-  Eigen::JacobiSVD<Eigen::Matrix<double, 4, 10>> svd(mixer_matrix,
-                                                     Eigen::FullPivHouseholderQRPreconditioner
-                                                       | Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Matrix<double, 10, 4> Sig;
+  Eigen::JacobiSVD<Eigen::Matrix<double, 4, NUM_MIXER_OUTPUTS>> svd(
+    mixer_matrix,
+    Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 4> Sig;
   Sig.setZero();
   Sig(0, 0) = 1.0 / svd.singularValues()[0];
   Sig(1, 1) = 1.0 / svd.singularValues()[1];
@@ -192,7 +192,8 @@ void Mixer::calculate_mixer_values()
   Sig(3, 3) = 1.0 / svd.singularValues()[3];
 
   // Pseudoinverse of the mixing matrix
-  Eigen::Matrix<double, 10, 4> mixer_matrix_pinv = svd.matrixV() * Sig * svd.matrixU().transpose();
+  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 4> mixer_matrix_pinv =
+    svd.matrixV() * Sig * svd.matrixU().transpose();
 
   // Fill in the mixing matrix from the inverted matrix above
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {

From ef17b4719e0e6cf63fa46e24341684af09f388d7 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 16:25:20 -0600
Subject: [PATCH 15/41] fixed include error

---
 include/mixer.h | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 70a12f78..1ea4fc15 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -34,8 +34,8 @@
 
 #include "interface/param_listener.h"
 
-#include <eigen3/Eigen/Dense>
-#include <eigen3/Eigen/SVD>
+#include <Eigen/Dense>
+#include <Eigen/SVD>
 
 #include <cstdbool>
 #include <cstdint>

From a6ced2208a6dab0488401f971c9170e468276c9a Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Thu, 15 Aug 2024 16:29:01 -0600
Subject: [PATCH 16/41] Added find eigen call back

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 3efea7c0..503f848f 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -29,6 +29,7 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
 endif()
 
 ### source files ###
+find_package(Eigen3 REQUIRED)
 
 include_directories(
     include

From 6c5e163dccfe3df23182f1ed6dcbf31eabe49220 Mon Sep 17 00:00:00 2001
From: JMoore5353 <jacobmoor2@gmail.com>
Date: Tue, 27 Aug 2024 10:27:41 -0600
Subject: [PATCH 17/41] Moved Eigen dependency back to the rosflight_firmware
 CMakeLists.txt

---
 CMakeLists.txt | 1 +
 1 file changed, 1 insertion(+)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index 503f848f..c535ebb8 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -30,6 +30,7 @@ endif()
 
 ### source files ###
 find_package(Eigen3 REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIRS})
 
 include_directories(
     include

From 6821d109a4d752163f2efd17dfcd93671b1e1455 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Wed, 9 Oct 2024 13:10:15 -0600
Subject: [PATCH 18/41] removed the quadplane mixing matrix to avoid confusion
 since it doesn't work in the current implementation

---
 include/mixer.h | 12 +-----------
 1 file changed, 1 insertion(+), 11 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 1ea4fc15..1199b279 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -65,8 +65,7 @@ class Mixer : public ParamListenerInterface
     FIXEDWING = 10,
     PASSTHROUGH = 11,
     INVERTED_VTAIL = 12,
-    QUADPLANE = 13,
-    CUSTOM = 14,
+    CUSTOM = 13,
     NUM_MIXERS,
     INVALID_MIXER = 255
   };
@@ -209,14 +208,6 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
     {0.0f, 0.0f, 0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
 
-  const mixer_t quadplane_mixing = {
-    {   S,     S,    S,    M,    M,     M,     M,     M, NONE, NONE},  // Ailerons, Rudder, Elevator, Tractor Motor, Quadrotors
-    {  50,    50,   50,   50,  490,   490,   490,   490,   50,   50},  // Rate (Hz)
-    {0.0f,  0.0f, 0.0f, 1.0f, 1.0f,  1.0f,  1.0f,  1.0f, 0.0f, 0.0f},  // F Mix
-    {1.0f,  0.0f, 0.0f, 0.0f, 0.0f, -1.0f,  0.0f,  1.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f,  1.0f, 0.0f, 0.0f, 1.0f,  0.0f, -1.0f,  0.0f, 0.0f, 0.0f},  // Y Mix
-    {0.0f,  0.0f, 1.0f, 0.0f, 1.0f, -1.0f,  1.0f, -1.0f, 0.0f, 0.0f}}; // Z Mix
-
   const mixer_t passthrough_mixing = {
     {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
@@ -251,7 +242,6 @@ class Mixer : public ParamListenerInterface
     &fixedwing_mixing,
     &passthrough_mixing,
     &fixedwing_inverted_vtail_mixing,
-    &quadplane_mixing,
     &custom_mixing,
   };
 

From 537d75b2401d968584d2ade4a2a1599afdc54aaa Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 14 Oct 2024 13:59:57 -0600
Subject: [PATCH 19/41] added flag to switch between fixedwing mixing and
 multirotor mixing.

---
 include/mixer.h |  2 ++
 src/mixer.cpp   | 58 ++++++++++++++++++++++++++++++++++++++-----------
 2 files changed, 47 insertions(+), 13 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 1199b279..4deb54b0 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -273,6 +273,8 @@ class Mixer : public ParamListenerInterface
   inline const float * get_outputs() const { return raw_outputs_; }
 
   void calculate_mixer_values();
+  void mix_multirotor();
+  void mix_fixedwing();
 };
 
 } // namespace rosflight_firmware
diff --git a/src/mixer.cpp b/src/mixer.cpp
index e635eb00..89da6bb1 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -262,24 +262,19 @@ void Mixer::set_new_aux_command(aux_command_t new_aux_command)
   }
 }
 
-void Mixer::mix_output()
+void Mixer::mix_multirotor()
 {
   Controller::Output commands = RF_.controller_.output();
-  float max_output = 1.0f;
 
-  // Reverse fixed-wing channels just before mixing if we need to
-  if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
-    commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
-    commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
-  } else if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
+  if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
                * RF_.controller_.max_thrust()) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
     commands.z = 0.0;
   }
 
-  if (mixer_to_use_ == nullptr) { return; }
+  // Mix the inputs
+  float max_output = 1.0;
 
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
@@ -292,7 +287,8 @@ void Mixer::mix_output()
       if (omega_squared < 0.0) { omega_squared = 0.0; }
 
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
-      // Note that we assume constant airspeed and propeller speed, leading to a constant advance ratio, torque, and thrust constants.
+      // Note that we assume constant airspeed and propeller speed, leading to constant advance ratio,
+      // torque, and thrust constants.
       double V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
@@ -304,10 +300,11 @@ void Mixer::mix_output()
     }
   }
 
-  // saturate outputs to maintain controllability even during aggressive maneuvers
-  // TODO: Fix how this saturates to maintain controlability.
+  // There is no relative scaling on the above equations. In other words, if the input F command is too
+  // high, then it will "drown out" all other desired outputs. Therefore, we saturate motor outputs to 
+  // maintain controllability even during aggressive maneuvers.
   float scale_factor = 1.0;
-  // if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
+  if (max_output > 1.0) { scale_factor = 1.0 / max_output; }
 
   // Perform Motor Output Scaling
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
@@ -315,6 +312,41 @@ void Mixer::mix_output()
     if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; }
   }
 
+}
+
+void Mixer::mix_fixedwing()
+{
+  Controller::Output commands = RF_.controller_.output();
+
+  // Reverse fixed-wing channels just before mixing if we need to
+  if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
+    commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
+    commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
+    commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
+  }
+
+  // Mix the outputs
+  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    if (mixer_to_use_->output_type[i] != NONE) {
+      // Matrix multiply to mix outputs
+      outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
+                     + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
+
+    }
+  }
+}
+
+void Mixer::mix_output()
+{
+  if (mixer_to_use_ == nullptr) { return; }
+
+  // Mix according to airframe type
+  if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
+    mix_fixedwing();
+  } else {
+    mix_multirotor();
+  }
+
   // Insert AUX Commands, and assemble combined_output_types array (Does not override mixer values)
 
   // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not

From 10ebc1d180746dd3744a979ba3c7c200087254ac Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 14 Oct 2024 14:01:09 -0600
Subject: [PATCH 20/41] default F behavior for fixedwings is passthrough to
 mixer instead of calculating max thrust

---
 src/controller.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/controller.cpp b/src/controller.cpp
index 806f46e8..3b048848 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -257,11 +257,13 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
   }
 
   // THROTTLE
-  if (command.F.type == THROTTLE) {
-    // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability
+  if (command.F.type == THROTTLE && !RF_.params_.get_param_int(PARAM_FIXED_WING)) {
+    // Converts the input throttle command to thrust command, and scales the saturation limit by
+    // RC_MAX_THROTTLE to maintain controllability.
     out.F = command.F.value * max_thrust_ * max_throttle_;
   } else {
-    // If it is not a throttle setting, then it is passthrough (to the mixer)
+    // If it is not a throttle setting, or if the vehicle type is FIXED_WING, then pass directly 
+    // to the mixer.
     out.F = command.F.value;
   }
 

From a571d4b2ffe28ffeddc909f82492a1384ebc2da9 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 15 Oct 2024 13:25:53 -0600
Subject: [PATCH 21/41] refactored some names and got rid of unused
 dependencies

---
 include/controller.h |  1 -
 src/controller.cpp   | 20 +++++++++-----------
 2 files changed, 9 insertions(+), 12 deletions(-)

diff --git a/include/controller.h b/include/controller.h
index 7987b63a..d9d98e38 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -106,7 +106,6 @@ class Controller : public ParamListenerInterface
   PID yaw_rate_;
 
   float max_thrust_;
-  float max_throttle_;
 
   uint64_t prev_time_us_;
 };
diff --git a/src/controller.cpp b/src/controller.cpp
index 3b048848..c22ee20c 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -38,7 +38,6 @@
 
 #include <cstdbool>
 #include <cstdint>
-#include <iostream>
 
 namespace rosflight_firmware
 {
@@ -53,29 +52,27 @@ void Controller::init()
   // Calculate the max thrust to convert from throttle setting to thrust
   calculate_max_thrust();
 
-  max_throttle_ = RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
-
   // Don't saturate the torque values
   float max_torque = INFINITY;
-  float min = -max_torque;
+  float min_torque = -max_torque;
 
   float tau = RF_.params_.get_param_float(PARAM_PID_TAU);
 
   roll_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_P),
              RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_I),
-             RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max_torque, min, tau);
+             RF_.params_.get_param_float(PARAM_PID_ROLL_ANGLE_D), max_torque, min_torque, tau);
   roll_rate_.init(RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_P),
                   RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_I),
-                  RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max_torque, min, tau);
+                  RF_.params_.get_param_float(PARAM_PID_ROLL_RATE_D), max_torque, min_torque, tau);
   pitch_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_P),
               RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_I),
-              RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max_torque, min, tau);
+              RF_.params_.get_param_float(PARAM_PID_PITCH_ANGLE_D), max_torque, min_torque, tau);
   pitch_rate_.init(RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_P),
                    RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_I),
-                   RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max_torque, min, tau);
+                   RF_.params_.get_param_float(PARAM_PID_PITCH_RATE_D), max_torque, min_torque, tau);
   yaw_rate_.init(RF_.params_.get_param_float(PARAM_PID_YAW_RATE_P),
                  RF_.params_.get_param_float(PARAM_PID_YAW_RATE_I),
-                 RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max_torque, min, tau);
+                 RF_.params_.get_param_float(PARAM_PID_YAW_RATE_D), max_torque, min_torque, tau);
 }
 
 void Controller::calculate_max_thrust()
@@ -260,7 +257,8 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
   if (command.F.type == THROTTLE && !RF_.params_.get_param_int(PARAM_FIXED_WING)) {
     // Converts the input throttle command to thrust command, and scales the saturation limit by
     // RC_MAX_THROTTLE to maintain controllability.
-    out.F = command.F.value * max_thrust_ * max_throttle_;
+    float max_throttle = RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
+    out.F = command.F.value * max_thrust_ * max_throttle;
   } else {
     // If it is not a throttle setting, or if the vehicle type is FIXED_WING, then pass directly 
     // to the mixer.
@@ -298,7 +296,7 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator)
   if (dt > 0.0001f) {
     // calculate D term (use dirty derivative if we don't have access to a measurement of the
     // derivative) The dirty derivative is a sort of low-pass filtered version of the derivative.
-    //// TODO: (Include reference to Dr. Beard's notes here)
+    // See Eq. 10.4 of Introduction to Feedback Control by Beard, McLain, Peterson, Killpack
     differentiator_ = (2.0f * tau_ - dt) / (2.0f * tau_ + dt) * differentiator_
       + 2.0f / (2.0f * tau_ + dt) * (x - prev_x_);
     xdot = differentiator_;

From e0ece541b4fc7e7766abd603c0626870f49be522 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 15 Oct 2024 13:32:33 -0600
Subject: [PATCH 22/41] changed the mixer pwm outputs to be compatible with
 Varmint and Pixracer Pro PWM output groups

---
 include/mixer.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 4deb54b0..e7f13680 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -138,7 +138,7 @@ class Mixer : public ParamListenerInterface
 
   const mixer_t hex_plus_mixing = {
     {   M,          M,          M,     M,         M,         M, NONE, NONE, NONE, NONE},  // output_type
-    { 490,        490,        490,   490,       490,       490,   50,   50,   50,   50},  // Rate (Hz)
+    { 490,        490,        490,   490,       490,       490,   490,   490,   50,   50},  // Rate (Hz)
     {3398.687478919854, 3398.6874789198537, 3398.6874789198537, 3398.6874789198523, 3398.6874789198537, 3398.6874789198537, 0, 0, 0, 0},
     {0.0, -23546.797570149454, -23546.797570149447, 0.0, 23546.797570149443, 23546.79757014946, 0, 0, 0, 0},
     {27189.49983135882, 13594.749915679433, -13594.749915679424, -27189.49983135882, -13594.749915679417, 13594.749915679415, 0, 0, 0, 0},
@@ -146,7 +146,7 @@ class Mixer : public ParamListenerInterface
 
   const mixer_t hex_x_mixing = {
     {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
     {3398.687478919854, 3398.687478919853, 3398.687478919853, 3398.6874789198523, 3398.687478919854, 3398.687478919854, 0, 0, 0, 0},
     {-13594.749915679407, -27189.49983135883, -13594.74991567941, 13594.749915679384, 27189.49983135884, 13594.74991567942, 0, 0, 0, 0},
     {23546.797570149465, 0.0, -23546.797570149454, -23546.797570149472, 0.0, 23546.797570149465, 0, 0, 0, 0},
@@ -170,7 +170,7 @@ class Mixer : public ParamListenerInterface
 
   const mixer_t Y6_mixing = {
     {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
     {3398.6874789198546, 3398.6874789198523, 3398.6874789198537, 3398.6874789198546, 3398.687478919853, 3398.6874789198528, 0, 0, 0, 0},
     {-23546.79757014947, 0.0, 23546.797570149454, -23546.79757014944, 0.0, 23546.79757014947, 0, 0, 0, 0},
     {13594.749915679397, -27189.499831358822, 13594.749915679413, 13594.749915679437, -27189.499831358844, 13594.749915679426, 0, 0, 0, 0},
@@ -186,7 +186,7 @@ class Mixer : public ParamListenerInterface
 
   const mixer_t tricopter_mixing = {
     {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 50, 50, 50, 50},
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
     {3398.687478919855, 3398.687478919855, 3398.6874789198528, 3398.687478919852, 3398.6874789198537, 3398.6874789198528, 0, 0, 0, 0},
     {-23546.797570149458, -23546.797570149447, 0.0, 0.0, 23546.797570149454, 23546.797570149465, 0, 0, 0, 0},
     {13594.749915679417, 13594.74991567943, -27189.499831358844, -27189.49983135883, 13594.749915679406, 13594.749915679418, 0, 0, 0, 0},

From ed08c3b7fd6a51206258c4b10c103c4d5228b5a4 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 29 Oct 2024 12:04:26 -0600
Subject: [PATCH 23/41] recomputed canned mixers and added mixer parameters to
 the firmware

---
 include/mixer.h | 197 +++++++++++++++++++++++++++---------------------
 include/param.h |  70 +++++++++++++++++
 src/mixer.cpp   | 188 +++++++++++++++++++++++++++++++++++----------
 src/param.cpp   |  70 +++++++++++++++++
 4 files changed, 399 insertions(+), 126 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index e7f13680..5e4011e9 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -61,11 +61,10 @@ class Mixer : public ParamListenerInterface
     OCTO_X = 6,
     Y6 = 7,
     X8 = 8,
-    TRICOPTER = 9,
-    FIXEDWING = 10,
-    PASSTHROUGH = 11,
-    INVERTED_VTAIL = 12,
-    CUSTOM = 13,
+    FIXEDWING = 9,
+    PASSTHROUGH = 10,
+    INVERTED_VTAIL = 11,
+    CUSTOM = 12,
     NUM_MIXERS,
     INVALID_MIXER = 255
   };
@@ -82,10 +81,12 @@ class Mixer : public ParamListenerInterface
   {
     output_type_t output_type[NUM_MIXER_OUTPUTS];
     float default_pwm_rate[NUM_MIXER_OUTPUTS];
-    float F[NUM_MIXER_OUTPUTS];
-    float x[NUM_MIXER_OUTPUTS];
-    float y[NUM_MIXER_OUTPUTS];
-    float z[NUM_MIXER_OUTPUTS];
+    float Fx[NUM_MIXER_OUTPUTS];
+    float Fy[NUM_MIXER_OUTPUTS];
+    float Fz[NUM_MIXER_OUTPUTS];
+    float Qx[NUM_MIXER_OUTPUTS];
+    float Qy[NUM_MIXER_OUTPUTS];
+    float Qz[NUM_MIXER_OUTPUTS];
   } mixer_t;
 
   typedef struct
@@ -109,122 +110,145 @@ class Mixer : public ParamListenerInterface
 
   void write_motor(uint8_t index, float value);
   void write_servo(uint8_t index, float value);
+  mixer_t invert_mixer(const mixer_t* mixer_to_invert);
+  void add_header_to_mixer(mixer_t* mixer);
+  void load_primary_mixer_values();
+
+  
 
   // clang-format off
 
   const mixer_t esc_calibration_mixing = {
     {   M,    M,    M,    M,    M,    M, NONE, NONE, NONE, NONE},  // output type
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f},  // F Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // X Mix
+    {0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 0, 0, 0, 0},               // F_x Mix
+    {0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 0, 0, 0, 0},               // F_y Mix
+    {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f},  // F_z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
 
   const mixer_t quadcopter_plus_mixing = {
-    {   M,     M,     M,     M, NONE, NONE, NONE, NONE, NONE, NONE},  // output_type
-    { 490,   490,   490,   490,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {5098.031218379775, 5098.0312183797805, 5098.031218379778, 5098.0312183797805, 0, 0, 0, 0, 0, 0},
-    {0.0, -40784.24974703961, 0.0, 40784.24974703687, 0, 0, 0, 0, 0, 0},
-    {40784.249747038244, 0.0, -40784.24974703822, 0.0, 0, 0, 0, 0, 0, 0},
-    {223010.98943043666, -223010.98943043654, 223010.98943043666, -223010.98943043657, 0, 0, 0, 0, 0, 0}};
+    {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE},            // output type
+    {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},  // F_z Mix
+    { 0.0000f, -1.0000f,  0.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},  // Q_x Mix
+    { 1.0000f,  0.0000f, -1.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix 
 
   const mixer_t quadcopter_x_mixing = {
-    {    M,     M,     M,     M, NONE, NONE, NONE, NONE, NONE, NONE},  // output_type
-    {  490,   490,   490,   490,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {5098.031218379779, 5098.03121837978, 5098.031218379777, 5098.031218379782, 0, 0, 0, 0, 0, 0},
-    {-28838.81956173643, -28838.819561736505, 28838.81956173648, 28838.819561736465, 0, 0, 0, 0, 0, 0},
-    {28838.819561736495, -28838.819561736498, -28838.819561736433, 28838.81956173645, 0, 0, 0, 0, 0, 0},
-    {223010.9894304366, -223010.9894304366, 223010.98943043666, -223010.9894304366, 0, 0, 0, 0, 0, 0}};
+    {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE},             // output type
+    {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                 // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},   // F_z Mix
+    {-0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_x Mix
+    { 0.7071f, -0.7071f, -0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}};  // Q_z Mix
 
   const mixer_t hex_plus_mixing = {
-    {   M,          M,          M,     M,         M,         M, NONE, NONE, NONE, NONE},  // output_type
-    { 490,        490,        490,   490,       490,       490,   490,   490,   50,   50},  // Rate (Hz)
-    {3398.687478919854, 3398.6874789198537, 3398.6874789198537, 3398.6874789198523, 3398.6874789198537, 3398.6874789198537, 0, 0, 0, 0},
-    {0.0, -23546.797570149454, -23546.797570149447, 0.0, 23546.797570149443, 23546.79757014946, 0, 0, 0, 0},
-    {27189.49983135882, 13594.749915679433, -13594.749915679424, -27189.49983135882, -13594.749915679417, 13594.749915679415, 0, 0, 0, 0},
-    {148673.99295362437, -148673.99295362443, 148673.99295362437, -148673.99295362437, 148673.9929536245, -148673.9929536244, 0, 0, 0, 0}};
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    { 0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
+    { 1.0000f,  0.5000f, -0.5000f, -1.0000f, -0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t hex_x_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {3398.687478919854, 3398.687478919853, 3398.687478919853, 3398.6874789198523, 3398.687478919854, 3398.687478919854, 0, 0, 0, 0},
-    {-13594.749915679407, -27189.49983135883, -13594.74991567941, 13594.749915679384, 27189.49983135884, 13594.74991567942, 0, 0, 0, 0},
-    {23546.797570149465, 0.0, -23546.797570149454, -23546.797570149472, 0.0, 23546.797570149465, 0, 0, 0, 0},
-    {148673.99295362426, -148673.9929536243, 148673.99295362443, -148673.99295362443, 148673.99295362446, -148673.99295362437, 0, 0, 0, 0}};
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    {-0.5000f, -1.0000f, -0.5000f,  0.5000f,  1.0000f,  0.5000f, 0, 0, 0, 0},  // Q_x Mix
+    { 0.8660f,  0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f, 0, 0, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t octocopter_plus_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {2549.0156091898907, 2549.0156091898903, 2549.01560918989, 2549.0156091898893, 2549.015609189889, 2549.0156091898893, 2549.01560918989, 2549.0156091898903, 0, 0},
-    {0.0, -14419.409780868224, -20392.124873519115, -14419.409780868233, 0.0, 14419.409780868235, 20392.124873519115, 14419.409780868242, 0, 0},
-    {20392.124873519126, 14419.409780868244, 0.0, -14419.409780868249, -20392.124873519126, -14419.409780868249, 0.0, 14419.409780868238, 0, 0},
-    {111505.49471521821, -111505.4947152183, 111505.49471521829, -111505.49471521829, 111505.49471521827, -111505.49471521826, 111505.49471521827, -111505.49471521827, 0, 0}};
+    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    { 0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f,  1.0000f,  0.7071f, 0, 0},  // Q_x Mix
+    { 1.0000f,  0.7071f,  0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t octocopter_x_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {2549.0156091898893, 2549.01560918989, 2549.01560918989, 2549.0156091898903, 2549.0156091898903, 2549.01560918989, 2549.01560918989, 2549.0156091898893, 0, 0},
-    {-7803.728339815821, -18839.866795058642, -18839.86679505863, -7803.728339815828, 7803.728339815823, 18839.86679505863, 18839.86679505864, 7803.728339815834, 0, 0},
-    {18839.86679505862, 7803.728339815822, -7803.728339815816, -18839.866795058624, -18839.86679505863, -7803.728339815828, 7803.728339815817, 18839.86679505862, 0, 0},
-    {111505.49471521824, -111505.49471521845, 111505.49471521832, -111505.49471521827, 111505.49471521837, -111505.49471521827, 111505.49471521833, -111505.49471521836, 0, 0}};
+    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    {-0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f,  0.9239f,  0.3827f, 0, 0},  // Q_x Mix
+    { 0.9239f,  0.3827f, -0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t Y6_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {3398.6874789198546, 3398.6874789198523, 3398.6874789198537, 3398.6874789198546, 3398.687478919853, 3398.6874789198528, 0, 0, 0, 0},
-    {-23546.79757014947, 0.0, 23546.797570149454, -23546.79757014944, 0.0, 23546.79757014947, 0, 0, 0, 0},
-    {13594.749915679397, -27189.499831358822, 13594.749915679413, 13594.749915679437, -27189.499831358844, 13594.749915679426, 0, 0, 0, 0},
-    {148673.9929536246, -148673.9929536245, 148673.9929536245, -148673.99295362452, 148673.99295362446, -148673.9929536245, 0, 0, 0, 0}};
+    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    {-0.8660f, -0.8660f,  0.0000f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
+    { 0.5000f,  0.5000f, -1.0000f, -1.0000f,  0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t X8_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {2549.01560918989, 2549.01560918989, 2549.0156091898893, 2549.0156091898893, 2549.01560918989, 2549.01560918989, 2549.0156091898903, 2549.0156091898903, 0, 0},
-    {-14419.409780868245, -14419.409780868247, -14419.40978086824, -14419.409780868236, 14419.409780868244, 14419.409780868247, 14419.40978086824, 14419.409780868244, 0, 0},
-    {14419.409780868236, 14419.40978086824, -14419.409780868247, -14419.409780868247, -14419.40978086824, -14419.409780868244, 14419.409780868249, 14419.409780868245, 0, 0},
-    {111505.49471521829, -111505.49471521827, 111505.49471521823, -111505.49471521829, 111505.49471521827, -111505.49471521826, 111505.49471521829, -111505.49471521823, 0, 0}};
-
-  const mixer_t tricopter_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},
-    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},
-    {3398.687478919855, 3398.687478919855, 3398.6874789198528, 3398.687478919852, 3398.6874789198537, 3398.6874789198528, 0, 0, 0, 0},
-    {-23546.797570149458, -23546.797570149447, 0.0, 0.0, 23546.797570149454, 23546.797570149465, 0, 0, 0, 0},
-    {13594.749915679417, 13594.74991567943, -27189.499831358844, -27189.49983135883, 13594.749915679406, 13594.749915679418, 0, 0, 0, 0},
-    {148673.99295362437, -148673.99295362429, 148673.99295362443, -148673.9929536245, 148673.9929536244, -148673.99295362443, 0, 0, 0, 0}};
+    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
+    { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
+    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    {-0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_x Mix
+    { 0.7071f,  0.7071f, -0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_y Mix
+    { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t fixedwing_mixing = {
     {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
     { 50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix 
+    {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
+    {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
+    {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t fixedwing_inverted_vtail_mixing = {
     {   S, NONE, NONE,     S,    S,    M, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
     {  50,   50,   50,    50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {1.0f, 0.0f, 0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {0.0f, 0.0f, 0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_y Mix
+    {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix 
+    {1.0f, 0.0f, 0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
+    {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
+    {0.0f, 0.0f, 0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t passthrough_mixing = {
     {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
 
   const mixer_t custom_mixing = {
     {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // X Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
 
-  mixer_t custom_mixer_;
+  mixer_t primary_mixer_;
 
   const mixer_t * mixer_to_use_;
 
@@ -238,7 +262,6 @@ class Mixer : public ParamListenerInterface
     &octocopter_x_mixing,
     &Y6_mixing,
     &X8_mixing,
-    &tricopter_mixing,
     &fixedwing_mixing,
     &passthrough_mixing,
     &fixedwing_inverted_vtail_mixing,
diff --git a/include/param.h b/include/param.h
index d24f0cc0..dca700b0 100644
--- a/include/param.h
+++ b/include/param.h
@@ -95,6 +95,76 @@ enum : uint16_t
   PARAM_MOTOR_8_PSI,
   PARAM_MOTOR_9_PSI,
 
+  PARAM_PRIMARY_MIXER_1_1,
+  PARAM_PRIMARY_MIXER_2_1,
+  PARAM_PRIMARY_MIXER_3_1,
+  PARAM_PRIMARY_MIXER_4_1,
+  PARAM_PRIMARY_MIXER_5_1,
+  PARAM_PRIMARY_MIXER_6_1,
+
+  PARAM_PRIMARY_MIXER_1_2,
+  PARAM_PRIMARY_MIXER_2_2,
+  PARAM_PRIMARY_MIXER_3_2,
+  PARAM_PRIMARY_MIXER_4_2,
+  PARAM_PRIMARY_MIXER_5_2,
+  PARAM_PRIMARY_MIXER_6_2,
+
+  PARAM_PRIMARY_MIXER_1_3,
+  PARAM_PRIMARY_MIXER_2_3,
+  PARAM_PRIMARY_MIXER_3_3,
+  PARAM_PRIMARY_MIXER_4_3,
+  PARAM_PRIMARY_MIXER_5_3,
+  PARAM_PRIMARY_MIXER_6_3,
+
+  PARAM_PRIMARY_MIXER_1_4,
+  PARAM_PRIMARY_MIXER_2_4,
+  PARAM_PRIMARY_MIXER_3_4,
+  PARAM_PRIMARY_MIXER_4_4,
+  PARAM_PRIMARY_MIXER_5_4,
+  PARAM_PRIMARY_MIXER_6_4,
+
+  PARAM_PRIMARY_MIXER_1_5,
+  PARAM_PRIMARY_MIXER_2_5,
+  PARAM_PRIMARY_MIXER_3_5,
+  PARAM_PRIMARY_MIXER_4_5,
+  PARAM_PRIMARY_MIXER_5_5,
+  PARAM_PRIMARY_MIXER_6_5,
+
+  PARAM_PRIMARY_MIXER_1_6,
+  PARAM_PRIMARY_MIXER_2_6,
+  PARAM_PRIMARY_MIXER_3_6,
+  PARAM_PRIMARY_MIXER_4_6,
+  PARAM_PRIMARY_MIXER_5_6,
+  PARAM_PRIMARY_MIXER_6_6,
+
+  PARAM_PRIMARY_MIXER_1_7,
+  PARAM_PRIMARY_MIXER_2_7,
+  PARAM_PRIMARY_MIXER_3_7,
+  PARAM_PRIMARY_MIXER_4_7,
+  PARAM_PRIMARY_MIXER_5_7,
+  PARAM_PRIMARY_MIXER_6_7,
+
+  PARAM_PRIMARY_MIXER_1_8,
+  PARAM_PRIMARY_MIXER_2_8,
+  PARAM_PRIMARY_MIXER_3_8,
+  PARAM_PRIMARY_MIXER_4_8,
+  PARAM_PRIMARY_MIXER_5_8,
+  PARAM_PRIMARY_MIXER_6_8,
+  
+  PARAM_PRIMARY_MIXER_1_9,
+  PARAM_PRIMARY_MIXER_2_9,
+  PARAM_PRIMARY_MIXER_3_9,
+  PARAM_PRIMARY_MIXER_4_9,
+  PARAM_PRIMARY_MIXER_5_9,
+  PARAM_PRIMARY_MIXER_6_9,
+                          
+  PARAM_PRIMARY_MIXER_1_10,
+  PARAM_PRIMARY_MIXER_2_10,
+  PARAM_PRIMARY_MIXER_3_10,
+  PARAM_PRIMARY_MIXER_4_10,
+  PARAM_PRIMARY_MIXER_5_10,
+  PARAM_PRIMARY_MIXER_6_10,
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 89da6bb1..71a84f4b 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -110,10 +110,27 @@ void Mixer::init_mixing()
     mixer_to_use_ = array_of_mixers_[mixer_choice];
 
     if (mixer_to_use_ == &custom_mixing) {
+      // If specified, compute the custom mixing matrix per the motor and propeller parameters
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                             "Calculating custom mixer values...");
-      calculate_mixer_values();
-      mixer_to_use_ = &custom_mixer_;
+      load_primary_mixer_values();
+      
+      // TODO: Add these as parameters in the firmware, otherwise we can't control S vs M for custom mixers
+      // Add the header values (PWM rate and output type) to mixer
+      add_header_to_mixer(&primary_mixer_);
+      mixer_to_use_ = &primary_mixer_;
+    } else if (mixer_to_use_ != &fixedwing_mixing ||
+               mixer_to_use_ != &fixedwing_inverted_vtail_mixing) {
+      // Don't invert the fixedwing mixers
+
+      // TODO: Test to see if selecting passthrough mixing will break the inversion
+      // TODO: Is there any reason to use passthrough mixing?
+      // Otherwise, invert the selected "canned" matrix
+      primary_mixer_ = invert_mixer(mixer_to_use_);
+      
+      // Add the header values (PWM rate and output type) to mixer
+      add_header_to_mixer(&primary_mixer_);
+      mixer_to_use_ = &primary_mixer_;
     }
   }
 
@@ -161,59 +178,151 @@ void Mixer::update_parameters()
   psi_[9] = RF_.params_.get_param_float(PARAM_MOTOR_9_PSI);
 }
 
-void Mixer::calculate_mixer_values()
+Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
 {
-  // Create the mixing matrix as defined in Ch. 14. Initialize the matrix to zero.
-  Eigen::Matrix<double, 4, 10> mixer_matrix;
+  Eigen::Matrix<float, 6, NUM_MIXER_OUTPUTS> mixer_matrix;
   mixer_matrix.setZero();
 
-  // Fill in the mixing matrix with the correct values according to the number of motors
-  for (int i = 0; i < num_motors_; ++i) {
-    // Determine the direction of the yaw moment generated by the motor i
-    // Note: This assumes that there are an even number of motors and that they are placed so that every other motor spins the same direction
-    int yaw_dir = (i % 2 == 0) ? 1 : -1;
-
-    mixer_matrix(0, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_;
-    mixer_matrix(1, i) =
-      rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * (-sin(psi_[i]));
-    mixer_matrix(2, i) = rho_ * pow(D_, 4.0) / (4 * pow(M_PI, 2.0)) * C_T_ * l_[i] * cos(psi_[i]);
-    mixer_matrix(3, i) = rho_ * pow(D_, 5.0) / (4 * pow(M_PI, 2.0)) * yaw_dir * C_Q_;
+  // Convert the mixer_t to an Eigen matrix
+  for (int i=0; i<NUM_MIXER_OUTPUTS; i++) {
+    mixer_matrix(0, i) = mixer_to_invert->Fx[i];
+    mixer_matrix(1, i) = mixer_to_invert->Fy[i];
+    mixer_matrix(2, i) = mixer_to_invert->Fz[i];
+    mixer_matrix(3, i) = mixer_to_invert->Qx[i];
+    mixer_matrix(4, i) = mixer_to_invert->Qy[i];
+    mixer_matrix(5, i) = mixer_to_invert->Qz[i];
   }
 
   // Calculate the pseudoinverse of the mixing matrix using the SVD
-  Eigen::JacobiSVD<Eigen::Matrix<double, 4, NUM_MIXER_OUTPUTS>> svd(
+  Eigen::JacobiSVD<Eigen::Matrix<float, 6, NUM_MIXER_OUTPUTS>> svd(
     mixer_matrix,
     Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
-  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 4> Sig;
+  Eigen::Matrix<float, NUM_MIXER_OUTPUTS, 6> Sig;
   Sig.setZero();
   Sig(0, 0) = 1.0 / svd.singularValues()[0];
   Sig(1, 1) = 1.0 / svd.singularValues()[1];
   Sig(2, 2) = 1.0 / svd.singularValues()[2];
   Sig(3, 3) = 1.0 / svd.singularValues()[3];
+  Sig(4, 4) = 1.0 / svd.singularValues()[4];
+  Sig(5, 5) = 1.0 / svd.singularValues()[5];
 
   // Pseudoinverse of the mixing matrix
-  Eigen::Matrix<double, NUM_MIXER_OUTPUTS, 4> mixer_matrix_pinv =
+  Eigen::Matrix<float, NUM_MIXER_OUTPUTS, 6> mixer_matrix_pinv =
     svd.matrixV() * Sig * svd.matrixU().transpose();
 
   // Fill in the mixing matrix from the inverted matrix above
+  mixer_t inverted_mixer;
+
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (i < num_motors_) {
-      custom_mixer_.default_pwm_rate[i] = 490;
-      custom_mixer_.output_type[i] = M;
-      custom_mixer_.F[i] = mixer_matrix_pinv(i, 0);
-      custom_mixer_.x[i] = mixer_matrix_pinv(i, 1);
-      custom_mixer_.y[i] = mixer_matrix_pinv(i, 2);
-      custom_mixer_.z[i] = mixer_matrix_pinv(i, 3);
+    if (i < RF_.params_.get_param_int(PARAM_NUM_MOTORS)) {
+      inverted_mixer.output_type[i] = M;
+      inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0);
+      inverted_mixer.Fy[i] = mixer_matrix_pinv(i, 1);
+      inverted_mixer.Fz[i] = mixer_matrix_pinv(i, 2);
+      inverted_mixer.Qx[i] = mixer_matrix_pinv(i, 3);
+      inverted_mixer.Qy[i] = mixer_matrix_pinv(i, 4);
+      inverted_mixer.Qz[i] = mixer_matrix_pinv(i, 5);
     } else {
-      // Set the rest of the ouput types to NONE with a default PWM rate of 50
-      custom_mixer_.default_pwm_rate[i] = 50;
-      custom_mixer_.output_type[i] = NONE;
-      custom_mixer_.F[i] = 0.0;
-      custom_mixer_.x[i] = 0.0;
-      custom_mixer_.y[i] = 0.0;
-      custom_mixer_.z[i] = 0.0;
+      // Set the rest of the ouput types to NONE
+      inverted_mixer.output_type[i] = NONE;
+      inverted_mixer.Fx[i] = 0.0;
+      inverted_mixer.Fy[i] = 0.0;
+      inverted_mixer.Fz[i] = 0.0;
+      inverted_mixer.Qx[i] = 0.0;
+      inverted_mixer.Qy[i] = 0.0;
+      inverted_mixer.Qz[i] = 0.0;
     }
   }
+
+  return inverted_mixer;
+}
+
+void Mixer::add_header_to_mixer(mixer_t* mixer)
+{
+  // Fill in the default PWM rates to use in the header of the mixer
+  for (int i = 0; i < NUM_MIXER_OUTPUTS; i++){
+    if (i < RF_.params_.get_param_int(PARAM_NUM_MOTORS) || 
+        (RF_.params_.get_param_int(PARAM_NUM_MOTORS) > 4 && i < 8)) {
+      // This makes sure the PWM groups are correct for the hardware
+      mixer->default_pwm_rate[i] = 490;
+    } else {
+      mixer->default_pwm_rate[i] = 50;
+    }
+  }
+}
+
+void Mixer::load_primary_mixer_values()
+{
+  // Load the mixer values from the firmware parameters
+  primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
+  primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
+  primary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_1);
+  primary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_1);
+  primary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_1);
+  primary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_1);
+
+  primary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_2);
+  primary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_2);
+  primary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_2);
+  primary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_2);
+  primary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_2);
+  primary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_2);
+
+  primary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_3);
+  primary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_3);
+  primary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_3);
+  primary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_3);
+  primary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_3);
+  primary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_3);
+
+  primary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_4);
+  primary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_4);
+  primary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_4);
+  primary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_4);
+  primary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_4);
+  primary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_4);
+
+  primary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_5);
+  primary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_5);
+  primary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_5);
+  primary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_5);
+  primary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_5);
+  primary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_5);
+
+  primary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_6);
+  primary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_6);
+  primary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_6);
+  primary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_6);
+  primary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_6);
+  primary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_6);
+
+  primary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_7);
+  primary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_7);
+  primary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_7);
+  primary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_7);
+  primary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_7);
+  primary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_7);
+
+  primary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_8);
+  primary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_8);
+  primary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_8);
+  primary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_8);
+  primary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_8);
+  primary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_8);
+
+  primary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_9);
+  primary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_9);
+  primary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_9);
+  primary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_9);
+  primary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_9);
+  primary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_9);
+
+  primary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_10);
+  primary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_10);
+  primary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_10);
+  primary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_10);
+  primary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_10);
+  primary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_10);
 }
 
 void Mixer::init_PWM()
@@ -279,9 +388,10 @@ void Mixer::mix_multirotor()
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      double omega_squared =
-        (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
-         + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
+      // TODO: Update the commands vector to include fx and fy 
+      float omega_squared =
+        (commands.F * mixer_to_use_->Fz[i] + commands.x * mixer_to_use_->Qx[i]
+         + commands.y * mixer_to_use_->Qy[i] + commands.z * mixer_to_use_->Qz[i]);
 
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
@@ -289,7 +399,7 @@ void Mixer::mix_multirotor()
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
       // Note that we assume constant airspeed and propeller speed, leading to constant advance ratio,
       // torque, and thrust constants.
-      double V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
+      float V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
       // Convert desired V_in setting to a throttle setting
@@ -329,8 +439,8 @@ void Mixer::mix_fixedwing()
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      outputs_[i] = (commands.F * mixer_to_use_->F[i] + commands.x * mixer_to_use_->x[i]
-                     + commands.y * mixer_to_use_->y[i] + commands.z * mixer_to_use_->z[i]);
+      outputs_[i] = (commands.F * mixer_to_use_->Fz[i] + commands.x * mixer_to_use_->Qx[i]
+                     + commands.y * mixer_to_use_->Qy[i] + commands.z * mixer_to_use_->Qz[i]);
 
     }
   }
diff --git a/src/param.cpp b/src/param.cpp
index 27af30e8..eacaed0b 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -141,6 +141,76 @@ void Params::set_defaults(void)
   init_param_float(PARAM_MOTOR_8_PSI, "MOTOR_8_PSI", 0.0f); // Angular position of motor 8 from the body from x axis (rad) | 0.0 | 6.28319
   init_param_float(PARAM_MOTOR_9_PSI, "MOTOR_9_PSI", 0.0f); // Angular position of motor 9 from the body from x axis (rad) | 0.0 | 6.28319
 
+  init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRIMARY_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
+  init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRIMARY_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
+  init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRIMARY_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
+  init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRIMARY_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
+  init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRIMARY_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
+  init_param_float(PARAM_PRIMARY_MIXER_6_1, "PRIMARY_MIXER_6_1", 0.0f); // Value of the custom mixer at element [6,1]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRIMARY_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
+  init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRIMARY_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
+  init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRIMARY_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
+  init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRIMARY_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
+  init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRIMARY_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
+  init_param_float(PARAM_PRIMARY_MIXER_6_2, "PRIMARY_MIXER_6_2", 0.0f); // Value of the custom mixer at element [6,2]
+                                                                    
+  init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRIMARY_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
+  init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRIMARY_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
+  init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRIMARY_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
+  init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRIMARY_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
+  init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRIMARY_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
+  init_param_float(PARAM_PRIMARY_MIXER_6_3, "PRIMARY_MIXER_6_3", 0.0f); // Value of the custom mixer at element [6,3]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRIMARY_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
+  init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRIMARY_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
+  init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRIMARY_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
+  init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRIMARY_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
+  init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRIMARY_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
+  init_param_float(PARAM_PRIMARY_MIXER_6_4, "PRIMARY_MIXER_6_4", 0.0f); // Value of the custom mixer at element [6,4]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRIMARY_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
+  init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRIMARY_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
+  init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRIMARY_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
+  init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRIMARY_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
+  init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRIMARY_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
+  init_param_float(PARAM_PRIMARY_MIXER_6_5, "PRIMARY_MIXER_6_5", 0.0f); // Value of the custom mixer at element [6,5]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRIMARY_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
+  init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRIMARY_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
+  init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRIMARY_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
+  init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRIMARY_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
+  init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRIMARY_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
+  init_param_float(PARAM_PRIMARY_MIXER_6_6, "PRIMARY_MIXER_6_6", 0.0f); // Value of the custom mixer at element [6,6]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRIMARY_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
+  init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRIMARY_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
+  init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRIMARY_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
+  init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRIMARY_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
+  init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRIMARY_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
+  init_param_float(PARAM_PRIMARY_MIXER_6_7, "PRIMARY_MIXER_6_7", 0.0f); // Value of the custom mixer at element [6,7]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRIMARY_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
+  init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRIMARY_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
+  init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRIMARY_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
+  init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRIMARY_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
+  init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRIMARY_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
+  init_param_float(PARAM_PRIMARY_MIXER_6_8, "PRIMARY_MIXER_6_8", 0.0f); // Value of the custom mixer at element [6,8]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRIMARY_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
+  init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRIMARY_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
+  init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRIMARY_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
+  init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRIMARY_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
+  init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRIMARY_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
+  init_param_float(PARAM_PRIMARY_MIXER_6_9, "PRIMARY_MIXER_6_9", 0.0f); // Value of the custom mixer at element [6,9]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_10, "PRIMARY_MIXER_1_10", 0.0f); // Value of the custom mixer at element [1,10]
+  init_param_float(PARAM_PRIMARY_MIXER_2_10, "PRIMARY_MIXER_2_10", 0.0f); // Value of the custom mixer at element [2,10]
+  init_param_float(PARAM_PRIMARY_MIXER_3_10, "PRIMARY_MIXER_3_10", 0.0f); // Value of the custom mixer at element [3,10]
+  init_param_float(PARAM_PRIMARY_MIXER_4_10, "PRIMARY_MIXER_4_10", 0.0f); // Value of the custom mixer at element [4,10]
+  init_param_float(PARAM_PRIMARY_MIXER_5_10, "PRIMARY_MIXER_5_10", 0.0f); // Value of the custom mixer at element [5,10]
+  init_param_float(PARAM_PRIMARY_MIXER_6_10, "PRIMARY_MIXER_6_10", 0.0f); // Value of the custom mixer at element [6,10]
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/

From a3ac22ba14598cf65b0a443807565b60703e7a7a Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 29 Oct 2024 13:07:42 -0600
Subject: [PATCH 24/41] removed unused parameters and fixed mixing matrix
 paramter names

---
 include/param.h |  22 -------
 src/mixer.cpp   |  48 +--------------
 src/param.cpp   | 160 +++++++++++++++++++++---------------------------
 3 files changed, 72 insertions(+), 158 deletions(-)

diff --git a/include/param.h b/include/param.h
index dca700b0..d42518df 100644
--- a/include/param.h
+++ b/include/param.h
@@ -73,28 +73,6 @@ enum : uint16_t
   PARAM_PROP_CQ,
   PARAM_VOLT_MAX,
 
-  PARAM_MOTOR_0_POS,
-  PARAM_MOTOR_1_POS,
-  PARAM_MOTOR_2_POS,
-  PARAM_MOTOR_3_POS,
-  PARAM_MOTOR_4_POS,
-  PARAM_MOTOR_5_POS,
-  PARAM_MOTOR_6_POS,
-  PARAM_MOTOR_7_POS,
-  PARAM_MOTOR_8_POS,
-  PARAM_MOTOR_9_POS,
-
-  PARAM_MOTOR_0_PSI,
-  PARAM_MOTOR_1_PSI,
-  PARAM_MOTOR_2_PSI,
-  PARAM_MOTOR_3_PSI,
-  PARAM_MOTOR_4_PSI,
-  PARAM_MOTOR_5_PSI,
-  PARAM_MOTOR_6_PSI,
-  PARAM_MOTOR_7_PSI,
-  PARAM_MOTOR_8_PSI,
-  PARAM_MOTOR_9_PSI,
-
   PARAM_PRIMARY_MIXER_1_1,
   PARAM_PRIMARY_MIXER_2_1,
   PARAM_PRIMARY_MIXER_3_1,
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 71a84f4b..d9716326 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -34,6 +34,7 @@
 #include "rosflight.h"
 
 #include <cstdint>
+#include <iostream>
 
 namespace rosflight_firmware
 {
@@ -58,26 +59,6 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PROP_CQ:
     case PARAM_NUM_MOTORS:
     case PARAM_VOLT_MAX:
-    case PARAM_MOTOR_0_POS:
-    case PARAM_MOTOR_1_POS:
-    case PARAM_MOTOR_2_POS:
-    case PARAM_MOTOR_3_POS:
-    case PARAM_MOTOR_4_POS:
-    case PARAM_MOTOR_5_POS:
-    case PARAM_MOTOR_6_POS:
-    case PARAM_MOTOR_7_POS:
-    case PARAM_MOTOR_8_POS:
-    case PARAM_MOTOR_9_POS:
-    case PARAM_MOTOR_0_PSI:
-    case PARAM_MOTOR_1_PSI:
-    case PARAM_MOTOR_2_PSI:
-    case PARAM_MOTOR_3_PSI:
-    case PARAM_MOTOR_4_PSI:
-    case PARAM_MOTOR_5_PSI:
-    case PARAM_MOTOR_6_PSI:
-    case PARAM_MOTOR_7_PSI:
-    case PARAM_MOTOR_8_PSI:
-    case PARAM_MOTOR_9_PSI:
       init_mixing();
       break;
     case PARAM_MOTOR_PWM_SEND_RATE:
@@ -92,9 +73,6 @@ void Mixer::param_change_callback(uint16_t param_id)
 
 void Mixer::init_mixing()
 {
-  // Update parameters
-  update_parameters();
-
   // clear the invalid mixer error
   RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER);
 
@@ -123,6 +101,8 @@ void Mixer::init_mixing()
                mixer_to_use_ != &fixedwing_inverted_vtail_mixing) {
       // Don't invert the fixedwing mixers
 
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+                            "Inverting selected mixing matrix...");
       // TODO: Test to see if selecting passthrough mixing will break the inversion
       // TODO: Is there any reason to use passthrough mixing?
       // Otherwise, invert the selected "canned" matrix
@@ -154,28 +134,6 @@ void Mixer::update_parameters()
   C_Q_ = RF_.params_.get_param_float(PARAM_PROP_CQ);
   num_motors_ = RF_.params_.get_param_int(PARAM_NUM_MOTORS);
   V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX);
-
-  l_[0] = RF_.params_.get_param_float(PARAM_MOTOR_0_POS);
-  l_[1] = RF_.params_.get_param_float(PARAM_MOTOR_1_POS);
-  l_[2] = RF_.params_.get_param_float(PARAM_MOTOR_2_POS);
-  l_[3] = RF_.params_.get_param_float(PARAM_MOTOR_3_POS);
-  l_[4] = RF_.params_.get_param_float(PARAM_MOTOR_4_POS);
-  l_[5] = RF_.params_.get_param_float(PARAM_MOTOR_5_POS);
-  l_[6] = RF_.params_.get_param_float(PARAM_MOTOR_6_POS);
-  l_[7] = RF_.params_.get_param_float(PARAM_MOTOR_7_POS);
-  l_[8] = RF_.params_.get_param_float(PARAM_MOTOR_8_POS);
-  l_[9] = RF_.params_.get_param_float(PARAM_MOTOR_9_POS);
-
-  psi_[0] = RF_.params_.get_param_float(PARAM_MOTOR_0_PSI);
-  psi_[1] = RF_.params_.get_param_float(PARAM_MOTOR_1_PSI);
-  psi_[2] = RF_.params_.get_param_float(PARAM_MOTOR_2_PSI);
-  psi_[3] = RF_.params_.get_param_float(PARAM_MOTOR_3_PSI);
-  psi_[4] = RF_.params_.get_param_float(PARAM_MOTOR_4_PSI);
-  psi_[5] = RF_.params_.get_param_float(PARAM_MOTOR_5_PSI);
-  psi_[6] = RF_.params_.get_param_float(PARAM_MOTOR_6_PSI);
-  psi_[7] = RF_.params_.get_param_float(PARAM_MOTOR_7_PSI);
-  psi_[8] = RF_.params_.get_param_float(PARAM_MOTOR_8_PSI);
-  psi_[9] = RF_.params_.get_param_float(PARAM_MOTOR_9_PSI);
 }
 
 Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
diff --git a/src/param.cpp b/src/param.cpp
index eacaed0b..bc916eae 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -119,97 +119,75 @@ void Params::set_defaults(void)
   init_param_float(PARAM_PROP_CQ, "PROP_CQ", 0.0045f); // Torque coefficient of the propeller | 0 | 100.0
   init_param_float(PARAM_VOLT_MAX, "VOLT_MAX", 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0
 
-  init_param_float(PARAM_MOTOR_0_POS, "MOTOR_0_POS", 0.25f); // Radial position of motor 0 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_1_POS, "MOTOR_1_POS", 0.25f); // Radial position of motor 1 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_2_POS, "MOTOR_2_POS", 0.25f); // Radial position of motor 2 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_3_POS, "MOTOR_3_POS", 0.25f); // Radial position of motor 3 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_4_POS, "MOTOR_4_POS", 0.0f); // Radial position of motor 4 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_5_POS, "MOTOR_5_POS", 0.0f); // Radial position of motor 5 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_6_POS, "MOTOR_6_POS", 0.0f); // Radial position of motor 6 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_7_POS, "MOTOR_7_POS", 0.0f); // Radial position of motor 7 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_8_POS, "MOTOR_8_POS", 0.0f); // Radial position of motor 8 from the center of mass (m) | 0.0 | 1000.0
-  init_param_float(PARAM_MOTOR_9_POS, "MOTOR_9_POS", 0.0f); // Radial position of motor 9 from the center of mass (m) | 0.0 | 1000.0
-
-  init_param_float(PARAM_MOTOR_0_PSI, "MOTOR_0_PSI", M_PI_4); // Angular position of motor 0 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_1_PSI, "MOTOR_1_PSI", 3 * M_PI_4); // Angular position of motor 1 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_2_PSI, "MOTOR_2_PSI", 5 * M_PI_4); // Angular position of motor 2 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_3_PSI, "MOTOR_3_PSI", 7 * M_PI_4); // Angular position of motor 3 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_4_PSI, "MOTOR_4_PSI", 0.0f); // Angular position of motor 4 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_5_PSI, "MOTOR_5_PSI", 0.0f); // Angular position of motor 5 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_6_PSI, "MOTOR_6_PSI", 0.0f); // Angular position of motor 6 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_7_PSI, "MOTOR_7_PSI", 0.0f); // Angular position of motor 7 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_8_PSI, "MOTOR_8_PSI", 0.0f); // Angular position of motor 8 from the body from x axis (rad) | 0.0 | 6.28319
-  init_param_float(PARAM_MOTOR_9_PSI, "MOTOR_9_PSI", 0.0f); // Angular position of motor 9 from the body from x axis (rad) | 0.0 | 6.28319
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRIMARY_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
-  init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRIMARY_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
-  init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRIMARY_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
-  init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRIMARY_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
-  init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRIMARY_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
-  init_param_float(PARAM_PRIMARY_MIXER_6_1, "PRIMARY_MIXER_6_1", 0.0f); // Value of the custom mixer at element [6,1]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRIMARY_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
-  init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRIMARY_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
-  init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRIMARY_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
-  init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRIMARY_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
-  init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRIMARY_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
-  init_param_float(PARAM_PRIMARY_MIXER_6_2, "PRIMARY_MIXER_6_2", 0.0f); // Value of the custom mixer at element [6,2]
-                                                                    
-  init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRIMARY_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
-  init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRIMARY_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
-  init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRIMARY_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
-  init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRIMARY_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
-  init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRIMARY_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
-  init_param_float(PARAM_PRIMARY_MIXER_6_3, "PRIMARY_MIXER_6_3", 0.0f); // Value of the custom mixer at element [6,3]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRIMARY_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
-  init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRIMARY_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
-  init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRIMARY_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
-  init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRIMARY_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
-  init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRIMARY_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
-  init_param_float(PARAM_PRIMARY_MIXER_6_4, "PRIMARY_MIXER_6_4", 0.0f); // Value of the custom mixer at element [6,4]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRIMARY_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
-  init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRIMARY_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
-  init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRIMARY_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
-  init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRIMARY_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
-  init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRIMARY_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
-  init_param_float(PARAM_PRIMARY_MIXER_6_5, "PRIMARY_MIXER_6_5", 0.0f); // Value of the custom mixer at element [6,5]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRIMARY_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
-  init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRIMARY_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
-  init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRIMARY_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
-  init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRIMARY_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
-  init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRIMARY_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
-  init_param_float(PARAM_PRIMARY_MIXER_6_6, "PRIMARY_MIXER_6_6", 0.0f); // Value of the custom mixer at element [6,6]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRIMARY_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
-  init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRIMARY_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
-  init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRIMARY_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
-  init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRIMARY_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
-  init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRIMARY_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
-  init_param_float(PARAM_PRIMARY_MIXER_6_7, "PRIMARY_MIXER_6_7", 0.0f); // Value of the custom mixer at element [6,7]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRIMARY_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
-  init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRIMARY_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
-  init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRIMARY_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
-  init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRIMARY_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
-  init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRIMARY_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
-  init_param_float(PARAM_PRIMARY_MIXER_6_8, "PRIMARY_MIXER_6_8", 0.0f); // Value of the custom mixer at element [6,8]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRIMARY_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
-  init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRIMARY_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
-  init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRIMARY_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
-  init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRIMARY_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
-  init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRIMARY_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
-  init_param_float(PARAM_PRIMARY_MIXER_6_9, "PRIMARY_MIXER_6_9", 0.0f); // Value of the custom mixer at element [6,9]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_10, "PRIMARY_MIXER_1_10", 0.0f); // Value of the custom mixer at element [1,10]
-  init_param_float(PARAM_PRIMARY_MIXER_2_10, "PRIMARY_MIXER_2_10", 0.0f); // Value of the custom mixer at element [2,10]
-  init_param_float(PARAM_PRIMARY_MIXER_3_10, "PRIMARY_MIXER_3_10", 0.0f); // Value of the custom mixer at element [3,10]
-  init_param_float(PARAM_PRIMARY_MIXER_4_10, "PRIMARY_MIXER_4_10", 0.0f); // Value of the custom mixer at element [4,10]
-  init_param_float(PARAM_PRIMARY_MIXER_5_10, "PRIMARY_MIXER_5_10", 0.0f); // Value of the custom mixer at element [5,10]
-  init_param_float(PARAM_PRIMARY_MIXER_6_10, "PRIMARY_MIXER_6_10", 0.0f); // Value of the custom mixer at element [6,10]
+  init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
+  init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
+  init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
+  init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRI_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
+  init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRI_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
+  init_param_float(PARAM_PRIMARY_MIXER_6_1, "PRI_MIXER_6_1", 0.0f); // Value of the custom mixer at element [6,1]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRI_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
+  init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRI_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
+  init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRI_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
+  init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRI_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
+  init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRI_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
+  init_param_float(PARAM_PRIMARY_MIXER_6_2, "PRI_MIXER_6_2", 0.0f); // Value of the custom mixer at element [6,2]
+                                                                
+  init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRI_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
+  init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRI_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
+  init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRI_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
+  init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRI_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
+  init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRI_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
+  init_param_float(PARAM_PRIMARY_MIXER_6_3, "PRI_MIXER_6_3", 0.0f); // Value of the custom mixer at element [6,3]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRI_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
+  init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRI_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
+  init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRI_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
+  init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRI_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
+  init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRI_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
+  init_param_float(PARAM_PRIMARY_MIXER_6_4, "PRI_MIXER_6_4", 0.0f); // Value of the custom mixer at element [6,4]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRI_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
+  init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRI_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
+  init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRI_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
+  init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRI_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
+  init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRI_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
+  init_param_float(PARAM_PRIMARY_MIXER_6_5, "PRI_MIXER_6_5", 0.0f); // Value of the custom mixer at element [6,5]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRI_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
+  init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRI_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
+  init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRI_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
+  init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRI_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
+  init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRI_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
+  init_param_float(PARAM_PRIMARY_MIXER_6_6, "PRI_MIXER_6_6", 0.0f); // Value of the custom mixer at element [6,6]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRI_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
+  init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRI_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
+  init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRI_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
+  init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRI_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
+  init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRI_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
+  init_param_float(PARAM_PRIMARY_MIXER_6_7, "PRI_MIXER_6_7", 0.0f); // Value of the custom mixer at element [6,7]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRI_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
+  init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRI_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
+  init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRI_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
+  init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRI_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
+  init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRI_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
+  init_param_float(PARAM_PRIMARY_MIXER_6_8, "PRI_MIXER_6_8", 0.0f); // Value of the custom mixer at element [6,8]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRI_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
+  init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRI_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
+  init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRI_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
+  init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
+  init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
+  init_param_float(PARAM_PRIMARY_MIXER_6_9, "PRI_MIXER_6_9", 0.0f); // Value of the custom mixer at element [6,9]
+
+  init_param_float(PARAM_PRIMARY_MIXER_1_10, "PRI_MIXER_1_10", 0.0f); // Value of the custom mixer at element [1,10]
+  init_param_float(PARAM_PRIMARY_MIXER_2_10, "PRI_MIXER_2_10", 0.0f); // Value of the custom mixer at element [2,10]
+  init_param_float(PARAM_PRIMARY_MIXER_3_10, "PRI_MIXER_3_10", 0.0f); // Value of the custom mixer at element [3,10]
+  init_param_float(PARAM_PRIMARY_MIXER_4_10, "PRI_MIXER_4_10", 0.0f); // Value of the custom mixer at element [4,10]
+  init_param_float(PARAM_PRIMARY_MIXER_5_10, "PRI_MIXER_5_10", 0.0f); // Value of the custom mixer at element [5,10]
+  init_param_float(PARAM_PRIMARY_MIXER_6_10, "PRI_MIXER_6_10", 0.0f); // Value of the custom mixer at element [6,10]
 
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/

From 3d622039d876939f43c0a1639958b94e6f0f7657 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Thu, 7 Nov 2024 12:06:18 -0700
Subject: [PATCH 25/41] Update the input forces to include Fx and Fy

---
 comms/mavlink/mavlink.cpp     |  22 ++--
 include/command_manager.h     |  40 ++++---
 include/controller.h          |  10 +-
 include/interface/comm_link.h |  10 +-
 include/mixer.h               |  54 ++++------
 src/comm_manager.cpp          |  54 ++++++----
 src/command_manager.cpp       |  63 ++++++-----
 src/controller.cpp            |  64 ++++++------
 src/mixer.cpp                 | 151 ++++++++++++++++++++++-----
 test/command_manager_test.cpp | 190 ++++++++++++++++++----------------
 10 files changed, 398 insertions(+), 260 deletions(-)

diff --git a/comms/mavlink/mavlink.cpp b/comms/mavlink/mavlink.cpp
index 9217c89b..707a3961 100644
--- a/comms/mavlink/mavlink.cpp
+++ b/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); }
 }
diff --git a/include/command_manager.h b/include/command_manager.h
index a22aaaed..2fdce9a5 100644
--- a/include/command_manager.h
+++ b/include/command_manager.h
@@ -61,10 +61,12 @@ typedef struct
 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,34 +79,46 @@ 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, PASSTHROUGH, 0.0},
                                            {true, PASSTHROUGH, 0.0},
@@ -119,10 +133,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
diff --git a/include/controller.h b/include/controller.h
index d9d98e38..0d5e6a99 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -51,10 +51,12 @@ 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);
diff --git a/include/interface/comm_link.h b/include/interface/comm_link.h
index c5a156e0..dc76162d 100644
--- a/include/interface/comm_link.h
+++ b/include/interface/comm_link.h
@@ -85,10 +85,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
diff --git a/include/mixer.h b/include/mixer.h
index 5e4011e9..af400cce 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -62,9 +62,8 @@ class Mixer : public ParamListenerInterface
     Y6 = 7,
     X8 = 8,
     FIXEDWING = 9,
-    PASSTHROUGH = 10,
-    INVERTED_VTAIL = 11,
-    CUSTOM = 12,
+    INVERTED_VTAIL = 10,
+    CUSTOM = 11,
     NUM_MIXERS,
     INVALID_MIXER = 255
   };
@@ -110,20 +109,20 @@ class Mixer : public ParamListenerInterface
 
   void write_motor(uint8_t index, float value);
   void write_servo(uint8_t index, float value);
-  mixer_t invert_mixer(const mixer_t* mixer_to_invert);
   void add_header_to_mixer(mixer_t* mixer);
   void load_primary_mixer_values();
-
-  
+  mixer_t invert_mixer(const mixer_t* mixer_to_invert);
+  float mix_multirotor_with_motor_parameters();
+  float mix_multirotor_without_motor_parameters();
 
   // clang-format off
 
   const mixer_t esc_calibration_mixing = {
-    {   M,    M,    M,    M,    M,    M, NONE, NONE, NONE, NONE},  // output type
+    {   M,    M,    M,    M,    M,    M,    M,    M,    M,    M},  // output type
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 0, 0, 0, 0},               // F_x Mix
-    {0.0,  0.0,  0.0,  0.0,  0.0,  0.0, 0, 0, 0, 0},               // F_y Mix
-    {1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f, 1.0f},  // F_z Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
+    {0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f, 0.1f},  // F_z Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
@@ -133,7 +132,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},  // F_z Mix
+    { 0.2500f,  0.2500f,  0.2500f,  0.2500f, 0, 0, 0, 0, 0, 0},  // F_z Mix
     { 0.0000f, -1.0000f,  0.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},  // Q_x Mix
     { 1.0000f,  0.0000f, -1.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix 
@@ -143,7 +142,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                 // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},   // F_z Mix
+    { 0.2500f,  0.2500f,  0.2500f,  0.2500f, 0, 0, 0, 0, 0, 0},   // F_z Mix
     {-0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_x Mix
     { 0.7071f, -0.7071f, -0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}};  // Q_z Mix
@@ -153,7 +152,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
     { 0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
     { 1.0000f,  0.5000f, -0.5000f, -1.0000f, -0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -163,7 +162,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
     {-0.5000f, -1.0000f, -0.5000f,  0.5000f,  1.0000f,  0.5000f, 0, 0, 0, 0},  // Q_x Mix
     { 0.8660f,  0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -173,7 +172,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
     { 0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f,  1.0000f,  0.7071f, 0, 0},  // Q_x Mix
     { 1.0000f,  0.7071f,  0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -183,7 +182,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
     {-0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f,  0.9239f,  0.3827f, 0, 0},  // Q_x Mix
     { 0.9239f,  0.3827f, -0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -193,7 +192,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0, 0, 0},  // F_z Mix
+    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
     {-0.8660f, -0.8660f,  0.0000f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
     { 0.5000f,  0.5000f, -1.0000f, -1.0000f,  0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -203,7 +202,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f,  1.0000f, 0, 0},  // F_z Mix
+    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
     {-0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_x Mix
     { 0.7071f,  0.7071f, -0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -211,9 +210,9 @@ class Mixer : public ParamListenerInterface
   const mixer_t fixedwing_mixing = {
     {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
     { 50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
+    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix 
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
     {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
     {0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
     {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
@@ -221,23 +220,13 @@ class Mixer : public ParamListenerInterface
   const mixer_t fixedwing_inverted_vtail_mixing = {
     {   S, NONE, NONE,     S,    S,    M, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
     {  50,   50,   50,    50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_x Mix
+    {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_y Mix
-    {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix 
+    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_z Mix
     {1.0f, 0.0f, 0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
     {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
     {0.0f, 0.0f, 0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
-  const mixer_t passthrough_mixing = {
-    {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},
-    {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
-
   const mixer_t custom_mixing = {
     {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
@@ -263,7 +252,6 @@ class Mixer : public ParamListenerInterface
     &Y6_mixing,
     &X8_mixing,
     &fixedwing_mixing,
-    &passthrough_mixing,
     &fixedwing_inverted_vtail_mixing,
     &custom_mixing,
   };
diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
index 44caa7b0..0f81ebb4 100644
--- a/src/comm_manager.cpp
+++ b/src/comm_manager.cpp
@@ -237,36 +237,46 @@ void CommManager::offboard_control_callback(const CommLinkInterface::OffboardCon
 {
   // put values into a new command struct
   control_t new_offboard_command;
-  new_offboard_command.x.value = control.x.value;
-  new_offboard_command.y.value = control.y.value;
-  new_offboard_command.z.value = control.z.value;
-  new_offboard_command.F.value = control.F.value;
+  new_offboard_command.Qx.value = control.Qx.value;
+  new_offboard_command.Qy.value = control.Qy.value;
+  new_offboard_command.Qz.value = control.Qz.value;
+  new_offboard_command.Fx.value = control.Fx.value;
+  new_offboard_command.Fy.value = control.Fy.value;
+  new_offboard_command.Fz.value = control.Fz.value;
 
   // Move flags into standard message
-  new_offboard_command.x.active = control.x.valid;
-  new_offboard_command.y.active = control.y.valid;
-  new_offboard_command.z.active = control.z.valid;
-  new_offboard_command.F.active = control.F.valid;
+  new_offboard_command.Qx.active = control.Qx.valid;
+  new_offboard_command.Qy.active = control.Qy.valid;
+  new_offboard_command.Qz.active = control.Qz.valid;
+  new_offboard_command.Fx.active = control.Fx.valid;
+  new_offboard_command.Fy.active = control.Fy.valid;
+  new_offboard_command.Fz.active = control.Fz.valid;
 
   // translate modes into standard message
   switch (control.mode) {
     case CommLinkInterface::OffboardControl::Mode::PASS_THROUGH:
-      new_offboard_command.x.type = PASSTHROUGH;
-      new_offboard_command.y.type = PASSTHROUGH;
-      new_offboard_command.z.type = PASSTHROUGH;
-      new_offboard_command.F.type = PASSTHROUGH;
+      new_offboard_command.Qx.type = PASSTHROUGH;
+      new_offboard_command.Qy.type = PASSTHROUGH;
+      new_offboard_command.Qz.type = PASSTHROUGH;
+      new_offboard_command.Fx.type = PASSTHROUGH;
+      new_offboard_command.Fy.type = PASSTHROUGH;
+      new_offboard_command.Fz.type = PASSTHROUGH;
       break;
     case CommLinkInterface::OffboardControl::Mode::ROLLRATE_PITCHRATE_YAWRATE_THROTTLE:
-      new_offboard_command.x.type = RATE;
-      new_offboard_command.y.type = RATE;
-      new_offboard_command.z.type = RATE;
-      new_offboard_command.F.type = THROTTLE;
+      new_offboard_command.Qx.type = RATE;
+      new_offboard_command.Qy.type = RATE;
+      new_offboard_command.Qz.type = RATE;
+      new_offboard_command.Fx.type = THROTTLE;
+      new_offboard_command.Fy.type = THROTTLE;
+      new_offboard_command.Fz.type = THROTTLE;
       break;
     case CommLinkInterface::OffboardControl::Mode::ROLL_PITCH_YAWRATE_THROTTLE:
-      new_offboard_command.x.type = ANGLE;
-      new_offboard_command.y.type = ANGLE;
-      new_offboard_command.z.type = RATE;
-      new_offboard_command.F.type = THROTTLE;
+      new_offboard_command.Qx.type = ANGLE;
+      new_offboard_command.Qy.type = ANGLE;
+      new_offboard_command.Qz.type = RATE;
+      new_offboard_command.Fx.type = THROTTLE;
+      new_offboard_command.Fy.type = THROTTLE;
+      new_offboard_command.Fz.type = THROTTLE;
       break;
   }
 
@@ -351,9 +361,9 @@ void CommManager::send_status(void)
 
   uint8_t control_mode = 0;
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)
-      || RF_.command_manager_.combined_control().x.type == PASSTHROUGH) {
+      || RF_.command_manager_.combined_control().Qx.type == PASSTHROUGH) {
     control_mode = MODE_PASS_THROUGH;
-  } else if (RF_.command_manager_.combined_control().x.type == ANGLE) {
+  } else if (RF_.command_manager_.combined_control().Qx.type == ANGLE) {
     control_mode = MODE_ROLL_PITCH_YAWRATE_THROTTLE;
   } else {
     control_mode = MODE_ROLLRATE_PITCHRATE_YAWRATE_THROTTLE;
diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index fd54f0c4..68371446 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -91,7 +91,8 @@ void CommandManager::init_failsafe()
     RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_FAILSAFE);
   }
 
-  multirotor_failsafe_command_.F.value = failsafe_thr_param;
+  // TODO: Add parameter to choose which F the RC throttle corresponds to
+  multirotor_failsafe_command_.Fz.value = failsafe_thr_param;
 
   if (fixedwing) {
     failsafe_command_ = fixedwing_failsafe_command_;
@@ -103,17 +104,20 @@ void CommandManager::init_failsafe()
 void CommandManager::interpret_rc(void)
 {
   // get initial, unscaled RC values
-  rc_command_.x.value = RF_.rc_.stick(RC::STICK_X);
-  rc_command_.y.value = RF_.rc_.stick(RC::STICK_Y);
-  rc_command_.z.value = RF_.rc_.stick(RC::STICK_Z);
-  rc_command_.F.value = RF_.rc_.stick(RC::STICK_F);
+  // TODO: Adjust this to choose the channel that the RC thottle corresponds to
+  rc_command_.Qx.value = RF_.rc_.stick(RC::STICK_X);
+  rc_command_.Qy.value = RF_.rc_.stick(RC::STICK_Y);
+  rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z);
+  rc_command_.Fx.value = 0.0;
+  rc_command_.Fy.value = 0.0;
+  rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
 
   // determine control mode for each channel and scale command values accordingly
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    rc_command_.x.type = PASSTHROUGH;
-    rc_command_.y.type = PASSTHROUGH;
-    rc_command_.z.type = PASSTHROUGH;
-    rc_command_.F.type = PASSTHROUGH;
+    rc_command_.Qx.type = PASSTHROUGH;
+    rc_command_.Qy.type = PASSTHROUGH;
+    rc_command_.Qz.type = PASSTHROUGH;
+    rc_command_.Fz.type = PASSTHROUGH;
   } else {
     // roll and pitch
     control_type_t roll_pitch_type;
@@ -124,28 +128,28 @@ void CommandManager::interpret_rc(void)
         (RF_.params_.get_param_int(PARAM_RC_ATTITUDE_MODE) == ATT_MODE_RATE) ? RATE : ANGLE;
     }
 
-    rc_command_.x.type = roll_pitch_type;
-    rc_command_.y.type = roll_pitch_type;
+    rc_command_.Qx.type = roll_pitch_type;
+    rc_command_.Qy.type = roll_pitch_type;
 
     // Scale command to appropriate units
     switch (roll_pitch_type) {
       case RATE:
-        rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE);
-        rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE);
+        rc_command_.Qx.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLLRATE);
+        rc_command_.Qy.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCHRATE);
         break;
       case ANGLE:
-        rc_command_.x.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL);
-        rc_command_.y.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH);
+        rc_command_.Qx.value *= RF_.params_.get_param_float(PARAM_RC_MAX_ROLL);
+        rc_command_.Qy.value *= RF_.params_.get_param_float(PARAM_RC_MAX_PITCH);
       default:
         break;
     }
 
     // yaw
-    rc_command_.z.type = RATE;
-    rc_command_.z.value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
+    rc_command_.Qz.type = RATE;
+    rc_command_.Qz.value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
 
     // throttle
-    rc_command_.F.type = THROTTLE;
+    rc_command_.Fz.type = THROTTLE;
   }
 }
 
@@ -195,10 +199,10 @@ bool CommandManager::do_throttle_muxing(void)
       && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) {
     override_this_channel = true;
   } else { // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override
-    if (muxes[MUX_F].onboard->active) {
+    if (muxes[MUX_FZ].onboard->active) {
       // Check if the parameter flag is set to have us always take the smaller throttle
       if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) {
-        override_this_channel = (muxes[MUX_F].rc->value < muxes[MUX_F].onboard->value);
+        override_this_channel = (muxes[MUX_FZ].rc->value < muxes[MUX_FZ].onboard->value);
       } else {
         override_this_channel = false;
       }
@@ -208,7 +212,7 @@ bool CommandManager::do_throttle_muxing(void)
   }
 
   // Set the combined channel output depending on whether RC is overriding for this channel or not
-  *muxes[MUX_F].combined = override_this_channel ? *muxes[MUX_F].rc : *muxes[MUX_F].onboard;
+  *muxes[MUX_FZ].combined = override_this_channel ? *muxes[MUX_FZ].rc : *muxes[MUX_FZ].onboard;
   return override_this_channel;
 }
 
@@ -255,16 +259,19 @@ bool CommandManager::run()
     if (RF_.board_.clock_millis()
         > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) {
       // If it has been longer than 100 ms, then disable the offboard control
-      offboard_command_.F.active = false;
-      offboard_command_.x.active = false;
-      offboard_command_.y.active = false;
-      offboard_command_.z.active = false;
+      // TODO: Check to make sure the FX and FY commands can be set to true
+      offboard_command_.Fx.active = false;
+      offboard_command_.Fy.active = false;
+      offboard_command_.Fz.active = false;
+      offboard_command_.Qx.active = false;
+      offboard_command_.Qy.active = false;
+      offboard_command_.Qz.active = false;
     }
 
     // Perform muxing
-    rc_override_ = do_roll_pitch_yaw_muxing(MUX_X);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Y);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_Z);
+    rc_override_ = do_roll_pitch_yaw_muxing(MUX_QX);
+    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QY);
+    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QZ);
     rc_override_ |= do_throttle_muxing();
 
     // Light to indicate override
diff --git a/src/controller.cpp b/src/controller.cpp
index c22ee20c..0a733d8d 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -119,17 +119,20 @@ void Controller::run()
   // Check if integrators should be updated
   //! @todo better way to figure out if throttle is high
   bool update_integrators = (RF_.state_manager_.state().armed)
-    && (RF_.command_manager_.combined_control().F.value > 0.1f) && dt_us < 10000;
+    && (RF_.command_manager_.combined_control().Fz.value > 0.1f) && dt_us < 10000;
 
   // Run the PID loops
   Controller::Output pid_output = run_pid_loops(
     dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
 
   // Add feedforward torques
-  output_.x = pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
-  output_.y = pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE);
-  output_.z = pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE);
-  output_.F = pid_output.F;
+  // TODO: Do the controllers need to input/output Fx and Fy?
+  output_.Qx = pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
+  output_.Qy = pid_output.Qy + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE);
+  output_.Qz = pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE);
+  output_.Fz = pid_output.Fz;
+  output_.Fx = RF_.command_manager_.combined_control().Fx.value;
+  output_.Fy = RF_.command_manager_.combined_control().Fy.value;
 }
 
 void Controller::calculate_equilbrium_torque_from_rc()
@@ -165,11 +168,11 @@ void Controller::calculate_equilbrium_torque_from_rc()
 
     // the output from the controller is going to be the static offsets
     RF_.params_.set_param_float(PARAM_X_EQ_TORQUE,
-                                pid_output.x + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE));
+                                pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE));
     RF_.params_.set_param_float(PARAM_Y_EQ_TORQUE,
-                                pid_output.y + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE));
+                                pid_output.Qy + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE));
     RF_.params_.set_param_float(PARAM_Z_EQ_TORQUE,
-                                pid_output.z + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE));
+                                pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE));
 
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
                           "Equilibrium torques found and applied.");
@@ -227,42 +230,40 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
   float dt = 1e-6 * dt_us;
 
   // ROLL
-  if (command.x.type == RATE) {
-    out.x = roll_rate_.run(dt, state.angular_velocity.x, command.x.value, update_integrators);
-  } else if (command.x.type == ANGLE) {
-    out.x =
-      roll_.run(dt, state.roll, command.x.value, update_integrators, state.angular_velocity.x);
+  if (command.Qx.type == RATE) {
+    out.Qx = roll_rate_.run(dt, state.angular_velocity.x, command.Qx.value, update_integrators);
+  } else if (command.Qx.type == ANGLE) {
+    out.Qx =
+      roll_.run(dt, state.roll, command.Qx.value, update_integrators, state.angular_velocity.x);
   } else {
-    out.x = command.x.value;
+    out.Qx = command.Qx.value;
   }
 
   // PITCH
-  if (command.y.type == RATE) {
-    out.y = pitch_rate_.run(dt, state.angular_velocity.y, command.y.value, update_integrators);
-  } else if (command.y.type == ANGLE) {
-    out.y =
-      pitch_.run(dt, state.pitch, command.y.value, update_integrators, state.angular_velocity.y);
+  if (command.Qy.type == RATE) {
+    out.Qy = pitch_rate_.run(dt, state.angular_velocity.y, command.Qy.value, update_integrators);
+  } else if (command.Qy.type == ANGLE) {
+    out.Qy =
+      pitch_.run(dt, state.pitch, command.Qy.value, update_integrators, state.angular_velocity.y);
   } else {
-    out.y = command.y.value;
+    out.Qy = command.Qy.value;
   }
 
   // YAW
-  if (command.z.type == RATE) {
-    out.z = yaw_rate_.run(dt, state.angular_velocity.z, command.z.value, update_integrators);
+  if (command.Qz.type == RATE) {
+    out.Qz = yaw_rate_.run(dt, state.angular_velocity.z, command.Qz.value, update_integrators);
   } else {
-    out.z = command.z.value;
+    out.Qz = command.Qz.value;
   }
 
   // THROTTLE
-  if (command.F.type == THROTTLE && !RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    // Converts the input throttle command to thrust command, and scales the saturation limit by
-    // RC_MAX_THROTTLE to maintain controllability.
-    float max_throttle = RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
-    out.F = command.F.value * max_thrust_ * max_throttle;
+  if (command.Fz.type == THROTTLE) {
+    // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability 
+    // during aggressive maneuvers.
+    out.Fz = command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
   } else {
-    // If it is not a throttle setting, or if the vehicle type is FIXED_WING, then pass directly 
-    // to the mixer.
-    out.F = command.F.value;
+    // If it is not a throttle setting then pass directly to the mixer.
+    out.Fz = command.Fz.value;
   }
 
   return out;
@@ -333,7 +334,6 @@ float Controller::PID::run(float dt, float x, float x_c, bool update_integrator,
   float u = p_term - d_term + i_term;
 
   // Integrator anti-windup
-  //// TODO: Include reference to Dr. Beard's notes here
   float u_sat = (u > max_) ? max_ : (u < min_) ? min_ : u;
   if (u != u_sat && fabs(i_term) > fabs(u - p_term + d_term) && ki_ > 0.0f) {
     integrator_ = (u_sat - p_term + d_term) / ki_;
diff --git a/src/mixer.cpp b/src/mixer.cpp
index d9716326..a19c643e 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -103,15 +103,45 @@ void Mixer::init_mixing()
 
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                             "Inverting selected mixing matrix...");
-      // TODO: Test to see if selecting passthrough mixing will break the inversion
-      // TODO: Is there any reason to use passthrough mixing?
+
       // Otherwise, invert the selected "canned" matrix
       primary_mixer_ = invert_mixer(mixer_to_use_);
-      
-      // Add the header values (PWM rate and output type) to mixer
-      add_header_to_mixer(&primary_mixer_);
+
       mixer_to_use_ = &primary_mixer_;
     }
+
+    std::cout << "Mixer: " << std::endl;
+    for (auto i : mixer_to_use_->output_type) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->default_pwm_rate) {
+      std::cout << i << " ";
+    }
+    for (auto i : mixer_to_use_->Fx) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->Fy) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->Fz) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->Qx) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->Qy) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : mixer_to_use_->Qz) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
   }
 
   init_PWM();
@@ -157,12 +187,14 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
     Eigen::FullPivHouseholderQRPreconditioner | Eigen::ComputeFullU | Eigen::ComputeFullV);
   Eigen::Matrix<float, NUM_MIXER_OUTPUTS, 6> Sig;
   Sig.setZero();
-  Sig(0, 0) = 1.0 / svd.singularValues()[0];
-  Sig(1, 1) = 1.0 / svd.singularValues()[1];
-  Sig(2, 2) = 1.0 / svd.singularValues()[2];
-  Sig(3, 3) = 1.0 / svd.singularValues()[3];
-  Sig(4, 4) = 1.0 / svd.singularValues()[4];
-  Sig(5, 5) = 1.0 / svd.singularValues()[5];
+
+  // Avoid dividing by zero in the Sigma matrix
+  if (svd.singularValues()[0] != 0.0) { Sig(0, 0) = 1.0 / svd.singularValues()[0]; }
+  if (svd.singularValues()[1] != 0.0) { Sig(1, 1) = 1.0 / svd.singularValues()[1]; }
+  if (svd.singularValues()[2] != 0.0) { Sig(2, 2) = 1.0 / svd.singularValues()[2]; }
+  if (svd.singularValues()[3] != 0.0) { Sig(3, 3) = 1.0 / svd.singularValues()[3]; }
+  if (svd.singularValues()[4] != 0.0) { Sig(4, 4) = 1.0 / svd.singularValues()[4]; }
+  if (svd.singularValues()[5] != 0.0) { Sig(5, 5) = 1.0 / svd.singularValues()[5]; }
 
   // Pseudoinverse of the mixing matrix
   Eigen::Matrix<float, NUM_MIXER_OUTPUTS, 6> mixer_matrix_pinv =
@@ -172,6 +204,8 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
   mixer_t inverted_mixer;
 
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    inverted_mixer.default_pwm_rate[i] = mixer_to_invert->default_pwm_rate[i];
+
     if (i < RF_.params_.get_param_int(PARAM_NUM_MOTORS)) {
       inverted_mixer.output_type[i] = M;
       inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0);
@@ -288,7 +322,7 @@ void Mixer::init_PWM()
   if (mixer_to_use_ != nullptr) {
     RF_.board_.pwm_init_multi(mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS);
   } else {
-    RF_.board_.pwm_init_multi(passthrough_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS);
+    RF_.board_.pwm_init_multi(esc_calibration_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS);
   }
 }
 
@@ -329,15 +363,54 @@ void Mixer::set_new_aux_command(aux_command_t new_aux_command)
   }
 }
 
-void Mixer::mix_multirotor()
+float Mixer::mix_multirotor_without_motor_parameters()
 {
   Controller::Output commands = RF_.controller_.output();
 
-  if (commands.F < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
+//  std::cout << "Commands: ";
+//  std::cout << commands.Fx << " ";
+//  std::cout << commands.Fy << " Fz: ";
+//  std::cout << commands.Fz << " ";
+//  std::cout << commands.Qx << " ";
+//  std::cout << commands.Qy << " ";
+//  std::cout << commands.Qz << " " << std::endl;
+
+  if (commands.Fz < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
+    // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
+    // arming/disarming
+    commands.Qz = 0.0;
+  }
+
+  // Mix the inputs
+  float max_output = 1.0;
+
+  for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    if (mixer_to_use_->output_type[i] != NONE) {
+      // Matrix multiply to mix outputs
+      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
+                    commands.Fy * mixer_to_use_->Fy[i] +
+                    commands.Fz * mixer_to_use_->Fz[i] + 
+                    commands.Qx * mixer_to_use_->Qx[i] + 
+                    commands.Qy * mixer_to_use_->Qy[i] + 
+                    commands.Qz * mixer_to_use_->Qz[i];
+
+      // Save off the largest control output if it is greater than 1.0 for future scaling
+      if (outputs_[i] > max_output) { max_output = outputs_[i]; }
+    }
+  }
+
+  return max_output;
+}
+
+float Mixer::mix_multirotor_with_motor_parameters()
+{
+  Controller::Output commands = RF_.controller_.output();
+
+  if (commands.Fz < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
                * RF_.controller_.max_thrust()) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
-    commands.z = 0.0;
+    commands.Qz = 0.0;
   }
 
   // Mix the inputs
@@ -346,10 +419,12 @@ void Mixer::mix_multirotor()
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      // TODO: Update the commands vector to include fx and fy 
-      float omega_squared =
-        (commands.F * mixer_to_use_->Fz[i] + commands.x * mixer_to_use_->Qx[i]
-         + commands.y * mixer_to_use_->Qy[i] + commands.z * mixer_to_use_->Qz[i]);
+      float omega_squared = commands.Fx * mixer_to_use_->Fx[i] +
+                            commands.Fy * mixer_to_use_->Fy[i] +
+                            commands.Fz * mixer_to_use_->Fz[i] +
+                            commands.Qx * mixer_to_use_->Qx[i] +
+                            commands.Qy * mixer_to_use_->Qy[i] +
+                            commands.Qz * mixer_to_use_->Qz[i];
 
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
@@ -368,6 +443,25 @@ void Mixer::mix_multirotor()
     }
   }
 
+  return max_output;
+}
+
+void Mixer::mix_multirotor()
+{
+  // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected.
+  float max_output;
+  if (mixer_to_use_ == &custom_mixing) {
+    max_output = mix_multirotor_with_motor_parameters();
+  } else {
+    max_output = mix_multirotor_without_motor_parameters();
+  }
+
+//  std::cout << "Outputs (pre scale): ";
+//  for (auto i : outputs_) {
+//    std::cout << i << " ";
+//  }
+//  std::cout << std::endl;
+
   // There is no relative scaling on the above equations. In other words, if the input F command is too
   // high, then it will "drown out" all other desired outputs. Therefore, we saturate motor outputs to 
   // maintain controllability even during aggressive maneuvers.
@@ -380,6 +474,12 @@ void Mixer::mix_multirotor()
     if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; }
   }
 
+ // std::cout << "Outputs (post scale): ";
+ // for (auto i : outputs_) {
+ //   std::cout << i << " ";
+ // }
+ // std::cout << std::endl;
+
 }
 
 void Mixer::mix_fixedwing()
@@ -388,18 +488,19 @@ void Mixer::mix_fixedwing()
 
   // Reverse fixed-wing channels just before mixing if we need to
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    commands.x *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
-    commands.y *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
-    commands.z *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
+    commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
+    commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
+    commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
   }
 
   // Mix the outputs
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
-      outputs_[i] = (commands.F * mixer_to_use_->Fz[i] + commands.x * mixer_to_use_->Qx[i]
-                     + commands.y * mixer_to_use_->Qy[i] + commands.z * mixer_to_use_->Qz[i]);
-
+      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
+                    commands.Qx * mixer_to_use_->Qx[i] +
+                    commands.Qy * mixer_to_use_->Qy[i] +
+                    commands.Qz * mixer_to_use_->Qz[i];
     }
   }
 }
diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp
index 5226f67b..c3de9f96 100644
--- a/test/command_manager_test.cpp
+++ b/test/command_manager_test.cpp
@@ -9,10 +9,12 @@
 #define CHN_LOW 1100
 #define CHN_HIGH 1900
 
-#define OFFBOARD_X -1.0
-#define OFFBOARD_Y 0.5
-#define OFFBOARD_Z -0.7
-#define OFFBOARD_F 0.9
+#define OFFBOARD_QX -1.0
+#define OFFBOARD_QY 0.5
+#define OFFBOARD_QZ -0.7
+#define OFFBOARD_FX 0.9
+#define OFFBOARD_FY 0.9
+#define OFFBOARD_FZ 0.9
 
 #define RC_X_PWM 1800
 #define RC_X ((RC_X_PWM - 1500) / 500.0 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL))
@@ -31,10 +33,12 @@ class CommandManagerTest : public ::testing::Test
   float max_roll, max_pitch, max_yawrate;
 
   control_t offboard_command = {20000,
-                                {true, ANGLE, OFFBOARD_X},
-                                {true, ANGLE, OFFBOARD_Y},
-                                {true, RATE, OFFBOARD_Z},
-                                {true, THROTTLE, OFFBOARD_F}};
+                                {true, ANGLE, OFFBOARD_QX},
+                                {true, ANGLE, OFFBOARD_QY},
+                                {true, RATE, OFFBOARD_QZ},
+                                {true, THROTTLE, OFFBOARD_FX}
+                                {true, THROTTLE, OFFBOARD_FY}
+                                {true, THROTTLE, OFFBOARD_FZ}};
 
   CommandManagerTest()
       : mavlink(board)
@@ -85,14 +89,14 @@ TEST_F(CommandManagerTest, Default)
   stepFirmware(20000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, 0.0);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, 0.0);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, 0.0);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, 0.0);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, 0.0);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, 0.0);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, RCCommands)
@@ -104,14 +108,14 @@ TEST_F(CommandManagerTest, RCCommands)
   stepFirmware(50000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, 1.0 * max_roll);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, -1.0 * max_pitch);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, -0.5 * max_yawrate);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, 0.5);
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, 1.0 * max_roll);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, -1.0 * max_pitch);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, -0.5 * max_yawrate);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, 0.5);
 }
 
 TEST_F(CommandManagerTest, ArmWithSticksByDefault)
@@ -207,14 +211,14 @@ TEST_F(CommandManagerTest, DefaultRCOutputd)
 
   // Check the output
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, 0.0);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, 0.0);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, 0.0);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, 0.0);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, 0.0);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, 0.0);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, RCOutput)
@@ -228,14 +232,14 @@ TEST_F(CommandManagerTest, RCOutput)
   // Check the output
   EXPECT_EQ(rf.state_manager_.state().armed, false);
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, max_roll * -0.5);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, max_pitch * 0.5);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, max_yawrate);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, 0.5);
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, max_roll * -0.5);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, max_pitch * 0.5);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, max_yawrate);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, 0.5);
 }
 
 TEST_F(CommandManagerTest, LoseRCDisarmed)
@@ -244,14 +248,14 @@ TEST_F(CommandManagerTest, LoseRCDisarmed)
   stepFirmware(50000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, 0.0 * max_roll);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, 0.0 * max_pitch);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, 0.0 * max_roll);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, 0.0 * max_pitch);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, 0.0 * max_yawrate);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 
   // We should also be disarmed and in error
   EXPECT_EQ(rf.state_manager_.state().armed, false);
@@ -281,14 +285,14 @@ TEST_F(CommandManagerTest, LoseRCArmed)
   stepFirmware(20000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, ANGLE);
-  EXPECT_CLOSE(output.x.value, 0.0 * max_roll);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_CLOSE(output.y.value, 0.0 * max_pitch);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_CLOSE(output.z.value, 0.0 * max_yawrate);
-  EXPECT_EQ(output.F.type, THROTTLE);
-  EXPECT_CLOSE(output.F.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE));
+  EXPECT_EQ(output.Qx.type, ANGLE);
+  EXPECT_CLOSE(output.Qx.value, 0.0 * max_roll);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_CLOSE(output.Qy.value, 0.0 * max_pitch);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_CLOSE(output.Qz.value, 0.0 * max_yawrate);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
+  EXPECT_CLOSE(output.Fz.value, rf.params_.get_param_float(PARAM_FAILSAFE_THROTTLE));
 
   // We should also be disarmed and in error
   EXPECT_EQ(rf.state_manager_.state().armed, true);
@@ -323,10 +327,12 @@ TEST_F(CommandManagerTest, OffboardCommandMuxNoMinThrottle)
   stepFirmware(20000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
-  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
-  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
-  EXPECT_CLOSE(output.F.value, OFFBOARD_F);
+  EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
+  EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
+  EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
+  EXPECT_CLOSE(output.Fx.value, OFFBOARD_FX);
+  EXPECT_CLOSE(output.Fy.value, OFFBOARD_FY);
+  EXPECT_CLOSE(output.Fz.value, OFFBOARD_FZ);
 }
 
 TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle)
@@ -340,10 +346,12 @@ TEST_F(CommandManagerTest, OffboardCommandMuxMinThrottle)
   stepFirmware(20000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
-  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
-  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
+  EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
+  EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
+  EXPECT_CLOSE(output.Fx.value, OFFBOARD_FX);
+  EXPECT_CLOSE(output.Fy.value, OFFBOARD_FY);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
@@ -355,10 +363,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxRollDeviation)
   stepFirmware(40000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
-  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
-  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
+  EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
+  EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
@@ -370,10 +378,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxPitchDeviation)
   stepFirmware(40000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
-  EXPECT_CLOSE(output.y.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH));
-  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
+  EXPECT_CLOSE(output.Qy.value, 0.5 * rf.params_.get_param_float(PARAM_RC_MAX_PITCH));
+  EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
@@ -385,10 +393,10 @@ TEST_F(CommandManagerTest, OffboardCommandMuxYawrateDeviation)
   stepFirmware(40000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
-  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
-  EXPECT_CLOSE(output.z.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE));
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
+  EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
+  EXPECT_CLOSE(output.Qz.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_YAWRATE));
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, OffboardCommandMuxLag)
@@ -400,24 +408,24 @@ TEST_F(CommandManagerTest, OffboardCommandMuxLag)
   stepFirmware(40000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
+  EXPECT_CLOSE(output.Qx.value, -0.5 * rf.params_.get_param_float(PARAM_RC_MAX_ROLL));
 
   rc_values[0] = 1500; // return stick to center
 
   stepFirmware(500000);
   setOffboard(offboard_command);
   output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, 0.0); // lag
+  EXPECT_CLOSE(output.Qx.value, 0.0); // lag
 
   stepFirmware(600000);
   setOffboard(offboard_command);
   output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, 0.0); // lag
+  EXPECT_CLOSE(output.Qx.value, 0.0); // lag
 
   setOffboard(offboard_command);
   stepFirmware(20000);
   output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, OFFBOARD_X);
+  EXPECT_CLOSE(output.Qx.value, OFFBOARD_QX);
 }
 
 TEST_F(CommandManagerTest, StaleOffboardCommand)
@@ -430,33 +438,33 @@ TEST_F(CommandManagerTest, StaleOffboardCommand)
   stepFirmware(timeout_us + 40000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, PartialMux)
 {
-  offboard_command.x.active = false;
+  offboard_command.Qx.active = false;
   stepFirmware(1000000);
   setOffboard(offboard_command);
   stepFirmware(30000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_CLOSE(output.x.value, 0.0);
-  EXPECT_CLOSE(output.y.value, OFFBOARD_Y);
-  EXPECT_CLOSE(output.z.value, OFFBOARD_Z);
-  EXPECT_CLOSE(output.F.value, 0.0);
+  EXPECT_CLOSE(output.Qx.value, 0.0);
+  EXPECT_CLOSE(output.Qy.value, OFFBOARD_QY);
+  EXPECT_CLOSE(output.Qz.value, OFFBOARD_QZ);
+  EXPECT_CLOSE(output.Fz.value, 0.0);
 }
 
 TEST_F(CommandManagerTest, MixedTypes)
 {
-  offboard_command.x.type = RATE;
+  offboard_command.Qx.type = RATE;
   stepFirmware(1000000);
   setOffboard(offboard_command);
   stepFirmware(30000);
 
   control_t output = rf.command_manager_.combined_control();
-  EXPECT_EQ(output.x.type, RATE);
-  EXPECT_EQ(output.y.type, ANGLE);
-  EXPECT_EQ(output.z.type, RATE);
-  EXPECT_EQ(output.F.type, THROTTLE);
+  EXPECT_EQ(output.Qx.type, RATE);
+  EXPECT_EQ(output.Qy.type, ANGLE);
+  EXPECT_EQ(output.Qz.type, RATE);
+  EXPECT_EQ(output.Fz.type, THROTTLE);
 }

From 180a3cb79252eccda4102da6bf06a3d36a2501a6 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Thu, 7 Nov 2024 12:53:29 -0700
Subject: [PATCH 26/41] added mixer header values to the parameter list

---
 include/mixer.h | 20 ++++++-------
 include/param.h | 22 +++++++++++++++
 src/mixer.cpp   | 75 +++++++++++++++++++++++--------------------------
 src/param.cpp   | 22 +++++++++++++++
 4 files changed, 89 insertions(+), 50 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index af400cce..a72c296c 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -208,9 +208,9 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t fixedwing_mixing = {
-    {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
+    {   S,    S,    S,    M, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
     { 50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
+    {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
     {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
@@ -218,14 +218,14 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t fixedwing_inverted_vtail_mixing = {
-    {   S, NONE, NONE,     S,    S,    M, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
-    {  50,   50,   50,    50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_y Mix
-    {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},   // F_z Mix
-    {1.0f, 0.0f, 0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
-    {0.0f, 0.0f, 0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
-    {0.0f, 0.0f, 0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
+    {   S,     S,    S,    M, NONE, NONE, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
+    {  50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
+    {0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
+    {0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
+    {0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
+    {1.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
+    {0.0f, -0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_y Mix 
+    {0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t custom_mixing = {
     {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
diff --git a/include/param.h b/include/param.h
index d42518df..a80c4372 100644
--- a/include/param.h
+++ b/include/param.h
@@ -73,6 +73,28 @@ enum : uint16_t
   PARAM_PROP_CQ,
   PARAM_VOLT_MAX,
 
+  PARAM_PRIMARY_MIXER_OUTPUT_0,
+  PARAM_PRIMARY_MIXER_OUTPUT_1,
+  PARAM_PRIMARY_MIXER_OUTPUT_2,
+  PARAM_PRIMARY_MIXER_OUTPUT_3,
+  PARAM_PRIMARY_MIXER_OUTPUT_4,
+  PARAM_PRIMARY_MIXER_OUTPUT_5,
+  PARAM_PRIMARY_MIXER_OUTPUT_6,
+  PARAM_PRIMARY_MIXER_OUTPUT_7,
+  PARAM_PRIMARY_MIXER_OUTPUT_8,
+  PARAM_PRIMARY_MIXER_OUTPUT_9,
+
+  PARAM_PRIMARY_MIXER_PWM_RATE_0,
+  PARAM_PRIMARY_MIXER_PWM_RATE_1,
+  PARAM_PRIMARY_MIXER_PWM_RATE_2,
+  PARAM_PRIMARY_MIXER_PWM_RATE_3,
+  PARAM_PRIMARY_MIXER_PWM_RATE_4,
+  PARAM_PRIMARY_MIXER_PWM_RATE_5,
+  PARAM_PRIMARY_MIXER_PWM_RATE_6,
+  PARAM_PRIMARY_MIXER_PWM_RATE_7,
+  PARAM_PRIMARY_MIXER_PWM_RATE_8,
+  PARAM_PRIMARY_MIXER_PWM_RATE_9,
+
   PARAM_PRIMARY_MIXER_1_1,
   PARAM_PRIMARY_MIXER_2_1,
   PARAM_PRIMARY_MIXER_3_1,
diff --git a/src/mixer.cpp b/src/mixer.cpp
index a19c643e..d1b56490 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -90,14 +90,11 @@ void Mixer::init_mixing()
     if (mixer_to_use_ == &custom_mixing) {
       // If specified, compute the custom mixing matrix per the motor and propeller parameters
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
-                            "Calculating custom mixer values...");
+                            "Loading saved custom mixer values...");
+
       load_primary_mixer_values();
-      
-      // TODO: Add these as parameters in the firmware, otherwise we can't control S vs M for custom mixers
-      // Add the header values (PWM rate and output type) to mixer
-      add_header_to_mixer(&primary_mixer_);
       mixer_to_use_ = &primary_mixer_;
-    } else if (mixer_to_use_ != &fixedwing_mixing ||
+    } else if (mixer_to_use_ != &fixedwing_mixing &&
                mixer_to_use_ != &fixedwing_inverted_vtail_mixing) {
       // Don't invert the fixedwing mixers
 
@@ -106,7 +103,6 @@ void Mixer::init_mixing()
 
       // Otherwise, invert the selected "canned" matrix
       primary_mixer_ = invert_mixer(mixer_to_use_);
-
       mixer_to_use_ = &primary_mixer_;
     }
 
@@ -118,6 +114,7 @@ void Mixer::init_mixing()
     for (auto i : mixer_to_use_->default_pwm_rate) {
       std::cout << i << " ";
     }
+    std::cout << std::endl;
     for (auto i : mixer_to_use_->Fx) {
       std::cout << i << " ";
     }
@@ -204,47 +201,45 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
   mixer_t inverted_mixer;
 
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
+    inverted_mixer.output_type[i] = mixer_to_invert->output_type[i];
     inverted_mixer.default_pwm_rate[i] = mixer_to_invert->default_pwm_rate[i];
-
-    if (i < RF_.params_.get_param_int(PARAM_NUM_MOTORS)) {
-      inverted_mixer.output_type[i] = M;
-      inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0);
-      inverted_mixer.Fy[i] = mixer_matrix_pinv(i, 1);
-      inverted_mixer.Fz[i] = mixer_matrix_pinv(i, 2);
-      inverted_mixer.Qx[i] = mixer_matrix_pinv(i, 3);
-      inverted_mixer.Qy[i] = mixer_matrix_pinv(i, 4);
-      inverted_mixer.Qz[i] = mixer_matrix_pinv(i, 5);
-    } else {
-      // Set the rest of the ouput types to NONE
-      inverted_mixer.output_type[i] = NONE;
-      inverted_mixer.Fx[i] = 0.0;
-      inverted_mixer.Fy[i] = 0.0;
-      inverted_mixer.Fz[i] = 0.0;
-      inverted_mixer.Qx[i] = 0.0;
-      inverted_mixer.Qy[i] = 0.0;
-      inverted_mixer.Qz[i] = 0.0;
-    }
+    inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0);
+    inverted_mixer.Fy[i] = mixer_matrix_pinv(i, 1);
+    inverted_mixer.Fz[i] = mixer_matrix_pinv(i, 2);
+    inverted_mixer.Qx[i] = mixer_matrix_pinv(i, 3);
+    inverted_mixer.Qy[i] = mixer_matrix_pinv(i, 4);
+    inverted_mixer.Qz[i] = mixer_matrix_pinv(i, 5);
   }
 
   return inverted_mixer;
 }
 
-void Mixer::add_header_to_mixer(mixer_t* mixer)
-{
-  // Fill in the default PWM rates to use in the header of the mixer
-  for (int i = 0; i < NUM_MIXER_OUTPUTS; i++){
-    if (i < RF_.params_.get_param_int(PARAM_NUM_MOTORS) || 
-        (RF_.params_.get_param_int(PARAM_NUM_MOTORS) > 4 && i < 8)) {
-      // This makes sure the PWM groups are correct for the hardware
-      mixer->default_pwm_rate[i] = 490;
-    } else {
-      mixer->default_pwm_rate[i] = 50;
-    }
-  }
-}
-
 void Mixer::load_primary_mixer_values()
 {
+  // Load the mixer header values
+  // TODO: Is there a better way to do this?
+  primary_mixer_.output_type[0] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0);
+  primary_mixer_.output_type[1] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1);
+  primary_mixer_.output_type[2] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2);
+  primary_mixer_.output_type[3] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3);
+  primary_mixer_.output_type[4] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4);
+  primary_mixer_.output_type[5] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5);
+  primary_mixer_.output_type[6] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6);
+  primary_mixer_.output_type[7] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7);
+  primary_mixer_.output_type[8] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8);
+  primary_mixer_.output_type[9] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9);
+
+  primary_mixer_.default_pwm_rate[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0);
+  primary_mixer_.default_pwm_rate[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1);
+  primary_mixer_.default_pwm_rate[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2);
+  primary_mixer_.default_pwm_rate[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3);
+  primary_mixer_.default_pwm_rate[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4);
+  primary_mixer_.default_pwm_rate[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5);
+  primary_mixer_.default_pwm_rate[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6);
+  primary_mixer_.default_pwm_rate[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7);
+  primary_mixer_.default_pwm_rate[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8);
+  primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9);
+
   // Load the mixer values from the firmware parameters
   primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
   primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
diff --git a/src/param.cpp b/src/param.cpp
index bc916eae..eaa6373a 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -119,6 +119,28 @@ void Params::set_defaults(void)
   init_param_float(PARAM_PROP_CQ, "PROP_CQ", 0.0045f); // Torque coefficient of the propeller | 0 | 100.0
   init_param_float(PARAM_VOLT_MAX, "VOLT_MAX", 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0
 
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0, "PRIM_MIXER_OUT_0", 0); // Output type of mixer output 0. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1, "PRIM_MIXER_OUT_1", 0); // Output type of mixer output 1. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2, "PRIM_MIXER_OUT_2", 0); // Output type of mixer output 2. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3, "PRIM_MIXER_OUT_3", 0); // Output type of mixer output 3. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4, "PRIM_MIXER_OUT_4", 0); // Output type of mixer output 4. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5, "PRIM_MIXER_OUT_5", 0); // Output type of mixer output 5. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6, "PRIM_MIXER_OUT_6", 0); // Output type of mixer output 6. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7, "PRIM_MIXER_OUT_7", 0); // Output type of mixer output 7. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8, "PRIM_MIXER_OUT_8", 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9, "PRIM_MIXER_OUT_9", 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3
+
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRIM_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRIM_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRIM_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRIM_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRIM_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRIM_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRIM_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRIM_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRIM_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRIM_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9
+
   init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
   init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
   init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]

From 8816fd85b7ea51ff93af789490e577566893517f Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 11 Nov 2024 13:45:16 -0700
Subject: [PATCH 27/41] fixed Fz for the canned mixers, since we are in NED
 frame

---
 include/mixer.h | 18 ++++++++++--------
 1 file changed, 10 insertions(+), 8 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index a72c296c..18c5d262 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -132,7 +132,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_y Mix
-    { 0.2500f,  0.2500f,  0.2500f,  0.2500f, 0, 0, 0, 0, 0, 0},  // F_z Mix
+    {-0.2500f, -0.2500f, -0.2500f, -0.2500f, 0, 0, 0, 0, 0, 0},  // F_z Mix
     { 0.0000f, -1.0000f,  0.0000f,  1.0000f, 0, 0, 0, 0, 0, 0},  // Q_x Mix
     { 1.0000f,  0.0000f, -1.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix 
@@ -142,7 +142,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                 // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_y Mix
-    { 0.2500f,  0.2500f,  0.2500f,  0.2500f, 0, 0, 0, 0, 0, 0},   // F_z Mix
+    {-0.2500f, -0.2500f, -0.2500f, -0.2500f, 0, 0, 0, 0, 0, 0},   // F_z Mix
     {-0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_x Mix
     { 0.7071f, -0.7071f, -0.7071f,  0.7071f, 0, 0, 0, 0, 0, 0},   // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}};  // Q_z Mix
@@ -152,7 +152,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
+    {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0},  // F_z Mix
     { 0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
     { 1.0000f,  0.5000f, -0.5000f, -1.0000f, -0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -162,7 +162,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
+    {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0},  // F_z Mix
     {-0.5000f, -1.0000f, -0.5000f,  0.5000f,  1.0000f,  0.5000f, 0, 0, 0, 0},  // Q_x Mix
     { 0.8660f,  0.0000f, -0.8660f, -0.8660f,  0.0000f,  0.8660f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -172,7 +172,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
+    {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0},  // F_z Mix
     { 0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f,  1.0000f,  0.7071f, 0, 0},  // Q_x Mix
     { 1.0000f,  0.7071f,  0.0000f, -0.7071f, -1.0000f, -0.7071f,  0.0000f,  0.7071f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -182,7 +182,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
+    {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0},  // F_z Mix
     {-0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f,  0.9239f,  0.3827f, 0, 0},  // Q_x Mix
     { 0.9239f,  0.3827f, -0.3827f, -0.9239f, -0.9239f, -0.3827f,  0.3827f,  0.9239f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -192,7 +192,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
-    { 0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f,  0.1667f, 0, 0, 0, 0},  // F_z Mix
+    {-0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, -0.1667f, 0, 0, 0, 0},  // F_z Mix
     {-0.8660f, -0.8660f,  0.0000f,  0.0000f,  0.8660f,  0.8660f, 0, 0, 0, 0},  // Q_x Mix
     { 0.5000f,  0.5000f, -1.0000f, -1.0000f,  0.5000f,  0.5000f, 0, 0, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
@@ -202,7 +202,7 @@ class Mixer : public ParamListenerInterface
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
-    { 0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f,  0.1250f, 0, 0},  // F_z Mix
+    {-0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, -0.1250f, 0, 0},  // F_z Mix
     {-0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_x Mix
     { 0.7071f,  0.7071f, -0.7071f, -0.7071f, -0.7071f, -0.7071f,  0.7071f,  0.7071f, 0, 0},  // Q_y Mix
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
@@ -240,6 +240,7 @@ class Mixer : public ParamListenerInterface
   mixer_t primary_mixer_;
 
   const mixer_t * mixer_to_use_;
+  bool use_motor_parameters_;
 
   const mixer_t* array_of_mixers_[NUM_MIXERS] = {
     &esc_calibration_mixing,
@@ -282,6 +283,7 @@ class Mixer : public ParamListenerInterface
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
   inline const float * get_outputs() const { return raw_outputs_; }
+  inline const bool use_motor_parameters() const { return use_motor_parameters_; }
 
   void calculate_mixer_values();
   void mix_multirotor();

From 274df44b806bfcc2a7660ea2c7c8e370e36c8598 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 11 Nov 2024 13:49:48 -0700
Subject: [PATCH 28/41] changed mixer indices for clarity and fixed some bugs
 when using custom mixer

---
 include/param.h    |  36 +++---
 src/controller.cpp |  11 +-
 src/mixer.cpp      | 279 ++++++++++++++++++++++++++++++++-------------
 src/param.cpp      |  36 +++---
 4 files changed, 244 insertions(+), 118 deletions(-)

diff --git a/include/param.h b/include/param.h
index a80c4372..27cf14aa 100644
--- a/include/param.h
+++ b/include/param.h
@@ -95,75 +95,75 @@ enum : uint16_t
   PARAM_PRIMARY_MIXER_PWM_RATE_8,
   PARAM_PRIMARY_MIXER_PWM_RATE_9,
 
+  PARAM_PRIMARY_MIXER_0_0,
+  PARAM_PRIMARY_MIXER_1_0,
+  PARAM_PRIMARY_MIXER_2_0,
+  PARAM_PRIMARY_MIXER_3_0,
+  PARAM_PRIMARY_MIXER_4_0,
+  PARAM_PRIMARY_MIXER_5_0,
+
+  PARAM_PRIMARY_MIXER_0_1,
   PARAM_PRIMARY_MIXER_1_1,
   PARAM_PRIMARY_MIXER_2_1,
   PARAM_PRIMARY_MIXER_3_1,
   PARAM_PRIMARY_MIXER_4_1,
   PARAM_PRIMARY_MIXER_5_1,
-  PARAM_PRIMARY_MIXER_6_1,
 
+  PARAM_PRIMARY_MIXER_0_2,
   PARAM_PRIMARY_MIXER_1_2,
   PARAM_PRIMARY_MIXER_2_2,
   PARAM_PRIMARY_MIXER_3_2,
   PARAM_PRIMARY_MIXER_4_2,
   PARAM_PRIMARY_MIXER_5_2,
-  PARAM_PRIMARY_MIXER_6_2,
 
+  PARAM_PRIMARY_MIXER_0_3,
   PARAM_PRIMARY_MIXER_1_3,
   PARAM_PRIMARY_MIXER_2_3,
   PARAM_PRIMARY_MIXER_3_3,
   PARAM_PRIMARY_MIXER_4_3,
   PARAM_PRIMARY_MIXER_5_3,
-  PARAM_PRIMARY_MIXER_6_3,
 
+  PARAM_PRIMARY_MIXER_0_4,
   PARAM_PRIMARY_MIXER_1_4,
   PARAM_PRIMARY_MIXER_2_4,
   PARAM_PRIMARY_MIXER_3_4,
   PARAM_PRIMARY_MIXER_4_4,
   PARAM_PRIMARY_MIXER_5_4,
-  PARAM_PRIMARY_MIXER_6_4,
 
+  PARAM_PRIMARY_MIXER_0_5,
   PARAM_PRIMARY_MIXER_1_5,
   PARAM_PRIMARY_MIXER_2_5,
   PARAM_PRIMARY_MIXER_3_5,
   PARAM_PRIMARY_MIXER_4_5,
   PARAM_PRIMARY_MIXER_5_5,
-  PARAM_PRIMARY_MIXER_6_5,
 
+  PARAM_PRIMARY_MIXER_0_6,
   PARAM_PRIMARY_MIXER_1_6,
   PARAM_PRIMARY_MIXER_2_6,
   PARAM_PRIMARY_MIXER_3_6,
   PARAM_PRIMARY_MIXER_4_6,
   PARAM_PRIMARY_MIXER_5_6,
-  PARAM_PRIMARY_MIXER_6_6,
 
+  PARAM_PRIMARY_MIXER_0_7,
   PARAM_PRIMARY_MIXER_1_7,
   PARAM_PRIMARY_MIXER_2_7,
   PARAM_PRIMARY_MIXER_3_7,
   PARAM_PRIMARY_MIXER_4_7,
   PARAM_PRIMARY_MIXER_5_7,
-  PARAM_PRIMARY_MIXER_6_7,
-
+  
+  PARAM_PRIMARY_MIXER_0_8,
   PARAM_PRIMARY_MIXER_1_8,
   PARAM_PRIMARY_MIXER_2_8,
   PARAM_PRIMARY_MIXER_3_8,
   PARAM_PRIMARY_MIXER_4_8,
   PARAM_PRIMARY_MIXER_5_8,
-  PARAM_PRIMARY_MIXER_6_8,
-  
+                          
+  PARAM_PRIMARY_MIXER_0_9,
   PARAM_PRIMARY_MIXER_1_9,
   PARAM_PRIMARY_MIXER_2_9,
   PARAM_PRIMARY_MIXER_3_9,
   PARAM_PRIMARY_MIXER_4_9,
   PARAM_PRIMARY_MIXER_5_9,
-  PARAM_PRIMARY_MIXER_6_9,
-                          
-  PARAM_PRIMARY_MIXER_1_10,
-  PARAM_PRIMARY_MIXER_2_10,
-  PARAM_PRIMARY_MIXER_3_10,
-  PARAM_PRIMARY_MIXER_4_10,
-  PARAM_PRIMARY_MIXER_5_10,
-  PARAM_PRIMARY_MIXER_6_10,
 
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
diff --git a/src/controller.cpp b/src/controller.cpp
index 0a733d8d..ab7eaf29 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -126,7 +126,7 @@ void Controller::run()
     dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
 
   // Add feedforward torques
-  // TODO: Do the controllers need to input/output Fx and Fy?
+  // Note that the controller does not alter Fx and Fy
   output_.Qx = pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
   output_.Qy = pid_output.Qy + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE);
   output_.Qz = pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE);
@@ -137,6 +137,7 @@ void Controller::run()
 
 void Controller::calculate_equilbrium_torque_from_rc()
 {
+  // TODO: Verify that this is working!
   // Make sure we are disarmed
   if (!(RF_.state_manager_.state().armed)) {
     // Tell the user that we are doing a equilibrium torque calibration
@@ -260,7 +261,13 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
   if (command.Fz.type == THROTTLE) {
     // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability 
     // during aggressive maneuvers.
-    out.Fz = command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
+    // Also note the negative sign. Since the mixer assumes the inputs are in the NED
+    // frame, a throttle command corresponds to a thrust command in the negative direction.
+    out.Fz = -command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
+
+    if (RF_.mixer_.use_motor_parameters()) {
+      out.Fz *= max_thrust_;
+    }
   } else {
     // If it is not a throttle setting then pass directly to the mixer.
     out.Fz = command.Fz.value;
diff --git a/src/mixer.cpp b/src/mixer.cpp
index d1b56490..468f0e19 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -42,6 +42,7 @@ Mixer::Mixer(ROSflight & _rf)
     : RF_(_rf)
 {
   mixer_to_use_ = nullptr;
+  use_motor_parameters_ = false;
 }
 
 void Mixer::init() { init_mixing(); }
@@ -49,7 +50,100 @@ void Mixer::init() { init_mixing(); }
 void Mixer::param_change_callback(uint16_t param_id)
 {
   switch (param_id) {
+    case PARAM_PRIMARY_MIXER_OUTPUT_0:
+    case PARAM_PRIMARY_MIXER_OUTPUT_1:
+    case PARAM_PRIMARY_MIXER_OUTPUT_2:
+    case PARAM_PRIMARY_MIXER_OUTPUT_3:
+    case PARAM_PRIMARY_MIXER_OUTPUT_4:
+    case PARAM_PRIMARY_MIXER_OUTPUT_5:
+    case PARAM_PRIMARY_MIXER_OUTPUT_6:
+    case PARAM_PRIMARY_MIXER_OUTPUT_7:
+    case PARAM_PRIMARY_MIXER_OUTPUT_8:
+    case PARAM_PRIMARY_MIXER_OUTPUT_9:
+
+    case PARAM_PRIMARY_MIXER_PWM_RATE_0:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_1:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_2:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_3:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_4:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_5:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_6:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_7:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_8:
+    case PARAM_PRIMARY_MIXER_PWM_RATE_9:
+
+    case PARAM_PRIMARY_MIXER_0_0:
+    case PARAM_PRIMARY_MIXER_1_0:
+    case PARAM_PRIMARY_MIXER_2_0:
+    case PARAM_PRIMARY_MIXER_3_0:
+    case PARAM_PRIMARY_MIXER_4_0:
+    case PARAM_PRIMARY_MIXER_5_0:
+
+    case PARAM_PRIMARY_MIXER_0_1:
+    case PARAM_PRIMARY_MIXER_1_1:
+    case PARAM_PRIMARY_MIXER_2_1:
+    case PARAM_PRIMARY_MIXER_3_1:
+    case PARAM_PRIMARY_MIXER_4_1:
+    case PARAM_PRIMARY_MIXER_5_1:
+
+    case PARAM_PRIMARY_MIXER_0_2:
+    case PARAM_PRIMARY_MIXER_1_2:
+    case PARAM_PRIMARY_MIXER_2_2:
+    case PARAM_PRIMARY_MIXER_3_2:
+    case PARAM_PRIMARY_MIXER_4_2:
+    case PARAM_PRIMARY_MIXER_5_2:
+
+    case PARAM_PRIMARY_MIXER_0_3:
+    case PARAM_PRIMARY_MIXER_1_3:
+    case PARAM_PRIMARY_MIXER_2_3:
+    case PARAM_PRIMARY_MIXER_3_3:
+    case PARAM_PRIMARY_MIXER_4_3:
+    case PARAM_PRIMARY_MIXER_5_3:
+
+    case PARAM_PRIMARY_MIXER_0_4:
+    case PARAM_PRIMARY_MIXER_1_4:
+    case PARAM_PRIMARY_MIXER_2_4:
+    case PARAM_PRIMARY_MIXER_3_4:
+    case PARAM_PRIMARY_MIXER_4_4:
+    case PARAM_PRIMARY_MIXER_5_4:
+
+    case PARAM_PRIMARY_MIXER_0_5:
+    case PARAM_PRIMARY_MIXER_1_5:
+    case PARAM_PRIMARY_MIXER_2_5:
+    case PARAM_PRIMARY_MIXER_3_5:
+    case PARAM_PRIMARY_MIXER_4_5:
+    case PARAM_PRIMARY_MIXER_5_5:
+
+    case PARAM_PRIMARY_MIXER_0_6:
+    case PARAM_PRIMARY_MIXER_1_6:
+    case PARAM_PRIMARY_MIXER_2_6:
+    case PARAM_PRIMARY_MIXER_3_6:
+    case PARAM_PRIMARY_MIXER_4_6:
+    case PARAM_PRIMARY_MIXER_5_6:
+
+    case PARAM_PRIMARY_MIXER_0_7:
+    case PARAM_PRIMARY_MIXER_1_7:
+    case PARAM_PRIMARY_MIXER_2_7:
+    case PARAM_PRIMARY_MIXER_3_7:
+    case PARAM_PRIMARY_MIXER_4_7:
+    case PARAM_PRIMARY_MIXER_5_7:
+
+    case PARAM_PRIMARY_MIXER_0_8:
+    case PARAM_PRIMARY_MIXER_1_8:
+    case PARAM_PRIMARY_MIXER_2_8:
+    case PARAM_PRIMARY_MIXER_3_8:
+    case PARAM_PRIMARY_MIXER_4_8:
+    case PARAM_PRIMARY_MIXER_5_8:
+
+    case PARAM_PRIMARY_MIXER_0_9:
+    case PARAM_PRIMARY_MIXER_1_9:
+    case PARAM_PRIMARY_MIXER_2_9:
+    case PARAM_PRIMARY_MIXER_3_9:
+    case PARAM_PRIMARY_MIXER_4_9:
+    case PARAM_PRIMARY_MIXER_5_9:
     case PARAM_MIXER:
+      init_mixing();
+      break;
     case PARAM_MOTOR_RESISTANCE:
     case PARAM_AIR_DENSITY:
     case PARAM_MOTOR_KV:
@@ -59,7 +153,7 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PROP_CQ:
     case PARAM_NUM_MOTORS:
     case PARAM_VOLT_MAX:
-      init_mixing();
+      update_parameters();
       break;
     case PARAM_MOTOR_PWM_SEND_RATE:
     case PARAM_RC_TYPE:
@@ -94,6 +188,12 @@ void Mixer::init_mixing()
 
       load_primary_mixer_values();
       mixer_to_use_ = &primary_mixer_;
+
+      // Set flag since we are using custom mixer
+      use_motor_parameters_ = true;
+
+      // Update the motor parameters that will be used
+      update_parameters();
     } else if (mixer_to_use_ != &fixedwing_mixing &&
                mixer_to_use_ != &fixedwing_inverted_vtail_mixing) {
       // Don't invert the fixedwing mixers
@@ -104,6 +204,9 @@ void Mixer::init_mixing()
       // Otherwise, invert the selected "canned" matrix
       primary_mixer_ = invert_mixer(mixer_to_use_);
       mixer_to_use_ = &primary_mixer_;
+
+      // Unset flag since we are not using custom mixer
+      use_motor_parameters_ = false;
     }
 
     std::cout << "Mixer: " << std::endl;
@@ -241,75 +344,75 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9);
 
   // Load the mixer values from the firmware parameters
-  primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
-  primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
-  primary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_1);
-  primary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_1);
-  primary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_1);
-  primary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_1);
-
-  primary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_2);
-  primary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_2);
-  primary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_2);
-  primary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_2);
-  primary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_2);
-  primary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_2);
-
-  primary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_3);
-  primary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_3);
-  primary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_3);
-  primary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_3);
-  primary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_3);
-  primary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_3);
-
-  primary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_4);
-  primary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_4);
-  primary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_4);
-  primary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_4);
-  primary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_4);
-  primary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_4);
-
-  primary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_5);
-  primary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_5);
-  primary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_5);
-  primary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_5);
-  primary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_5);
-  primary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_5);
-
-  primary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_6);
-  primary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_6);
-  primary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_6);
-  primary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_6);
-  primary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_6);
-  primary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_6);
-
-  primary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_7);
-  primary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_7);
-  primary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_7);
-  primary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_7);
-  primary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_7);
-  primary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_7);
-
-  primary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_8);
-  primary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_8);
-  primary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_8);
-  primary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_8);
-  primary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_8);
-  primary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_8);
-
-  primary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_9);
-  primary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_9);
-  primary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_9);
-  primary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_9);
-  primary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_9);
-  primary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_9);
-
-  primary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_10);
-  primary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_10);
-  primary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_10);
-  primary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_10);
-  primary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_10);
-  primary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_6_10);
+  primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_0);
+  primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_0);
+  primary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_0);
+  primary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_0);
+  primary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_0);
+  primary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_0);
+
+  primary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_1);
+  primary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
+  primary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
+  primary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_1);
+  primary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_1);
+  primary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_1);
+
+  primary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_2);
+  primary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_2);
+  primary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_2);
+  primary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_2);
+  primary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_2);
+  primary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_2);
+
+  primary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_3);
+  primary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_3);
+  primary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_3);
+  primary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_3);
+  primary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_3);
+  primary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_3);
+
+  primary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_4);
+  primary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_4);
+  primary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_4);
+  primary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_4);
+  primary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_4);
+  primary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_4);
+
+  primary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_5);
+  primary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_5);
+  primary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_5);
+  primary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_5);
+  primary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_5);
+  primary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_5);
+
+  primary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_6);
+  primary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_6);
+  primary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_6);
+  primary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_6);
+  primary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_6);
+  primary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_6);
+
+  primary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_7);
+  primary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_7);
+  primary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_7);
+  primary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_7);
+  primary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_7);
+  primary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_7);
+
+  primary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_8);
+  primary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_8);
+  primary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_8);
+  primary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_8);
+  primary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_8);
+  primary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_8);
+
+  primary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_9);
+  primary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_9);
+  primary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_9);
+  primary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_9);
+  primary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_9);
+  primary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_9);
 }
 
 void Mixer::init_PWM()
@@ -401,7 +504,15 @@ float Mixer::mix_multirotor_with_motor_parameters()
 {
   Controller::Output commands = RF_.controller_.output();
 
-  if (commands.Fz < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
+  // std::cout << "Commands: ";
+  // std::cout << commands.Fx << " ";
+  // std::cout << commands.Fy << " Fz: ";
+  // std::cout << commands.Fz << " ";
+  // std::cout << commands.Qx << " ";
+  // std::cout << commands.Qy << " ";
+  // std::cout << commands.Qz << " " << std::endl;
+
+  if (abs(commands.Fz) < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
                * RF_.controller_.max_thrust()) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
@@ -411,6 +522,8 @@ float Mixer::mix_multirotor_with_motor_parameters()
   // Mix the inputs
   float max_output = 1.0;
 
+  // TODO: Skip this if not armed. Otherwise this may spin up because of the R*io.
+
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if (mixer_to_use_->output_type[i] != NONE) {
       // Matrix multiply to mix outputs
@@ -420,16 +533,21 @@ float Mixer::mix_multirotor_with_motor_parameters()
                             commands.Qx * mixer_to_use_->Qx[i] +
                             commands.Qy * mixer_to_use_->Qy[i] +
                             commands.Qz * mixer_to_use_->Qz[i];
-
+      
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
 
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
-      // Note that we assume constant airspeed and propeller speed, leading to constant advance ratio,
-      // torque, and thrust constants.
+      // Note that we assume constant advance ratio, leading to constant torque and thrust constants.
       float V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
+      // std::cout << "Vin: " << V_in << " " << std::endl;
+      // std::cout << "p " << rho_ << " D^5 " << pow(D_, 5.0) << " C_Q " << C_Q_ << " R " << R_ << " ";
+      // std::cout << "io " << i_0_ << " k_v " << K_V_ << " omsqrt " << sqrt(omega_squared) << std::endl;
+      // std::cout << "Vin_0 " << rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_ << " ";
+      // std::cout << "Vin_1 " << R_ * i_0_ + K_V_ * sqrt(omega_squared) << std::endl;
+
       // Convert desired V_in setting to a throttle setting
       outputs_[i] = V_in / V_max_;
 
@@ -445,7 +563,7 @@ void Mixer::mix_multirotor()
 {
   // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected.
   float max_output;
-  if (mixer_to_use_ == &custom_mixing) {
+  if (use_motor_parameters_) {
     max_output = mix_multirotor_with_motor_parameters();
   } else {
     max_output = mix_multirotor_without_motor_parameters();
@@ -469,11 +587,11 @@ void Mixer::mix_multirotor()
     if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; }
   }
 
- // std::cout << "Outputs (post scale): ";
- // for (auto i : outputs_) {
- //   std::cout << i << " ";
- // }
- // std::cout << std::endl;
+ std::cout << "Outputs (post scale): ";
+ for (auto i : outputs_) {
+   std::cout << i << " ";
+ }
+ std::cout << std::endl;
 
 }
 
@@ -555,6 +673,7 @@ void Mixer::mix_output()
       }
       raw_outputs_[i] = value;
     }
+    // TODO: Why are the outputs not zero for the non-motor channels?
   }
   RF_.board_.pwm_write_multi(raw_outputs_, NUM_TOTAL_OUTPUTS);
 }
diff --git a/src/param.cpp b/src/param.cpp
index eaa6373a..f3e87214 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -141,75 +141,75 @@ void Params::set_defaults(void)
   init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRIM_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8
   init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRIM_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_0, "PRI_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0]
+  init_param_float(PARAM_PRIMARY_MIXER_1_0, "PRI_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0]
+  init_param_float(PARAM_PRIMARY_MIXER_2_0, "PRI_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0]
+  init_param_float(PARAM_PRIMARY_MIXER_3_0, "PRI_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0]
+  init_param_float(PARAM_PRIMARY_MIXER_4_0, "PRI_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0]
+  init_param_float(PARAM_PRIMARY_MIXER_5_0, "PRI_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0]
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_1, "PRI_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1]
   init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
   init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
   init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
   init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRI_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
   init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRI_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
-  init_param_float(PARAM_PRIMARY_MIXER_6_1, "PRI_MIXER_6_1", 0.0f); // Value of the custom mixer at element [6,1]
-
+                                                                
+  init_param_float(PARAM_PRIMARY_MIXER_0_2, "PRI_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2]
   init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRI_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
   init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRI_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
   init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRI_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
   init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRI_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
   init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRI_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
-  init_param_float(PARAM_PRIMARY_MIXER_6_2, "PRI_MIXER_6_2", 0.0f); // Value of the custom mixer at element [6,2]
-                                                                
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_3, "PRI_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3]
   init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRI_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
   init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRI_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
   init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRI_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
   init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRI_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
   init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRI_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
-  init_param_float(PARAM_PRIMARY_MIXER_6_3, "PRI_MIXER_6_3", 0.0f); // Value of the custom mixer at element [6,3]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_4, "PRI_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4]
   init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRI_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
   init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRI_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
   init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRI_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
   init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRI_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
   init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRI_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
-  init_param_float(PARAM_PRIMARY_MIXER_6_4, "PRI_MIXER_6_4", 0.0f); // Value of the custom mixer at element [6,4]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_5, "PRI_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5]
   init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRI_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
   init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRI_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
   init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRI_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
   init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRI_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
   init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRI_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
-  init_param_float(PARAM_PRIMARY_MIXER_6_5, "PRI_MIXER_6_5", 0.0f); // Value of the custom mixer at element [6,5]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_6, "PRI_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6]
   init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRI_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
   init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRI_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
   init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRI_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
   init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRI_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
   init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRI_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
-  init_param_float(PARAM_PRIMARY_MIXER_6_6, "PRI_MIXER_6_6", 0.0f); // Value of the custom mixer at element [6,6]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_7, "PRI_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7]
   init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRI_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
   init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRI_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
   init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRI_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
   init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRI_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
   init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRI_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
-  init_param_float(PARAM_PRIMARY_MIXER_6_7, "PRI_MIXER_6_7", 0.0f); // Value of the custom mixer at element [6,7]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_8, "PRI_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8]
   init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRI_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
   init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRI_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
   init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRI_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
   init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRI_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
   init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRI_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
-  init_param_float(PARAM_PRIMARY_MIXER_6_8, "PRI_MIXER_6_8", 0.0f); // Value of the custom mixer at element [6,8]
 
+  init_param_float(PARAM_PRIMARY_MIXER_0_9, "PRI_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9]
   init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRI_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
   init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRI_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
   init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRI_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
   init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
   init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
-  init_param_float(PARAM_PRIMARY_MIXER_6_9, "PRI_MIXER_6_9", 0.0f); // Value of the custom mixer at element [6,9]
-
-  init_param_float(PARAM_PRIMARY_MIXER_1_10, "PRI_MIXER_1_10", 0.0f); // Value of the custom mixer at element [1,10]
-  init_param_float(PARAM_PRIMARY_MIXER_2_10, "PRI_MIXER_2_10", 0.0f); // Value of the custom mixer at element [2,10]
-  init_param_float(PARAM_PRIMARY_MIXER_3_10, "PRI_MIXER_3_10", 0.0f); // Value of the custom mixer at element [3,10]
-  init_param_float(PARAM_PRIMARY_MIXER_4_10, "PRI_MIXER_4_10", 0.0f); // Value of the custom mixer at element [4,10]
-  init_param_float(PARAM_PRIMARY_MIXER_5_10, "PRI_MIXER_5_10", 0.0f); // Value of the custom mixer at element [5,10]
-  init_param_float(PARAM_PRIMARY_MIXER_6_10, "PRI_MIXER_6_10", 0.0f); // Value of the custom mixer at element [6,10]
 
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/

From bcd88bd64eac911a52f7a955ee7299b5c92fe267 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 11 Nov 2024 14:14:30 -0700
Subject: [PATCH 29/41] fixed aux channels not getting initialized. Changed the
 way servos are mixed with multirotor

---
 src/mixer.cpp | 39 +++++++++++++++++++++++++--------------
 1 file changed, 25 insertions(+), 14 deletions(-)

diff --git a/src/mixer.cpp b/src/mixer.cpp
index 468f0e19..ecabd912 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -43,6 +43,11 @@ Mixer::Mixer(ROSflight & _rf)
 {
   mixer_to_use_ = nullptr;
   use_motor_parameters_ = false;
+
+  for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
+    aux_command_.channel[i].type = NONE;
+    aux_command_.channel[i].value = 0.0f;
+  }
 }
 
 void Mixer::init() { init_mixing(); }
@@ -522,11 +527,9 @@ float Mixer::mix_multirotor_with_motor_parameters()
   // Mix the inputs
   float max_output = 1.0;
 
-  // TODO: Skip this if not armed. Otherwise this may spin up because of the R*io.
-
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (mixer_to_use_->output_type[i] != NONE) {
-      // Matrix multiply to mix outputs
+    if (mixer_to_use_->output_type[i] == M) {
+      // Matrix multiply to mix outputs for Motor type
       float omega_squared = commands.Fx * mixer_to_use_->Fx[i] +
                             commands.Fy * mixer_to_use_->Fy[i] +
                             commands.Fz * mixer_to_use_->Fz[i] +
@@ -551,9 +554,18 @@ float Mixer::mix_multirotor_with_motor_parameters()
       // Convert desired V_in setting to a throttle setting
       outputs_[i] = V_in / V_max_;
 
-      // Save off the largest control output if it is greater than 1.0 for future scaling
-      if (outputs_[i] > max_output) { max_output = outputs_[i]; }
+    } else if (mixer_to_use_->output_type[i] == S) {
+      // Matrix multiply to mix outputs for Servo type
+      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
+                    commands.Fy * mixer_to_use_->Fy[i] +
+                    commands.Fz * mixer_to_use_->Fz[i] +
+                    commands.Qx * mixer_to_use_->Qx[i] +
+                    commands.Qy * mixer_to_use_->Qy[i] +
+                    commands.Qz * mixer_to_use_->Qz[i];
     }
+
+    // Save off the largest control output if it is greater than 1.0 for future scaling
+    if (outputs_[i] > max_output) { max_output = outputs_[i]; }
   }
 
   return max_output;
@@ -586,13 +598,6 @@ void Mixer::mix_multirotor()
     // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated)
     if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; }
   }
-
- std::cout << "Outputs (post scale): ";
- for (auto i : outputs_) {
-   std::cout << i << " ";
- }
- std::cout << std::endl;
-
 }
 
 void Mixer::mix_fixedwing()
@@ -648,6 +653,13 @@ void Mixer::mix_output()
     combined_output_type_[i] = aux_command_.channel[i].type;
   }
 
+  // std::cout << "Outputs (post scale): ";
+  // for (auto i : outputs_) {
+  //   std::cout << i << " ";
+  // }
+  // std::cout << std::endl;
+
+
   // Write to outputs
   for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
     float value = outputs_[i];
@@ -673,7 +685,6 @@ void Mixer::mix_output()
       }
       raw_outputs_[i] = value;
     }
-    // TODO: Why are the outputs not zero for the non-motor channels?
   }
   RF_.board_.pwm_write_multi(raw_outputs_, NUM_TOTAL_OUTPUTS);
 }

From c6c66db054e955596932f1387555314c29d4477b Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 12 Nov 2024 12:30:41 -0700
Subject: [PATCH 30/41] Added parameter to choose which axis the RC throttle
 corresponds to

---
 include/command_manager.h |  7 ++++
 include/mixer.h           | 13 ++++---
 include/param.h           |  1 +
 src/command_manager.cpp   | 60 ++++++++++++++++++++++++++----
 src/controller.cpp        | 38 +++++++++++++++++--
 src/mixer.cpp             | 78 ++++++++++++++++++---------------------
 src/param.cpp             |  1 +
 7 files changed, 138 insertions(+), 60 deletions(-)

diff --git a/include/command_manager.h b/include/command_manager.h
index 2fdce9a5..065dc52c 100644
--- a/include/command_manager.h
+++ b/include/command_manager.h
@@ -58,6 +58,13 @@ 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;
diff --git a/include/mixer.h b/include/mixer.h
index 18c5d262..d077c937 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -33,6 +33,7 @@
 #define ROSFLIGHT_FIRMWARE_MIXER_H
 
 #include "interface/param_listener.h"
+#include "controller.h"
 
 #include <Eigen/Dense>
 #include <Eigen/SVD>
@@ -112,8 +113,8 @@ class Mixer : public ParamListenerInterface
   void add_header_to_mixer(mixer_t* mixer);
   void load_primary_mixer_values();
   mixer_t invert_mixer(const mixer_t* mixer_to_invert);
-  float mix_multirotor_with_motor_parameters();
-  float mix_multirotor_without_motor_parameters();
+  float mix_multirotor_with_motor_parameters(Controller::Output commands);
+  float mix_multirotor_without_motor_parameters(Controller::Output commands);
 
   // clang-format off
 
@@ -208,9 +209,9 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t fixedwing_mixing = {
-    {   S,    S,    S,    M, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
+    {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
     { 50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
+    {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
     {1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
@@ -218,9 +219,9 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t fixedwing_inverted_vtail_mixing = {
-    {   S,     S,    S,    M, NONE, NONE, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
+    {   S,     S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
     {  50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
-    {0.0f,  0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
+    {0.0f,  0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
     {0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_z Mix
     {1.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // Q_x Mix 
diff --git a/include/param.h b/include/param.h
index 27cf14aa..d0bd9d8a 100644
--- a/include/param.h
+++ b/include/param.h
@@ -262,6 +262,7 @@ enum : uint16_t
   PARAM_RC_Y_CHANNEL,
   PARAM_RC_Z_CHANNEL,
   PARAM_RC_F_CHANNEL,
+  PARAM_RC_F_AXIS,
   PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL,
   PARAM_RC_THROTTLE_OVERRIDE_CHANNEL,
   PARAM_RC_ATT_CONTROL_TYPE_CHANNEL,
diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index 68371446..a66fff3d 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -91,8 +91,18 @@ void CommandManager::init_failsafe()
     RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_FAILSAFE);
   }
 
-  // TODO: Add parameter to choose which F the RC throttle corresponds to
-  multirotor_failsafe_command_.Fz.value = failsafe_thr_param;
+  // Make sure the failsafe is set to the axis associated with the RC F command
+  switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
+    case X_AXIS:
+      multirotor_failsafe_command_.Fx.value = failsafe_thr_param;
+      break;
+    case Y_AXIS:
+      multirotor_failsafe_command_.Fy.value = failsafe_thr_param;
+      break;
+    default:
+      multirotor_failsafe_command_.Fz.value = failsafe_thr_param;
+      break;
+  }
 
   if (fixedwing) {
     failsafe_command_ = fixedwing_failsafe_command_;
@@ -108,15 +118,33 @@ void CommandManager::interpret_rc(void)
   rc_command_.Qx.value = RF_.rc_.stick(RC::STICK_X);
   rc_command_.Qy.value = RF_.rc_.stick(RC::STICK_Y);
   rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z);
-  rc_command_.Fx.value = 0.0;
-  rc_command_.Fy.value = 0.0;
-  rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
+
+  // Load the RC command based on the axis associated with the RC F command
+  switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
+    case X_AXIS:   // RC F = X axis
+      rc_command_.Fx.value = RF_.rc_.stick(RC::STICK_F);
+      rc_command_.Fy.value = 0.0;
+      rc_command_.Fz.value = 0.0;
+      break;
+    case Y_AXIS:   // RC F = Y axis
+      rc_command_.Fx.value = 0.0;
+      rc_command_.Fy.value = RF_.rc_.stick(RC::STICK_F);
+      rc_command_.Fz.value = 0.0;
+      break;
+    default:   // RC F = Z axis
+      rc_command_.Fx.value = 0.0;
+      rc_command_.Fy.value = 0.0;
+      rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
+      break;
+  }
 
   // determine control mode for each channel and scale command values accordingly
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
     rc_command_.Qx.type = PASSTHROUGH;
     rc_command_.Qy.type = PASSTHROUGH;
     rc_command_.Qz.type = PASSTHROUGH;
+    rc_command_.Fx.type = PASSTHROUGH;
+    rc_command_.Fy.type = PASSTHROUGH;
     rc_command_.Fz.type = PASSTHROUGH;
   } else {
     // roll and pitch
@@ -149,6 +177,8 @@ void CommandManager::interpret_rc(void)
     rc_command_.Qz.value *= RF_.params_.get_param_float(PARAM_RC_MAX_YAWRATE);
 
     // throttle
+    rc_command_.Fx.type = THROTTLE;
+    rc_command_.Fy.type = THROTTLE;
     rc_command_.Fz.type = THROTTLE;
   }
 }
@@ -194,15 +224,29 @@ bool CommandManager::do_roll_pitch_yaw_muxing(MuxChannel channel)
 bool CommandManager::do_throttle_muxing(void)
 {
   bool override_this_channel = false;
+  MuxChannel selected_channel;
+  // Determine which channel to check based on which axis the RC channel corresponds to
+  switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
+    case X_AXIS: 
+      selected_channel = MUX_FX;
+      break;
+    case Y_AXIS: 
+      selected_channel = MUX_FY;
+      break;
+    default:
+      selected_channel = MUX_FZ;
+      break;
+  }
+
   // Check if the override switch exists and is triggered
   if (RF_.rc_.switch_mapped(RC::SWITCH_THROTTLE_OVERRIDE)
       && RF_.rc_.switch_on(RC::SWITCH_THROTTLE_OVERRIDE)) {
     override_this_channel = true;
   } else { // Otherwise check if the offboard throttle channel is active, if it isn't, have RC override
-    if (muxes[MUX_FZ].onboard->active) {
+    if (muxes[selected_channel].onboard->active) {
       // Check if the parameter flag is set to have us always take the smaller throttle
       if (RF_.params_.get_param_int(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE)) {
-        override_this_channel = (muxes[MUX_FZ].rc->value < muxes[MUX_FZ].onboard->value);
+        override_this_channel = (muxes[selected_channel].rc->value < muxes[selected_channel].onboard->value);
       } else {
         override_this_channel = false;
       }
@@ -212,7 +256,7 @@ bool CommandManager::do_throttle_muxing(void)
   }
 
   // Set the combined channel output depending on whether RC is overriding for this channel or not
-  *muxes[MUX_FZ].combined = override_this_channel ? *muxes[MUX_FZ].rc : *muxes[MUX_FZ].onboard;
+  *muxes[selected_channel].combined = override_this_channel ? *muxes[selected_channel].rc : *muxes[selected_channel].onboard;
   return override_this_channel;
 }
 
diff --git a/src/controller.cpp b/src/controller.cpp
index ab7eaf29..08111814 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -118,6 +118,7 @@ void Controller::run()
 
   // Check if integrators should be updated
   //! @todo better way to figure out if throttle is high
+  // TODO: fix this... Needs to be checked based on the throttle channel (not necessarily Fz)
   bool update_integrators = (RF_.state_manager_.state().armed)
     && (RF_.command_manager_.combined_control().Fz.value > 0.1f) && dt_us < 10000;
 
@@ -126,13 +127,13 @@ void Controller::run()
     dt_us, RF_.estimator_.state(), RF_.command_manager_.combined_control(), update_integrators);
 
   // Add feedforward torques
-  // Note that the controller does not alter Fx and Fy
   output_.Qx = pid_output.Qx + RF_.params_.get_param_float(PARAM_X_EQ_TORQUE);
   output_.Qy = pid_output.Qy + RF_.params_.get_param_float(PARAM_Y_EQ_TORQUE);
   output_.Qz = pid_output.Qz + RF_.params_.get_param_float(PARAM_Z_EQ_TORQUE);
+
+  output_.Fx = pid_output.Fx;
+  output_.Fy = pid_output.Fy;
   output_.Fz = pid_output.Fz;
-  output_.Fx = RF_.command_manager_.combined_control().Fx.value;
-  output_.Fy = RF_.command_manager_.combined_control().Fy.value;
 }
 
 void Controller::calculate_equilbrium_torque_from_rc()
@@ -257,12 +258,41 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
     out.Qz = command.Qz.value;
   }
 
-  // THROTTLE
+  // Fx
+  if (command.Fx.type == THROTTLE) {
+    // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability 
+    // during aggressive maneuvers.
+    out.Fx = command.Fx.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
+
+    if (RF_.mixer_.use_motor_parameters()) {
+      out.Fx *= max_thrust_;
+    }
+  } else {
+    // If it is not a throttle setting then pass directly to the mixer.
+    out.Fx = command.Fx.value;
+  }
+
+  // Fy
+  if (command.Fy.type == THROTTLE) {
+    // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability 
+    // during aggressive maneuvers.
+    out.Fy = command.Fy.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
+
+    if (RF_.mixer_.use_motor_parameters()) {
+      out.Fy *= max_thrust_;
+    }
+  } else {
+    // If it is not a throttle setting then pass directly to the mixer.
+    out.Fy = command.Fy.value;
+  }
+
+  // Fz
   if (command.Fz.type == THROTTLE) {
     // Scales the saturation limit by RC_MAX_THROTTLE to maintain controllability 
     // during aggressive maneuvers.
     // Also note the negative sign. Since the mixer assumes the inputs are in the NED
     // frame, a throttle command corresponds to a thrust command in the negative direction.
+    // Note that this also assumes that a high throttle means fly "up" (negative down)
     out.Fz = -command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
 
     if (RF_.mixer_.use_motor_parameters()) {
diff --git a/src/mixer.cpp b/src/mixer.cpp
index ecabd912..f3a3cdb2 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -466,24 +466,8 @@ void Mixer::set_new_aux_command(aux_command_t new_aux_command)
   }
 }
 
-float Mixer::mix_multirotor_without_motor_parameters()
+float Mixer::mix_multirotor_without_motor_parameters(Controller::Output commands)
 {
-  Controller::Output commands = RF_.controller_.output();
-
-//  std::cout << "Commands: ";
-//  std::cout << commands.Fx << " ";
-//  std::cout << commands.Fy << " Fz: ";
-//  std::cout << commands.Fz << " ";
-//  std::cout << commands.Qx << " ";
-//  std::cout << commands.Qy << " ";
-//  std::cout << commands.Qz << " " << std::endl;
-
-  if (commands.Fz < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
-    // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
-    // arming/disarming
-    commands.Qz = 0.0;
-  }
-
   // Mix the inputs
   float max_output = 1.0;
 
@@ -505,25 +489,8 @@ float Mixer::mix_multirotor_without_motor_parameters()
   return max_output;
 }
 
-float Mixer::mix_multirotor_with_motor_parameters()
+float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
 {
-  Controller::Output commands = RF_.controller_.output();
-
-  // std::cout << "Commands: ";
-  // std::cout << commands.Fx << " ";
-  // std::cout << commands.Fy << " Fz: ";
-  // std::cout << commands.Fz << " ";
-  // std::cout << commands.Qx << " ";
-  // std::cout << commands.Qy << " ";
-  // std::cout << commands.Qz << " " << std::endl;
-
-  if (abs(commands.Fz) < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
-               * RF_.controller_.max_thrust()) {
-    // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
-    // arming/disarming
-    commands.Qz = 0.0;
-  }
-
   // Mix the inputs
   float max_output = 1.0;
 
@@ -573,12 +540,34 @@ float Mixer::mix_multirotor_with_motor_parameters()
 
 void Mixer::mix_multirotor()
 {
+  Controller::Output commands = RF_.controller_.output();
+
+  // Test the throttle command based on the axis corresponding to the RC F channel
+  float throttle_command = 0.0;
+  switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
+    case X_AXIS:
+      throttle_command = commands.Fx;
+      break;
+    case Y_AXIS:
+      throttle_command = commands.Fy;
+      break;
+    default:
+      throttle_command = commands.Fz;
+      break;
+  }
+  
+  if (throttle_command < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
+    // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
+    // arming/disarming
+    commands.Qz = 0.0;
+  }
+
   // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected.
   float max_output;
   if (use_motor_parameters_) {
-    max_output = mix_multirotor_with_motor_parameters();
+    max_output = mix_multirotor_with_motor_parameters(commands);
   } else {
-    max_output = mix_multirotor_without_motor_parameters();
+    max_output = mix_multirotor_without_motor_parameters(commands);
   }
 
 //  std::cout << "Outputs (pre scale): ";
@@ -603,6 +592,11 @@ void Mixer::mix_multirotor()
 void Mixer::mix_fixedwing()
 {
   Controller::Output commands = RF_.controller_.output();
+  
+  std::cout << "Command output/Mixer input: ";
+  std::cout << commands.Fx << " " << commands.Fy << " ";
+  std::cout << commands.Fz << " " << commands.Qx << " ";
+  std::cout << commands.Qy << " " << commands.Qz << " ";
 
   // Reverse fixed-wing channels just before mixing if we need to
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
@@ -653,11 +647,11 @@ void Mixer::mix_output()
     combined_output_type_[i] = aux_command_.channel[i].type;
   }
 
-  // std::cout << "Outputs (post scale): ";
-  // for (auto i : outputs_) {
-  //   std::cout << i << " ";
-  // }
-  // std::cout << std::endl;
+  std::cout << "Outputs (post scale): ";
+  for (auto i : outputs_) {
+    std::cout << i << " ";
+  }
+  std::cout << std::endl;
 
 
   // Write to outputs
diff --git a/src/param.cpp b/src/param.cpp
index f3e87214..3f6ace49 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -309,6 +309,7 @@ void Params::set_defaults(void)
   init_param_int(PARAM_RC_Y_CHANNEL, "RC_Y_CHN", 1); // RC input channel mapped to y-axis commands [0 - indexed] | 0 | 3
   init_param_int(PARAM_RC_Z_CHANNEL, "RC_Z_CHN", 3); // RC input channel mapped to z-axis commands [0 - indexed] | 0 | 3
   init_param_int(PARAM_RC_F_CHANNEL, "RC_F_CHN", 2); // RC input channel mapped to F-axis commands [0 - indexed] | 0 | 3
+  init_param_int(PARAM_RC_F_AXIS, "RC_F_AXIS", 2);   // NED axis that RC F-channel gets mapped to 0 - X, 1 - Y, 2 - Z | 0 | 2
   init_param_int(PARAM_RC_ATTITUDE_OVERRIDE_CHANNEL, "RC_ATT_OVRD_CHN", 4); // RC switch mapped to attitude override [0 indexed, -1 to disable] | 4 | 7
   init_param_int(PARAM_RC_THROTTLE_OVERRIDE_CHANNEL, "RC_THR_OVRD_CHN", 4); // RC switch channel mapped to throttle override [0 indexed, -1 to disable] | 4 | 7
   init_param_int(PARAM_RC_ATT_CONTROL_TYPE_CHANNEL,  "RC_ATT_CTRL_CHN",-1); // RC switch channel mapped to attitude control type [0 indexed, -1 to disable] | 4 | 7

From 9d85c98fb02b6c6c26b22f13b85c9fc826814c1e Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 26 Nov 2024 16:12:26 -0700
Subject: [PATCH 31/41] added support for secondary mixer

---
 include/command_manager.h |   5 +-
 include/mixer.h           |  19 +-
 include/param.h           |  74 ++++++-
 src/command_manager.cpp   |  20 +-
 src/controller.cpp        |   6 +-
 src/mixer.cpp             | 431 ++++++++++++++++++++++++++++----------
 src/param.cpp             | 118 +++++++++--
 7 files changed, 529 insertions(+), 144 deletions(-)

diff --git a/include/command_manager.h b/include/command_manager.h
index 065dc52c..c3f668b8 100644
--- a/include/command_manager.h
+++ b/include/command_manager.h
@@ -161,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_;
 
@@ -180,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);
diff --git a/include/mixer.h b/include/mixer.h
index d077c937..405de4e7 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -89,6 +89,19 @@ class Mixer : public ParamListenerInterface
     float Qz[NUM_MIXER_OUTPUTS];
   } mixer_t;
 
+  typedef struct 
+  {
+    mixer_t* primary_mixer_ptr;
+    output_type_t (*output_type)[NUM_MIXER_OUTPUTS];
+    float (*default_pwm_rate)[NUM_MIXER_OUTPUTS];
+    float (*Fx)[NUM_MIXER_OUTPUTS];
+    float (*Fy)[NUM_MIXER_OUTPUTS];
+    float (*Fz)[NUM_MIXER_OUTPUTS];
+    float (*Qx)[NUM_MIXER_OUTPUTS];
+    float (*Qy)[NUM_MIXER_OUTPUTS];
+    float (*Qz)[NUM_MIXER_OUTPUTS];
+  } mixer_selection_t;
+
   typedef struct
   {
     output_type_t type;
@@ -112,6 +125,7 @@ class Mixer : public ParamListenerInterface
   void write_servo(uint8_t index, float value);
   void add_header_to_mixer(mixer_t* mixer);
   void load_primary_mixer_values();
+  void load_secondary_mixer_values();
   mixer_t invert_mixer(const mixer_t* mixer_to_invert);
   float mix_multirotor_with_motor_parameters(Controller::Output commands);
   float mix_multirotor_without_motor_parameters(Controller::Output commands);
@@ -239,9 +253,9 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
 
   mixer_t primary_mixer_;
+  mixer_t secondary_mixer_;
 
-  const mixer_t * mixer_to_use_;
-  bool use_motor_parameters_;
+  mixer_selection_t mixer_to_use_;
 
   const mixer_t* array_of_mixers_[NUM_MIXERS] = {
     &esc_calibration_mixing,
@@ -284,7 +298,6 @@ class Mixer : public ParamListenerInterface
   void param_change_callback(uint16_t param_id) override;
   void set_new_aux_command(aux_command_t new_aux_command);
   inline const float * get_outputs() const { return raw_outputs_; }
-  inline const bool use_motor_parameters() const { return use_motor_parameters_; }
 
   void calculate_mixer_values();
   void mix_multirotor();
diff --git a/include/param.h b/include/param.h
index d0bd9d8a..1a61933b 100644
--- a/include/param.h
+++ b/include/param.h
@@ -72,6 +72,7 @@ enum : uint16_t
   PARAM_PROP_CT,
   PARAM_PROP_CQ,
   PARAM_VOLT_MAX,
+  PARAM_USE_MOTOR_PARAMETERS,
 
   PARAM_PRIMARY_MIXER_OUTPUT_0,
   PARAM_PRIMARY_MIXER_OUTPUT_1,
@@ -165,6 +166,76 @@ enum : uint16_t
   PARAM_PRIMARY_MIXER_4_9,
   PARAM_PRIMARY_MIXER_5_9,
 
+  PARAM_SECONDARY_MIXER_0_0,
+  PARAM_SECONDARY_MIXER_1_0,
+  PARAM_SECONDARY_MIXER_2_0,
+  PARAM_SECONDARY_MIXER_3_0,
+  PARAM_SECONDARY_MIXER_4_0,
+  PARAM_SECONDARY_MIXER_5_0,
+
+  PARAM_SECONDARY_MIXER_0_1,
+  PARAM_SECONDARY_MIXER_1_1,
+  PARAM_SECONDARY_MIXER_2_1,
+  PARAM_SECONDARY_MIXER_3_1,
+  PARAM_SECONDARY_MIXER_4_1,
+  PARAM_SECONDARY_MIXER_5_1,
+
+  PARAM_SECONDARY_MIXER_0_2,
+  PARAM_SECONDARY_MIXER_1_2,
+  PARAM_SECONDARY_MIXER_2_2,
+  PARAM_SECONDARY_MIXER_3_2,
+  PARAM_SECONDARY_MIXER_4_2,
+  PARAM_SECONDARY_MIXER_5_2,
+
+  PARAM_SECONDARY_MIXER_0_3,
+  PARAM_SECONDARY_MIXER_1_3,
+  PARAM_SECONDARY_MIXER_2_3,
+  PARAM_SECONDARY_MIXER_3_3,
+  PARAM_SECONDARY_MIXER_4_3,
+  PARAM_SECONDARY_MIXER_5_3,
+
+  PARAM_SECONDARY_MIXER_0_4,
+  PARAM_SECONDARY_MIXER_1_4,
+  PARAM_SECONDARY_MIXER_2_4,
+  PARAM_SECONDARY_MIXER_3_4,
+  PARAM_SECONDARY_MIXER_4_4,
+  PARAM_SECONDARY_MIXER_5_4,
+
+  PARAM_SECONDARY_MIXER_0_5,
+  PARAM_SECONDARY_MIXER_1_5,
+  PARAM_SECONDARY_MIXER_2_5,
+  PARAM_SECONDARY_MIXER_3_5,
+  PARAM_SECONDARY_MIXER_4_5,
+  PARAM_SECONDARY_MIXER_5_5,
+
+  PARAM_SECONDARY_MIXER_0_6,
+  PARAM_SECONDARY_MIXER_1_6,
+  PARAM_SECONDARY_MIXER_2_6,
+  PARAM_SECONDARY_MIXER_3_6,
+  PARAM_SECONDARY_MIXER_4_6,
+  PARAM_SECONDARY_MIXER_5_6,
+
+  PARAM_SECONDARY_MIXER_0_7,
+  PARAM_SECONDARY_MIXER_1_7,
+  PARAM_SECONDARY_MIXER_2_7,
+  PARAM_SECONDARY_MIXER_3_7,
+  PARAM_SECONDARY_MIXER_4_7,
+  PARAM_SECONDARY_MIXER_5_7,
+  
+  PARAM_SECONDARY_MIXER_0_8,
+  PARAM_SECONDARY_MIXER_1_8,
+  PARAM_SECONDARY_MIXER_2_8,
+  PARAM_SECONDARY_MIXER_3_8,
+  PARAM_SECONDARY_MIXER_4_8,
+  PARAM_SECONDARY_MIXER_5_8,
+                          
+  PARAM_SECONDARY_MIXER_0_9,
+  PARAM_SECONDARY_MIXER_1_9,
+  PARAM_SECONDARY_MIXER_2_9,
+  PARAM_SECONDARY_MIXER_3_9,
+  PARAM_SECONDARY_MIXER_4_9,
+  PARAM_SECONDARY_MIXER_5_9,
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
@@ -289,7 +360,8 @@ enum : uint16_t
   /***************************/
   /*** FRAME CONFIGURATION ***/
   /***************************/
-  PARAM_MIXER,
+  PARAM_PRIMARY_MIXER,
+  PARAM_SECONDARY_MIXER,
 
   PARAM_FIXED_WING,
   PARAM_ELEVATOR_REVERSE,
diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index a66fff3d..8d71db25 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -260,7 +260,11 @@ bool CommandManager::do_throttle_muxing(void)
   return override_this_channel;
 }
 
-bool CommandManager::rc_override_active() { return rc_override_; }
+bool CommandManager::rc_override_active() { return rc_throttle_override_ || rc_attitude_override_; }
+
+bool CommandManager::rc_throttle_override_active() { return rc_throttle_override_; }
+
+bool CommandManager::rc_attitude_override_active() { return rc_attitude_override_; }
 
 bool CommandManager::offboard_control_active()
 {
@@ -290,7 +294,7 @@ void CommandManager::override_combined_command_with_rc()
 
 bool CommandManager::run()
 {
-  bool last_rc_override = rc_override_;
+  bool last_rc_override = rc_override_active();
 
   // Check for and apply failsafe command
   if (RF_.state_manager_.state().failsafe) {
@@ -313,13 +317,13 @@ bool CommandManager::run()
     }
 
     // Perform muxing
-    rc_override_ = do_roll_pitch_yaw_muxing(MUX_QX);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QY);
-    rc_override_ |= do_roll_pitch_yaw_muxing(MUX_QZ);
-    rc_override_ |= do_throttle_muxing();
+    rc_attitude_override_ = do_roll_pitch_yaw_muxing(MUX_QX);
+    rc_attitude_override_ |= do_roll_pitch_yaw_muxing(MUX_QY);
+    rc_attitude_override_ |= do_roll_pitch_yaw_muxing(MUX_QZ);
+    rc_throttle_override_ = do_throttle_muxing();
 
     // Light to indicate override
-    if (rc_override_) {
+    if (rc_override_active()) {
       RF_.board_.led0_on();
     } else {
       RF_.board_.led0_off();
@@ -327,7 +331,7 @@ bool CommandManager::run()
   }
 
   // There was a change in rc_override state
-  if (last_rc_override != rc_override_) { RF_.comm_manager_.update_status(); }
+  if (last_rc_override != rc_override_active()) { RF_.comm_manager_.update_status(); }
   return true;
 }
 
diff --git a/src/controller.cpp b/src/controller.cpp
index 08111814..77ac14a0 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -264,7 +264,7 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
     // during aggressive maneuvers.
     out.Fx = command.Fx.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
 
-    if (RF_.mixer_.use_motor_parameters()) {
+    if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) {
       out.Fx *= max_thrust_;
     }
   } else {
@@ -278,7 +278,7 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
     // during aggressive maneuvers.
     out.Fy = command.Fy.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
 
-    if (RF_.mixer_.use_motor_parameters()) {
+    if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) {
       out.Fy *= max_thrust_;
     }
   } else {
@@ -295,7 +295,7 @@ Controller::Output Controller::run_pid_loops(uint32_t dt_us, const Estimator::St
     // Note that this also assumes that a high throttle means fly "up" (negative down)
     out.Fz = -command.Fz.value * RF_.params_.get_param_float(PARAM_RC_MAX_THROTTLE);
 
-    if (RF_.mixer_.use_motor_parameters()) {
+    if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) {
       out.Fz *= max_thrust_;
     }
   } else {
diff --git a/src/mixer.cpp b/src/mixer.cpp
index f3a3cdb2..f74fc2dd 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -34,6 +34,8 @@
 #include "rosflight.h"
 
 #include <cstdint>
+// We don't have a method to print the mixer currently being used for the user's benefit.
+// Keeping iostream is a temporary patch that is very useful when using the mixer.
 #include <iostream>
 
 namespace rosflight_firmware
@@ -41,8 +43,7 @@ namespace rosflight_firmware
 Mixer::Mixer(ROSflight & _rf)
     : RF_(_rf)
 {
-  mixer_to_use_ = nullptr;
-  use_motor_parameters_ = false;
+  mixer_to_use_.primary_mixer_ptr = nullptr;
 
   for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
     aux_command_.channel[i].type = NONE;
@@ -146,7 +147,78 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PRIMARY_MIXER_3_9:
     case PARAM_PRIMARY_MIXER_4_9:
     case PARAM_PRIMARY_MIXER_5_9:
-    case PARAM_MIXER:
+    case PARAM_PRIMARY_MIXER:
+
+    case PARAM_SECONDARY_MIXER_0_0:
+    case PARAM_SECONDARY_MIXER_1_0:
+    case PARAM_SECONDARY_MIXER_2_0:
+    case PARAM_SECONDARY_MIXER_3_0:
+    case PARAM_SECONDARY_MIXER_4_0:
+    case PARAM_SECONDARY_MIXER_5_0:
+
+    case PARAM_SECONDARY_MIXER_0_1:
+    case PARAM_SECONDARY_MIXER_1_1:
+    case PARAM_SECONDARY_MIXER_2_1:
+    case PARAM_SECONDARY_MIXER_3_1:
+    case PARAM_SECONDARY_MIXER_4_1:
+    case PARAM_SECONDARY_MIXER_5_1:
+
+    case PARAM_SECONDARY_MIXER_0_2:
+    case PARAM_SECONDARY_MIXER_1_2:
+    case PARAM_SECONDARY_MIXER_2_2:
+    case PARAM_SECONDARY_MIXER_3_2:
+    case PARAM_SECONDARY_MIXER_4_2:
+    case PARAM_SECONDARY_MIXER_5_2:
+
+    case PARAM_SECONDARY_MIXER_0_3:
+    case PARAM_SECONDARY_MIXER_1_3:
+    case PARAM_SECONDARY_MIXER_2_3:
+    case PARAM_SECONDARY_MIXER_3_3:
+    case PARAM_SECONDARY_MIXER_4_3:
+    case PARAM_SECONDARY_MIXER_5_3:
+
+    case PARAM_SECONDARY_MIXER_0_4:
+    case PARAM_SECONDARY_MIXER_1_4:
+    case PARAM_SECONDARY_MIXER_2_4:
+    case PARAM_SECONDARY_MIXER_3_4:
+    case PARAM_SECONDARY_MIXER_4_4:
+    case PARAM_SECONDARY_MIXER_5_4:
+
+    case PARAM_SECONDARY_MIXER_0_5:
+    case PARAM_SECONDARY_MIXER_1_5:
+    case PARAM_SECONDARY_MIXER_2_5:
+    case PARAM_SECONDARY_MIXER_3_5:
+    case PARAM_SECONDARY_MIXER_4_5:
+    case PARAM_SECONDARY_MIXER_5_5:
+
+    case PARAM_SECONDARY_MIXER_0_6:
+    case PARAM_SECONDARY_MIXER_1_6:
+    case PARAM_SECONDARY_MIXER_2_6:
+    case PARAM_SECONDARY_MIXER_3_6:
+    case PARAM_SECONDARY_MIXER_4_6:
+    case PARAM_SECONDARY_MIXER_5_6:
+
+    case PARAM_SECONDARY_MIXER_0_7:
+    case PARAM_SECONDARY_MIXER_1_7:
+    case PARAM_SECONDARY_MIXER_2_7:
+    case PARAM_SECONDARY_MIXER_3_7:
+    case PARAM_SECONDARY_MIXER_4_7:
+    case PARAM_SECONDARY_MIXER_5_7:
+
+    case PARAM_SECONDARY_MIXER_0_8:
+    case PARAM_SECONDARY_MIXER_1_8:
+    case PARAM_SECONDARY_MIXER_2_8:
+    case PARAM_SECONDARY_MIXER_3_8:
+    case PARAM_SECONDARY_MIXER_4_8:
+    case PARAM_SECONDARY_MIXER_5_8:
+
+    case PARAM_SECONDARY_MIXER_0_9:
+    case PARAM_SECONDARY_MIXER_1_9:
+    case PARAM_SECONDARY_MIXER_2_9:
+    case PARAM_SECONDARY_MIXER_3_9:
+    case PARAM_SECONDARY_MIXER_4_9:
+    case PARAM_SECONDARY_MIXER_5_9:
+    case PARAM_SECONDARY_MIXER:
       init_mixing();
       break;
     case PARAM_MOTOR_RESISTANCE:
@@ -175,75 +247,139 @@ void Mixer::init_mixing()
   // clear the invalid mixer error
   RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER);
 
-  uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_MIXER);
+  // Set up the primary mixer, used by the RC safety pilot
+  uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_PRIMARY_MIXER);
 
   if (mixer_choice >= NUM_MIXERS) {
-    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid Mixer Choice");
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid mixer choice for primary mixer");
 
     // set the invalid mixer flag
     RF_.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER);
-    mixer_to_use_ = nullptr;
+    mixer_to_use_.primary_mixer_ptr = nullptr;
   } else {
-    mixer_to_use_ = array_of_mixers_[mixer_choice];
-
-    if (mixer_to_use_ == &custom_mixing) {
-      // If specified, compute the custom mixing matrix per the motor and propeller parameters
+    if (array_of_mixers_[mixer_choice] == &custom_mixing) {
+      // Load the custom mixer for the primary mixer based off the saved parameters.
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
-                            "Loading saved custom mixer values...");
+                            "Loading saved custom mixer values for the primary mixer...");
 
       load_primary_mixer_values();
-      mixer_to_use_ = &primary_mixer_;
-
-      // Set flag since we are using custom mixer
-      use_motor_parameters_ = true;
 
       // Update the motor parameters that will be used
       update_parameters();
-    } else if (mixer_to_use_ != &fixedwing_mixing &&
-               mixer_to_use_ != &fixedwing_inverted_vtail_mixing) {
+    } else if (array_of_mixers_[mixer_choice] != &fixedwing_mixing &&
+                array_of_mixers_[mixer_choice] != &fixedwing_inverted_vtail_mixing) {
       // Don't invert the fixedwing mixers
 
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                             "Inverting selected mixing matrix...");
 
       // Otherwise, invert the selected "canned" matrix
-      primary_mixer_ = invert_mixer(mixer_to_use_);
-      mixer_to_use_ = &primary_mixer_;
+      primary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]);
+    } else {
+      primary_mixer_ = *array_of_mixers_[mixer_choice];
+    }
+
+    // Save a pointer to the primary mixer for checks later
+    mixer_to_use_.primary_mixer_ptr = &primary_mixer_;
+
+    // Load the primary mixer header to the mixer_to_use_ header. Note that both the primary and 
+    // secondary mixers will use the header for the primary mixer
+    mixer_to_use_.output_type = &primary_mixer_.output_type;
+    mixer_to_use_.default_pwm_rate = &primary_mixer_.default_pwm_rate;
+  }
+
+  // Compute or load the secondary mixer, used by the offboard control
+  mixer_choice = RF_.params_.get_param_int(PARAM_SECONDARY_MIXER);
+
+  if (mixer_choice >= NUM_MIXERS) {
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+        "Invalid mixer selected for secondary mixer, secondary mixer defaulting to primary");
+    
+    secondary_mixer_ = primary_mixer_;
+  } else if (array_of_mixers_[mixer_choice] == &custom_mixing) {
+    // Load the custom mixer for the secondary mixer based off the saved parameters.
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+                          "Loading saved custom mixer values for the secondary mixer...");
+
+    load_secondary_mixer_values();
+  } else if (array_of_mixers_[mixer_choice] != &fixedwing_mixing &&
+              array_of_mixers_[mixer_choice] != &fixedwing_inverted_vtail_mixing) {
+    // Don't invert the fixedwing mixers
+
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+                          "Inverting selected mixing matrix...");
+
+    // Otherwise, invert the selected "canned" matrix
+    secondary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]);
+  } else {
+    secondary_mixer_ = *array_of_mixers_[mixer_choice];
+  }
 
-      // Unset flag since we are not using custom mixer
-      use_motor_parameters_ = false;
+  if (mixer_to_use_.primary_mixer_ptr != nullptr) {
+    std::cout << "Primary Mixer: " << std::endl;
+    for (auto i : primary_mixer_.output_type) {
+      std::cout << i << " ";
     }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.default_pwm_rate) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Fx) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Fy) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Fz) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Qx) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Qy) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
+    for (auto i : primary_mixer_.Qz) {
+      std::cout << i << " ";
+    }
+    std::cout << std::endl;
 
-    std::cout << "Mixer: " << std::endl;
-    for (auto i : mixer_to_use_->output_type) {
+    std::cout << "Secondary Mixer: " << std::endl;
+    for (auto i : secondary_mixer_.output_type) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->default_pwm_rate) {
+    for (auto i : secondary_mixer_.default_pwm_rate) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Fx) {
+    for (auto i : secondary_mixer_.Fx) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Fy) {
+    for (auto i : secondary_mixer_.Fy) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Fz) {
+    for (auto i : secondary_mixer_.Fz) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Qx) {
+    for (auto i : secondary_mixer_.Qx) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Qy) {
+    for (auto i : secondary_mixer_.Qy) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
-    for (auto i : mixer_to_use_->Qz) {
+    for (auto i : secondary_mixer_.Qz) {
       std::cout << i << " ";
     }
     std::cout << std::endl;
@@ -336,7 +472,7 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.output_type[7] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7);
   primary_mixer_.output_type[8] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8);
   primary_mixer_.output_type[9] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9);
-
+                
   primary_mixer_.default_pwm_rate[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0);
   primary_mixer_.default_pwm_rate[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1);
   primary_mixer_.default_pwm_rate[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2);
@@ -347,7 +483,7 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.default_pwm_rate[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7);
   primary_mixer_.default_pwm_rate[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8);
   primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9);
-
+                
   // Load the mixer values from the firmware parameters
   primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_0);
   primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_0);
@@ -355,63 +491,63 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_0);
   primary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_0);
   primary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_0);
-
+                
   primary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_1);
   primary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
   primary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
   primary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_1);
   primary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_1);
   primary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_1);
-
+                
   primary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_2);
   primary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_2);
   primary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_2);
   primary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_2);
   primary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_2);
   primary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_2);
-
+                
   primary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_3);
   primary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_3);
   primary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_3);
   primary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_3);
   primary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_3);
   primary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_3);
-
+                
   primary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_4);
   primary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_4);
   primary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_4);
   primary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_4);
   primary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_4);
   primary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_4);
-
+                
   primary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_5);
   primary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_5);
   primary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_5);
   primary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_5);
   primary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_5);
   primary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_5);
-
+                
   primary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_6);
   primary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_6);
   primary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_6);
   primary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_6);
   primary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_6);
   primary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_6);
-
+                
   primary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_7);
   primary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_7);
   primary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_7);
   primary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_7);
   primary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_7);
   primary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_7);
-
+                
   primary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_8);
   primary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_8);
   primary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_8);
   primary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_8);
   primary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_8);
   primary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_8);
-
+                
   primary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_9);
   primary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_9);
   primary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_9);
@@ -420,10 +556,88 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_9);
 }
 
+void Mixer::load_secondary_mixer_values()
+{
+  // Load the mixer header values
+  // TODO: Is there a better way to do this?
+
+  // Load the mixer values from the firmware parameters
+  // The header values will be the same as the primary mixer
+  secondary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_0);
+  secondary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_0);
+  secondary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_0);
+  secondary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_0);
+  secondary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_0);
+  secondary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_0);
+                
+  secondary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_1);
+  secondary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_1);
+  secondary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_1);
+  secondary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_1);
+  secondary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_1);
+  secondary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_1);
+                
+  secondary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_2);
+  secondary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_2);
+  secondary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_2);
+  secondary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_2);
+  secondary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_2);
+  secondary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_2);
+                
+  secondary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_3);
+  secondary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_3);
+  secondary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_3);
+  secondary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_3);
+  secondary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_3);
+  secondary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_3);
+                
+  secondary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_4);
+  secondary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_4);
+  secondary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_4);
+  secondary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_4);
+  secondary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_4);
+  secondary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_4);
+                
+  secondary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_5);
+  secondary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_5);
+  secondary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_5);
+  secondary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_5);
+  secondary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_5);
+  secondary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_5);
+                
+  secondary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_6);
+  secondary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_6);
+  secondary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_6);
+  secondary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_6);
+  secondary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_6);
+  secondary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_6);
+                
+  secondary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_7);
+  secondary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_7);
+  secondary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_7);
+  secondary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_7);
+  secondary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_7);
+  secondary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_7);
+                
+  secondary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_8);
+  secondary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_8);
+  secondary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_8);
+  secondary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_8);
+  secondary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_8);
+  secondary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_8);
+                
+  secondary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_9);
+  secondary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_9);
+  secondary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_9);
+  secondary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_9);
+  secondary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_9);
+  secondary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_9);
+}
+
 void Mixer::init_PWM()
 {
-  if (mixer_to_use_ != nullptr) {
-    RF_.board_.pwm_init_multi(mixer_to_use_->default_pwm_rate, NUM_MIXER_OUTPUTS);
+  if (mixer_to_use_.primary_mixer_ptr != nullptr) {
+    RF_.board_.pwm_init_multi(primary_mixer_.default_pwm_rate, NUM_MIXER_OUTPUTS);
   } else {
     RF_.board_.pwm_init_multi(esc_calibration_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS);
   }
@@ -472,17 +686,19 @@ float Mixer::mix_multirotor_without_motor_parameters(Controller::Output commands
   float max_output = 1.0;
 
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (mixer_to_use_->output_type[i] != NONE) {
+    if ((*mixer_to_use_.output_type)[i] != NONE) {
       // Matrix multiply to mix outputs
-      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
-                    commands.Fy * mixer_to_use_->Fy[i] +
-                    commands.Fz * mixer_to_use_->Fz[i] + 
-                    commands.Qx * mixer_to_use_->Qx[i] + 
-                    commands.Qy * mixer_to_use_->Qy[i] + 
-                    commands.Qz * mixer_to_use_->Qz[i];
-
-      // Save off the largest control output if it is greater than 1.0 for future scaling
-      if (outputs_[i] > max_output) { max_output = outputs_[i]; }
+      outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                    commands.Fy * (*mixer_to_use_.Fy)[i] +
+                    commands.Fz * (*mixer_to_use_.Fz)[i] + 
+                    commands.Qx * (*mixer_to_use_.Qx)[i] + 
+                    commands.Qy * (*mixer_to_use_.Qy)[i] + 
+                    commands.Qz * (*mixer_to_use_.Qz)[i];
+
+      // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling
+      if ((*mixer_to_use_.output_type)[i] == M) {
+        if (abs(outputs_[i]) > max_output) { max_output = abs(outputs_[i]); }
+      }
     }
   }
 
@@ -495,14 +711,14 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
   float max_output = 1.0;
 
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (mixer_to_use_->output_type[i] == M) {
+    if ((*mixer_to_use_.output_type)[i] == M) {
       // Matrix multiply to mix outputs for Motor type
-      float omega_squared = commands.Fx * mixer_to_use_->Fx[i] +
-                            commands.Fy * mixer_to_use_->Fy[i] +
-                            commands.Fz * mixer_to_use_->Fz[i] +
-                            commands.Qx * mixer_to_use_->Qx[i] +
-                            commands.Qy * mixer_to_use_->Qy[i] +
-                            commands.Qz * mixer_to_use_->Qz[i];
+      float omega_squared = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                            commands.Fy * (*mixer_to_use_.Fy)[i] +
+                            commands.Fz * (*mixer_to_use_.Fz)[i] +
+                            commands.Qx * (*mixer_to_use_.Qx)[i] +
+                            commands.Qy * (*mixer_to_use_.Qy)[i] +
+                            commands.Qz * (*mixer_to_use_.Qz)[i];
       
       // Ensure that omega_squared is non-negative
       if (omega_squared < 0.0) { omega_squared = 0.0; }
@@ -512,27 +728,21 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
       float V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
-      // std::cout << "Vin: " << V_in << " " << std::endl;
-      // std::cout << "p " << rho_ << " D^5 " << pow(D_, 5.0) << " C_Q " << C_Q_ << " R " << R_ << " ";
-      // std::cout << "io " << i_0_ << " k_v " << K_V_ << " omsqrt " << sqrt(omega_squared) << std::endl;
-      // std::cout << "Vin_0 " << rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_ << " ";
-      // std::cout << "Vin_1 " << R_ * i_0_ + K_V_ * sqrt(omega_squared) << std::endl;
-
       // Convert desired V_in setting to a throttle setting
       outputs_[i] = V_in / V_max_;
 
-    } else if (mixer_to_use_->output_type[i] == S) {
+      // Save off the largest control output (for motors) if it is greater than 1.0 for future scaling
+      if (abs(outputs_[i]) > max_output) { max_output = abs(outputs_[i]); }
+
+    } else if ((*mixer_to_use_.output_type)[i] == S) {
       // Matrix multiply to mix outputs for Servo type
-      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
-                    commands.Fy * mixer_to_use_->Fy[i] +
-                    commands.Fz * mixer_to_use_->Fz[i] +
-                    commands.Qx * mixer_to_use_->Qx[i] +
-                    commands.Qy * mixer_to_use_->Qy[i] +
-                    commands.Qz * mixer_to_use_->Qz[i];
+      outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                    commands.Fy * (*mixer_to_use_.Fy)[i] +
+                    commands.Fz * (*mixer_to_use_.Fz)[i] +
+                    commands.Qx * (*mixer_to_use_.Qx)[i] +
+                    commands.Qy * (*mixer_to_use_.Qy)[i] +
+                    commands.Qz * (*mixer_to_use_.Qz)[i];
     }
-
-    // Save off the largest control output if it is greater than 1.0 for future scaling
-    if (outputs_[i] > max_output) { max_output = outputs_[i]; }
   }
 
   return max_output;
@@ -556,7 +766,7 @@ void Mixer::mix_multirotor()
       break;
   }
   
-  if (throttle_command < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
+  if (abs(throttle_command) < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)) {
     // For multirotors, disregard yaw commands if throttle is low to prevent motor spin-up while
     // arming/disarming
     commands.Qz = 0.0;
@@ -564,17 +774,18 @@ void Mixer::mix_multirotor()
 
   // Mix the outputs based on if a custom mixer (i.e. with motor parameters) is selected.
   float max_output;
-  if (use_motor_parameters_) {
+  if (RF_.params_.get_param_int(PARAM_USE_MOTOR_PARAMETERS)) {
     max_output = mix_multirotor_with_motor_parameters(commands);
   } else {
     max_output = mix_multirotor_without_motor_parameters(commands);
   }
-
-//  std::cout << "Outputs (pre scale): ";
-//  for (auto i : outputs_) {
-//    std::cout << i << " ";
-//  }
-//  std::cout << std::endl;
+  
+  // Check to see if the max_output is large. If it is, something is likely wrong with the mixer configuration, so
+  // warn the user. Note that 2 is an arbitrary value, but seems like a good upper limit since the max output should usually
+  // be between 0 and 1.
+  if (max_output > 2.0) {
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING, "Output from mixer is %f! Check mixer", max_output);
+  }
 
   // There is no relative scaling on the above equations. In other words, if the input F command is too
   // high, then it will "drown out" all other desired outputs. Therefore, we saturate motor outputs to 
@@ -585,7 +796,7 @@ void Mixer::mix_multirotor()
   // Perform Motor Output Scaling
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     // scale all motor outputs by scale factor (this is usually 1.0, unless we saturated)
-    if (mixer_to_use_->output_type[i] == M) { outputs_[i] *= scale_factor; }
+    if ((*mixer_to_use_.output_type)[i] == M) { outputs_[i] *= scale_factor; }
   }
 }
 
@@ -593,11 +804,6 @@ void Mixer::mix_fixedwing()
 {
   Controller::Output commands = RF_.controller_.output();
   
-  std::cout << "Command output/Mixer input: ";
-  std::cout << commands.Fx << " " << commands.Fy << " ";
-  std::cout << commands.Fz << " " << commands.Qx << " ";
-  std::cout << commands.Qy << " " << commands.Qz << " ";
-
   // Reverse fixed-wing channels just before mixing if we need to
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
     commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
@@ -607,19 +813,41 @@ void Mixer::mix_fixedwing()
 
   // Mix the outputs
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (mixer_to_use_->output_type[i] != NONE) {
+    if ((*mixer_to_use_.output_type)[i] != NONE) {
       // Matrix multiply to mix outputs
-      outputs_[i] = commands.Fx * mixer_to_use_->Fx[i] +
-                    commands.Qx * mixer_to_use_->Qx[i] +
-                    commands.Qy * mixer_to_use_->Qy[i] +
-                    commands.Qz * mixer_to_use_->Qz[i];
+      outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                    commands.Qx * (*mixer_to_use_.Qx)[i] +
+                    commands.Qy * (*mixer_to_use_.Qy)[i] +
+                    commands.Qz * (*mixer_to_use_.Qz)[i];
     }
   }
 }
 
 void Mixer::mix_output()
 {
-  if (mixer_to_use_ == nullptr) { return; }
+  if (mixer_to_use_.primary_mixer_ptr == nullptr) { return; }
+
+  // Check if under RC control. If RC throttle or attitude override is active,
+  // adjust the mixer_to_use_ accordingly.
+  if (RF_.command_manager_.rc_throttle_override_active()) {
+    mixer_to_use_.Fx = &primary_mixer_.Fx;
+    mixer_to_use_.Fy = &primary_mixer_.Fy;
+    mixer_to_use_.Fz = &primary_mixer_.Fz;
+  } else {
+    mixer_to_use_.Fx = &secondary_mixer_.Fx;
+    mixer_to_use_.Fy = &secondary_mixer_.Fy;
+    mixer_to_use_.Fz = &secondary_mixer_.Fz;
+  }
+
+  if (RF_.command_manager_.rc_attitude_override_active()) {
+    mixer_to_use_.Qx = &primary_mixer_.Qx;
+    mixer_to_use_.Qy = &primary_mixer_.Qy;
+    mixer_to_use_.Qz = &primary_mixer_.Qz;
+  } else {
+    mixer_to_use_.Qx = &secondary_mixer_.Qx;
+    mixer_to_use_.Qy = &secondary_mixer_.Qy;
+    mixer_to_use_.Qz = &secondary_mixer_.Qz;
+  }
 
   // Mix according to airframe type
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
@@ -633,11 +861,11 @@ void Mixer::mix_output()
   // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not
   // using
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if (mixer_to_use_->output_type[i] == NONE) {
+    if ((*mixer_to_use_.output_type)[i] == NONE) {
       outputs_[i] = aux_command_.channel[i].value;
       combined_output_type_[i] = aux_command_.channel[i].type;
     } else {
-      combined_output_type_[i] = mixer_to_use_->output_type[i];
+      combined_output_type_[i] = (*mixer_to_use_.output_type)[i];
     }
   }
 
@@ -647,13 +875,6 @@ void Mixer::mix_output()
     combined_output_type_[i] = aux_command_.channel[i].type;
   }
 
-  std::cout << "Outputs (post scale): ";
-  for (auto i : outputs_) {
-    std::cout << i << " ";
-  }
-  std::cout << std::endl;
-
-
   // Write to outputs
   for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
     float value = outputs_[i];
diff --git a/src/param.cpp b/src/param.cpp
index 3f6ace49..6f5a26e8 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -118,28 +118,29 @@ void Params::set_defaults(void)
   init_param_float(PARAM_PROP_CT, "PROP_CT", 0.075f); // Thrust coefficient of the propeller | 0 | 100.0
   init_param_float(PARAM_PROP_CQ, "PROP_CQ", 0.0045f); // Torque coefficient of the propeller | 0 | 100.0
   init_param_float(PARAM_VOLT_MAX, "VOLT_MAX", 25.0f); // Maximum voltage of the battery (V) | 0 | 100.0
-
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0, "PRIM_MIXER_OUT_0", 0); // Output type of mixer output 0. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1, "PRIM_MIXER_OUT_1", 0); // Output type of mixer output 1. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2, "PRIM_MIXER_OUT_2", 0); // Output type of mixer output 2. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3, "PRIM_MIXER_OUT_3", 0); // Output type of mixer output 3. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4, "PRIM_MIXER_OUT_4", 0); // Output type of mixer output 4. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5, "PRIM_MIXER_OUT_5", 0); // Output type of mixer output 5. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6, "PRIM_MIXER_OUT_6", 0); // Output type of mixer output 6. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7, "PRIM_MIXER_OUT_7", 0); // Output type of mixer output 7. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8, "PRIM_MIXER_OUT_8", 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3
-  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9, "PRIM_MIXER_OUT_9", 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3
-
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRIM_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRIM_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRIM_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRIM_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRIM_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRIM_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRIM_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRIM_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRIM_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRIM_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9
+  init_param_int(PARAM_USE_MOTOR_PARAMETERS, "USE_MOTOR_PARAM", false);   // Flag to use motor parameters in the mixer | 0 | 1
+
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0, "PRI_MIXER_OUT_0", 0); // Output type of mixer output 0. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1, "PRI_MIXER_OUT_1", 0); // Output type of mixer output 1. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2, "PRI_MIXER_OUT_2", 0); // Output type of mixer output 2. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_3, "PRI_MIXER_OUT_3", 0); // Output type of mixer output 3. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_4, "PRI_MIXER_OUT_4", 0); // Output type of mixer output 4. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_5, "PRI_MIXER_OUT_5", 0); // Output type of mixer output 5. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_6, "PRI_MIXER_OUT_6", 0); // Output type of mixer output 6. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_7, "PRI_MIXER_OUT_7", 0); // Output type of mixer output 7. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8, "PRI_MIXER_OUT_8", 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3
+  init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9, "PRI_MIXER_OUT_9", 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3
+
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRI_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRI_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRI_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRI_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRI_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRI_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRI_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRI_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRI_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRI_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9
 
   init_param_float(PARAM_PRIMARY_MIXER_0_0, "PRI_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0]
   init_param_float(PARAM_PRIMARY_MIXER_1_0, "PRI_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0]
@@ -211,6 +212,76 @@ void Params::set_defaults(void)
   init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
   init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
 
+  init_param_float(PARAM_SECONDARY_MIXER_0_0, "SEC_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0]
+  init_param_float(PARAM_SECONDARY_MIXER_1_0, "SEC_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0]
+  init_param_float(PARAM_SECONDARY_MIXER_2_0, "SEC_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0]
+  init_param_float(PARAM_SECONDARY_MIXER_3_0, "SEC_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0]
+  init_param_float(PARAM_SECONDARY_MIXER_4_0, "SEC_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0]
+  init_param_float(PARAM_SECONDARY_MIXER_5_0, "SEC_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_1, "SEC_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1]
+  init_param_float(PARAM_SECONDARY_MIXER_1_1, "SEC_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
+  init_param_float(PARAM_SECONDARY_MIXER_2_1, "SEC_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
+  init_param_float(PARAM_SECONDARY_MIXER_3_1, "SEC_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
+  init_param_float(PARAM_SECONDARY_MIXER_4_1, "SEC_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
+  init_param_float(PARAM_SECONDARY_MIXER_5_1, "SEC_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
+                                                                
+  init_param_float(PARAM_SECONDARY_MIXER_0_2, "SEC_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2]
+  init_param_float(PARAM_SECONDARY_MIXER_1_2, "SEC_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
+  init_param_float(PARAM_SECONDARY_MIXER_2_2, "SEC_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
+  init_param_float(PARAM_SECONDARY_MIXER_3_2, "SEC_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
+  init_param_float(PARAM_SECONDARY_MIXER_4_2, "SEC_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
+  init_param_float(PARAM_SECONDARY_MIXER_5_2, "SEC_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_3, "SEC_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3]
+  init_param_float(PARAM_SECONDARY_MIXER_1_3, "SEC_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
+  init_param_float(PARAM_SECONDARY_MIXER_2_3, "SEC_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
+  init_param_float(PARAM_SECONDARY_MIXER_3_3, "SEC_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
+  init_param_float(PARAM_SECONDARY_MIXER_4_3, "SEC_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
+  init_param_float(PARAM_SECONDARY_MIXER_5_3, "SEC_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_4, "SEC_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4]
+  init_param_float(PARAM_SECONDARY_MIXER_1_4, "SEC_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
+  init_param_float(PARAM_SECONDARY_MIXER_2_4, "SEC_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
+  init_param_float(PARAM_SECONDARY_MIXER_3_4, "SEC_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
+  init_param_float(PARAM_SECONDARY_MIXER_4_4, "SEC_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
+  init_param_float(PARAM_SECONDARY_MIXER_5_4, "SEC_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_5, "SEC_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5]
+  init_param_float(PARAM_SECONDARY_MIXER_1_5, "SEC_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
+  init_param_float(PARAM_SECONDARY_MIXER_2_5, "SEC_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
+  init_param_float(PARAM_SECONDARY_MIXER_3_5, "SEC_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
+  init_param_float(PARAM_SECONDARY_MIXER_4_5, "SEC_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
+  init_param_float(PARAM_SECONDARY_MIXER_5_5, "SEC_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_6, "SEC_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6]
+  init_param_float(PARAM_SECONDARY_MIXER_1_6, "SEC_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
+  init_param_float(PARAM_SECONDARY_MIXER_2_6, "SEC_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
+  init_param_float(PARAM_SECONDARY_MIXER_3_6, "SEC_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
+  init_param_float(PARAM_SECONDARY_MIXER_4_6, "SEC_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
+  init_param_float(PARAM_SECONDARY_MIXER_5_6, "SEC_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_7, "SEC_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7]
+  init_param_float(PARAM_SECONDARY_MIXER_1_7, "SEC_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
+  init_param_float(PARAM_SECONDARY_MIXER_2_7, "SEC_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
+  init_param_float(PARAM_SECONDARY_MIXER_3_7, "SEC_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
+  init_param_float(PARAM_SECONDARY_MIXER_4_7, "SEC_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
+  init_param_float(PARAM_SECONDARY_MIXER_5_7, "SEC_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_8, "SEC_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8]
+  init_param_float(PARAM_SECONDARY_MIXER_1_8, "SEC_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
+  init_param_float(PARAM_SECONDARY_MIXER_2_8, "SEC_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
+  init_param_float(PARAM_SECONDARY_MIXER_3_8, "SEC_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
+  init_param_float(PARAM_SECONDARY_MIXER_4_8, "SEC_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
+  init_param_float(PARAM_SECONDARY_MIXER_5_8, "SEC_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_9, "SEC_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9]
+  init_param_float(PARAM_SECONDARY_MIXER_1_9, "SEC_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
+  init_param_float(PARAM_SECONDARY_MIXER_2_9, "SEC_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
+  init_param_float(PARAM_SECONDARY_MIXER_3_9, "SEC_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
+  init_param_float(PARAM_SECONDARY_MIXER_4_9, "SEC_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
+  init_param_float(PARAM_SECONDARY_MIXER_5_9, "SEC_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
+
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
   /*****************************/
@@ -336,7 +407,8 @@ void Params::set_defaults(void)
   /***************************/
   /*** FRAME CONFIGURATION ***/
   /***************************/
-  init_param_int(PARAM_MIXER, "MIXER", Mixer::INVALID_MIXER); // Which mixer to choose - See Mixer documentation | 0 | 10
+  init_param_int(PARAM_PRIMARY_MIXER, "PRIMARY_MIXER", Mixer::INVALID_MIXER); // Which mixer to choose for primary mixer - See Mixer documentation | 0 | 11
+  init_param_int(PARAM_SECONDARY_MIXER, "SECONDARY_MIXER", Mixer::INVALID_MIXER); // Which mixer to choose for secondary mixer - See Mixer documentation | 0 | 11
 
   init_param_int(PARAM_FIXED_WING, "FIXED_WING", false); // switches on pass-through commands for fixed-wing operation | 0 | 1
   init_param_int(PARAM_ELEVATOR_REVERSE, "ELEVATOR_REV", 0); // reverses elevator servo output | 0 | 1

From be8a43a4ccdf67b937a9a36afd8ad501bb170846 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 26 Nov 2024 16:13:02 -0700
Subject: [PATCH 32/41] some updates to the unit tests

---
 test/command_manager_test.cpp | 1 +
 test/parameters_test.cpp      | 2 +-
 test/state_machine_test.cpp   | 4 ++--
 3 files changed, 4 insertions(+), 3 deletions(-)

diff --git a/test/command_manager_test.cpp b/test/command_manager_test.cpp
index c3de9f96..7ce644e7 100644
--- a/test/command_manager_test.cpp
+++ b/test/command_manager_test.cpp
@@ -55,6 +55,7 @@ class CommandManagerTest : public ::testing::Test
     for (int i = 0; i < 8; i++) { rc_values[i] = 1500; }
     rc_values[2] = 1000;
 
+    // TODO: Fix this unit test. There is no passthrough mixer
     rf.params_.set_param_int(PARAM_MIXER, Mixer::PASSTHROUGH);
     rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0);
     max_roll = rf.params_.get_param_float(PARAM_RC_MAX_ROLL);
diff --git a/test/parameters_test.cpp b/test/parameters_test.cpp
index b54be57b..a0f4a4b2 100644
--- a/test/parameters_test.cpp
+++ b/test/parameters_test.cpp
@@ -66,7 +66,7 @@ TEST(Parameters, DefaultParameters)
   EXPECT_PARAM_EQ_FLOAT(PARAM_RC_OVERRIDE_DEVIATION, 0.1f);
   EXPECT_PARAM_EQ_INT(PARAM_RC_OVERRIDE_TAKE_MIN_THROTTLE, 1);
   EXPECT_PARAM_EQ_INT(PARAM_RC_ATTITUDE_MODE, 1);
-  EXPECT_PARAM_EQ_INT(PARAM_MIXER, Mixer::INVALID_MIXER);
+  EXPECT_PARAM_EQ_INT(PARAM_PRIMARY_MIXER, Mixer::INVALID_MIXER);
   EXPECT_PARAM_EQ_INT(PARAM_FIXED_WING, 0);
   EXPECT_PARAM_EQ_INT(PARAM_AILERON_REVERSE, 0);
   EXPECT_PARAM_EQ_INT(PARAM_ELEVATOR_REVERSE, 0);
diff --git a/test/state_machine_test.cpp b/test/state_machine_test.cpp
index 235f5f87..40272292 100644
--- a/test/state_machine_test.cpp
+++ b/test/state_machine_test.cpp
@@ -25,7 +25,7 @@ class StateMachineTest : public ::testing::Test
     rf.init();
     rf.state_manager_.clear_error(
       rf.state_manager_.state().error_codes); // Clear All Errors to Start
-    rf.params_.set_param_int(PARAM_MIXER, 10);
+    rf.params_.set_param_int(PARAM_PRIMARY_MIXER, 10);
     rf.params_.set_param_int(PARAM_CALIBRATE_GYRO_ON_ARM, false); // default to turning this off
     rf.params_.set_param_float(PARAM_FAILSAFE_THROTTLE, 0.0f);
 
@@ -515,4 +515,4 @@ TEST_F(StateMachineTest, ArmAfterCorrectFailsafe)
   EXPECT_EQ(rf.state_manager_.state().armed, true);
   EXPECT_EQ(rf.state_manager_.state().error_codes, 0);
   EXPECT_EQ(rf.state_manager_.state().error, false);
-}
\ No newline at end of file
+}

From 8dfbba3528930a943282906a1fc92fcaf0117b03 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Fri, 6 Dec 2024 12:36:52 -0700
Subject: [PATCH 33/41] minor code cleanup

---
 include/controller.h    |  1 +
 include/mixer.h         |  2 +-
 src/command_manager.cpp |  2 --
 src/controller.cpp      | 10 +++++++---
 src/mixer.cpp           | 10 +++++-----
 5 files changed, 14 insertions(+), 11 deletions(-)

diff --git a/include/controller.h b/include/controller.h
index 0d5e6a99..c997c3e9 100644
--- a/include/controller.h
+++ b/include/controller.h
@@ -70,6 +70,7 @@ class Controller : public ParamListenerInterface
   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
diff --git a/include/mixer.h b/include/mixer.h
index 405de4e7..6dc17af6 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -278,7 +278,7 @@ class Mixer : public ParamListenerInterface
   float R_;                      // Motor resistance
   float rho_;                    // Air density
   float K_V_;                    // Motor back-emf constant
-  float K_Q_ = 0.01706;          // Motor torque constant
+  float K_Q_;                    // Motor torque constant
   float i_0_;                    // Motor no-load current
   float D_;                      // Propeller diameter
   float C_T_;                    // Thrust coefficient
diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index 8d71db25..663ae048 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -114,7 +114,6 @@ void CommandManager::init_failsafe()
 void CommandManager::interpret_rc(void)
 {
   // get initial, unscaled RC values
-  // TODO: Adjust this to choose the channel that the RC thottle corresponds to
   rc_command_.Qx.value = RF_.rc_.stick(RC::STICK_X);
   rc_command_.Qy.value = RF_.rc_.stick(RC::STICK_Y);
   rc_command_.Qz.value = RF_.rc_.stick(RC::STICK_Z);
@@ -307,7 +306,6 @@ bool CommandManager::run()
     if (RF_.board_.clock_millis()
         > offboard_command_.stamp_ms + RF_.params_.get_param_int(PARAM_OFFBOARD_TIMEOUT)) {
       // If it has been longer than 100 ms, then disable the offboard control
-      // TODO: Check to make sure the FX and FY commands can be set to true
       offboard_command_.Fx.active = false;
       offboard_command_.Fy.active = false;
       offboard_command_.Fz.active = false;
diff --git a/src/controller.cpp b/src/controller.cpp
index 77ac14a0..4d642017 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -101,6 +101,12 @@ void Controller::calculate_max_thrust()
   max_thrust_ = rho * pow(D, 4.0) * CT * pow(omega, 2.0) / (4 * pow(M_PI, 2.0)) * num_motors;
 }
 
+bool Controller::is_throttle_high(float threshold) {
+  return  RF_.command_manager_.combined_control().Fx.value > threshold ||
+          RF_.command_manager_.combined_control().Fy.value > threshold ||
+          RF_.command_manager_.combined_control().Fz.value > threshold;
+}
+
 void Controller::run()
 {
   // Time calculation
@@ -117,10 +123,8 @@ void Controller::run()
   prev_time_us_ = RF_.estimator_.state().timestamp_us;
 
   // Check if integrators should be updated
-  //! @todo better way to figure out if throttle is high
-  // TODO: fix this... Needs to be checked based on the throttle channel (not necessarily Fz)
   bool update_integrators = (RF_.state_manager_.state().armed)
-    && (RF_.command_manager_.combined_control().Fz.value > 0.1f) && dt_us < 10000;
+    && is_throttle_high(0.1f) && dt_us < 10000;
 
   // Run the PID loops
   Controller::Output pid_output = run_pid_loops(
diff --git a/src/mixer.cpp b/src/mixer.cpp
index f74fc2dd..89ac96d1 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -805,17 +805,17 @@ void Mixer::mix_fixedwing()
   Controller::Output commands = RF_.controller_.output();
   
   // Reverse fixed-wing channels just before mixing if we need to
-  if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {
-    commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
-    commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
-    commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
-  }
+  commands.Qx *= RF_.params_.get_param_int(PARAM_AILERON_REVERSE) ? -1 : 1;
+  commands.Qy *= RF_.params_.get_param_int(PARAM_ELEVATOR_REVERSE) ? -1 : 1;
+  commands.Qz *= RF_.params_.get_param_int(PARAM_RUDDER_REVERSE) ? -1 : 1;
 
   // Mix the outputs
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if ((*mixer_to_use_.output_type)[i] != NONE) {
       // Matrix multiply to mix outputs
       outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
+                    commands.Fy * (*mixer_to_use_.Fy)[i] +
+                    commands.Fz * (*mixer_to_use_.Fz)[i] +
                     commands.Qx * (*mixer_to_use_.Qx)[i] +
                     commands.Qy * (*mixer_to_use_.Qy)[i] +
                     commands.Qz * (*mixer_to_use_.Qz)[i];

From 935548d2dd391c74a285496c61484d1e10e7a7c9 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Wed, 11 Dec 2024 14:09:56 -0700
Subject: [PATCH 34/41] added descriptions to some parameters

---
 scripts/param_parser.py |  12 +-
 src/param.cpp           | 324 ++++++++++++++++++++--------------------
 2 files changed, 168 insertions(+), 168 deletions(-)

diff --git a/scripts/param_parser.py b/scripts/param_parser.py
index 0e6f7762..189c33cb 100755
--- a/scripts/param_parser.py
+++ b/scripts/param_parser.py
@@ -19,18 +19,18 @@
 i = 0
 for line in lines:
     # search for init_param lines
-    match = re.search("^\s*init_param", line)
+    match = re.search(r"^\s*init_param", line)
     if match != None:
-        name_with_quotes = re.search("\".*\"", line).group(0)
-        name = re.search("\w{1,16}", name_with_quotes).group(0)
+        name_with_quotes = re.search(r"\".*\"", line).group(0)
+        name = re.search(r"\w{1,16}", name_with_quotes).group(0)
         if name != 'DEFAULT':
             params.append(dict())
-            params[i]['type'] = re.split("\(", re.split("_", line)[2])[0]
+            params[i]['type'] = re.split(r"\(", re.split(r"_", line)[2])[0]
             params[i]['name'] = name
             # Find default value
-            params[i]['default'] =  re.split("\)", re.split(",", line)[2])[0]
+            params[i]['default'] =  re.split(r"\)", re.split(r",", line)[2])[0]
             # isolate the comment portion of the line and split it on the "|" characters
-            comment = re.split("\|", re.split("//", line)[1])
+            comment = re.split(r"\|", re.split(r"//", line)[1])
             # Isolate the Description
             params[i]["description"] = comment[0].strip()
             params[i]["min"] = comment[1].strip()
diff --git a/src/param.cpp b/src/param.cpp
index 6f5a26e8..c693ec22 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -110,7 +110,7 @@ void Params::set_defaults(void)
 
   init_param_float(PARAM_AIR_DENSITY, "AIR_DENSITY", 1.225f); // Density of the air (kg/m^3) | 0 | 1000.0
 
-  init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // number of vertical-facing motors on the vehicle | 1 | 8
+  init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // Number of vertical-facing motors on the vehicle | 1 | 8
   init_param_float(PARAM_MOTOR_RESISTANCE, "MOTOR_RESISTANCE", 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0
   init_param_float(PARAM_MOTOR_KV, "MOTOR_KV", 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0
   init_param_float(PARAM_NO_LOAD_CURRENT, "NO_LOAD_CURRENT", 1.5); // No-load current of the motor in amps | 0 | 1000.0
@@ -131,156 +131,156 @@ void Params::set_defaults(void)
   init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_8, "PRI_MIXER_OUT_8", 0); // Output type of mixer output 8. | 0 | 1 | 2 | 3
   init_param_int(PARAM_PRIMARY_MIXER_OUTPUT_9, "PRI_MIXER_OUT_9", 0); // Output type of mixer output 9. | 0 | 1 | 2 | 3
 
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRI_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRI_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRI_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRI_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRI_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRI_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRI_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRI_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRI_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8
-  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRI_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_0, "PRI_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0]
-  init_param_float(PARAM_PRIMARY_MIXER_1_0, "PRI_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0]
-  init_param_float(PARAM_PRIMARY_MIXER_2_0, "PRI_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0]
-  init_param_float(PARAM_PRIMARY_MIXER_3_0, "PRI_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0]
-  init_param_float(PARAM_PRIMARY_MIXER_4_0, "PRI_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0]
-  init_param_float(PARAM_PRIMARY_MIXER_5_0, "PRI_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_1, "PRI_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1]
-  init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
-  init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
-  init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
-  init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRI_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
-  init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRI_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
-                                                                
-  init_param_float(PARAM_PRIMARY_MIXER_0_2, "PRI_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2]
-  init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRI_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
-  init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRI_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
-  init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRI_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
-  init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRI_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
-  init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRI_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_3, "PRI_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3]
-  init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRI_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
-  init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRI_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
-  init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRI_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
-  init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRI_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
-  init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRI_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_4, "PRI_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4]
-  init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRI_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
-  init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRI_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
-  init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRI_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
-  init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRI_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
-  init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRI_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_5, "PRI_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5]
-  init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRI_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
-  init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRI_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
-  init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRI_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
-  init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRI_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
-  init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRI_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_6, "PRI_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6]
-  init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRI_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
-  init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRI_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
-  init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRI_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
-  init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRI_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
-  init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRI_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_7, "PRI_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7]
-  init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRI_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
-  init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRI_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
-  init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRI_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
-  init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRI_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
-  init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRI_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_8, "PRI_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8]
-  init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRI_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
-  init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRI_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
-  init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRI_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
-  init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRI_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
-  init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRI_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
-
-  init_param_float(PARAM_PRIMARY_MIXER_0_9, "PRI_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9]
-  init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRI_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
-  init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRI_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
-  init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRI_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
-  init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
-  init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_0, "SEC_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0]
-  init_param_float(PARAM_SECONDARY_MIXER_1_0, "SEC_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0]
-  init_param_float(PARAM_SECONDARY_MIXER_2_0, "SEC_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0]
-  init_param_float(PARAM_SECONDARY_MIXER_3_0, "SEC_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0]
-  init_param_float(PARAM_SECONDARY_MIXER_4_0, "SEC_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0]
-  init_param_float(PARAM_SECONDARY_MIXER_5_0, "SEC_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_1, "SEC_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1]
-  init_param_float(PARAM_SECONDARY_MIXER_1_1, "SEC_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1]
-  init_param_float(PARAM_SECONDARY_MIXER_2_1, "SEC_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1]
-  init_param_float(PARAM_SECONDARY_MIXER_3_1, "SEC_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1]
-  init_param_float(PARAM_SECONDARY_MIXER_4_1, "SEC_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1]
-  init_param_float(PARAM_SECONDARY_MIXER_5_1, "SEC_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1]
-                                                                
-  init_param_float(PARAM_SECONDARY_MIXER_0_2, "SEC_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2]
-  init_param_float(PARAM_SECONDARY_MIXER_1_2, "SEC_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2]
-  init_param_float(PARAM_SECONDARY_MIXER_2_2, "SEC_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2]
-  init_param_float(PARAM_SECONDARY_MIXER_3_2, "SEC_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2]
-  init_param_float(PARAM_SECONDARY_MIXER_4_2, "SEC_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2]
-  init_param_float(PARAM_SECONDARY_MIXER_5_2, "SEC_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_3, "SEC_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3]
-  init_param_float(PARAM_SECONDARY_MIXER_1_3, "SEC_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3]
-  init_param_float(PARAM_SECONDARY_MIXER_2_3, "SEC_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3]
-  init_param_float(PARAM_SECONDARY_MIXER_3_3, "SEC_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3]
-  init_param_float(PARAM_SECONDARY_MIXER_4_3, "SEC_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3]
-  init_param_float(PARAM_SECONDARY_MIXER_5_3, "SEC_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_4, "SEC_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4]
-  init_param_float(PARAM_SECONDARY_MIXER_1_4, "SEC_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4]
-  init_param_float(PARAM_SECONDARY_MIXER_2_4, "SEC_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4]
-  init_param_float(PARAM_SECONDARY_MIXER_3_4, "SEC_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4]
-  init_param_float(PARAM_SECONDARY_MIXER_4_4, "SEC_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4]
-  init_param_float(PARAM_SECONDARY_MIXER_5_4, "SEC_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_5, "SEC_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5]
-  init_param_float(PARAM_SECONDARY_MIXER_1_5, "SEC_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5]
-  init_param_float(PARAM_SECONDARY_MIXER_2_5, "SEC_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5]
-  init_param_float(PARAM_SECONDARY_MIXER_3_5, "SEC_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5]
-  init_param_float(PARAM_SECONDARY_MIXER_4_5, "SEC_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5]
-  init_param_float(PARAM_SECONDARY_MIXER_5_5, "SEC_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_6, "SEC_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6]
-  init_param_float(PARAM_SECONDARY_MIXER_1_6, "SEC_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6]
-  init_param_float(PARAM_SECONDARY_MIXER_2_6, "SEC_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6]
-  init_param_float(PARAM_SECONDARY_MIXER_3_6, "SEC_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6]
-  init_param_float(PARAM_SECONDARY_MIXER_4_6, "SEC_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6]
-  init_param_float(PARAM_SECONDARY_MIXER_5_6, "SEC_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_7, "SEC_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7]
-  init_param_float(PARAM_SECONDARY_MIXER_1_7, "SEC_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7]
-  init_param_float(PARAM_SECONDARY_MIXER_2_7, "SEC_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7]
-  init_param_float(PARAM_SECONDARY_MIXER_3_7, "SEC_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7]
-  init_param_float(PARAM_SECONDARY_MIXER_4_7, "SEC_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7]
-  init_param_float(PARAM_SECONDARY_MIXER_5_7, "SEC_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_8, "SEC_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8]
-  init_param_float(PARAM_SECONDARY_MIXER_1_8, "SEC_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8]
-  init_param_float(PARAM_SECONDARY_MIXER_2_8, "SEC_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8]
-  init_param_float(PARAM_SECONDARY_MIXER_3_8, "SEC_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8]
-  init_param_float(PARAM_SECONDARY_MIXER_4_8, "SEC_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8]
-  init_param_float(PARAM_SECONDARY_MIXER_5_8, "SEC_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8]
-
-  init_param_float(PARAM_SECONDARY_MIXER_0_9, "SEC_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9]
-  init_param_float(PARAM_SECONDARY_MIXER_1_9, "SEC_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9]
-  init_param_float(PARAM_SECONDARY_MIXER_2_9, "SEC_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9]
-  init_param_float(PARAM_SECONDARY_MIXER_3_9, "SEC_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9]
-  init_param_float(PARAM_SECONDARY_MIXER_4_9, "SEC_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9]
-  init_param_float(PARAM_SECONDARY_MIXER_5_9, "SEC_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9]
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_0, "PRI_MIXER_PWM_0", 0); // PWM frequenct output for mixer output 0 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_1, "PRI_MIXER_PWM_1", 0); // PWM frequenct output for mixer output 1 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_2, "PRI_MIXER_PWM_2", 0); // PWM frequenct output for mixer output 2 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_3, "PRI_MIXER_PWM_3", 0); // PWM frequenct output for mixer output 3 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_4, "PRI_MIXER_PWM_4", 0); // PWM frequenct output for mixer output 4 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_5, "PRI_MIXER_PWM_5", 0); // PWM frequenct output for mixer output 5 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_6, "PRI_MIXER_PWM_6", 0); // PWM frequenct output for mixer output 6 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7, "PRI_MIXER_PWM_7", 0); // PWM frequenct output for mixer output 7 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8, "PRI_MIXER_PWM_8", 0); // PWM frequenct output for mixer output 8 | 0 | 490
+  init_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9, "PRI_MIXER_PWM_9", 0); // PWM frequenct output for mixer output 9 | 0 | 490
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_0, "PRI_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_0, "PRI_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_0, "PRI_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_0, "PRI_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_0, "PRI_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_0, "PRI_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_1, "PRI_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_1, "PRI_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_1, "PRI_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_1, "PRI_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_1, "PRI_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_1, "PRI_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_2, "PRI_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_2, "PRI_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_2, "PRI_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_2, "PRI_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_2, "PRI_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_2, "PRI_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_3, "PRI_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_3, "PRI_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_3, "PRI_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_3, "PRI_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_3, "PRI_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_3, "PRI_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_4, "PRI_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_4, "PRI_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_4, "PRI_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_4, "PRI_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_4, "PRI_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_4, "PRI_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_5, "PRI_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_5, "PRI_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_5, "PRI_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_5, "PRI_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_5, "PRI_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_5, "PRI_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_6, "PRI_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_6, "PRI_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_6, "PRI_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_6, "PRI_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_6, "PRI_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_6, "PRI_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_7, "PRI_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_7, "PRI_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_7, "PRI_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_7, "PRI_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_7, "PRI_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_7, "PRI_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_8, "PRI_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_8, "PRI_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_8, "PRI_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_8, "PRI_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_8, "PRI_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_8, "PRI_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf
+
+  init_param_float(PARAM_PRIMARY_MIXER_0_9, "PRI_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_1_9, "PRI_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_2_9, "PRI_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_3_9, "PRI_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_4_9, "PRI_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf
+  init_param_float(PARAM_PRIMARY_MIXER_5_9, "PRI_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf
+
+  init_param_float(PARAM_SECONDARY_MIXER_0_0, "SEC_MIXER_0_0", 0.0f); // Value of the custom mixer at element [0,0] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_0, "SEC_MIXER_1_0", 0.0f); // Value of the custom mixer at element [1,0] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_0, "SEC_MIXER_2_0", 0.0f); // Value of the custom mixer at element [2,0] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_0, "SEC_MIXER_3_0", 0.0f); // Value of the custom mixer at element [3,0] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_0, "SEC_MIXER_4_0", 0.0f); // Value of the custom mixer at element [4,0] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_0, "SEC_MIXER_5_0", 0.0f); // Value of the custom mixer at element [5,0] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_1, "SEC_MIXER_0_1", 0.0f); // Value of the custom mixer at element [0,1] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_1, "SEC_MIXER_1_1", 0.0f); // Value of the custom mixer at element [1,1] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_1, "SEC_MIXER_2_1", 0.0f); // Value of the custom mixer at element [2,1] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_1, "SEC_MIXER_3_1", 0.0f); // Value of the custom mixer at element [3,1] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_1, "SEC_MIXER_4_1", 0.0f); // Value of the custom mixer at element [4,1] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_1, "SEC_MIXER_5_1", 0.0f); // Value of the custom mixer at element [5,1] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_2, "SEC_MIXER_0_2", 0.0f); // Value of the custom mixer at element [0,2] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_2, "SEC_MIXER_1_2", 0.0f); // Value of the custom mixer at element [1,2] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_2, "SEC_MIXER_2_2", 0.0f); // Value of the custom mixer at element [2,2] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_2, "SEC_MIXER_3_2", 0.0f); // Value of the custom mixer at element [3,2] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_2, "SEC_MIXER_4_2", 0.0f); // Value of the custom mixer at element [4,2] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_2, "SEC_MIXER_5_2", 0.0f); // Value of the custom mixer at element [5,2] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_3, "SEC_MIXER_0_3", 0.0f); // Value of the custom mixer at element [0,3] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_3, "SEC_MIXER_1_3", 0.0f); // Value of the custom mixer at element [1,3] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_3, "SEC_MIXER_2_3", 0.0f); // Value of the custom mixer at element [2,3] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_3, "SEC_MIXER_3_3", 0.0f); // Value of the custom mixer at element [3,3] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_3, "SEC_MIXER_4_3", 0.0f); // Value of the custom mixer at element [4,3] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_3, "SEC_MIXER_5_3", 0.0f); // Value of the custom mixer at element [5,3] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_4, "SEC_MIXER_0_4", 0.0f); // Value of the custom mixer at element [0,4] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_4, "SEC_MIXER_1_4", 0.0f); // Value of the custom mixer at element [1,4] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_4, "SEC_MIXER_2_4", 0.0f); // Value of the custom mixer at element [2,4] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_4, "SEC_MIXER_3_4", 0.0f); // Value of the custom mixer at element [3,4] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_4, "SEC_MIXER_4_4", 0.0f); // Value of the custom mixer at element [4,4] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_4, "SEC_MIXER_5_4", 0.0f); // Value of the custom mixer at element [5,4] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_5, "SEC_MIXER_0_5", 0.0f); // Value of the custom mixer at element [0,5] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_5, "SEC_MIXER_1_5", 0.0f); // Value of the custom mixer at element [1,5] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_5, "SEC_MIXER_2_5", 0.0f); // Value of the custom mixer at element [2,5] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_5, "SEC_MIXER_3_5", 0.0f); // Value of the custom mixer at element [3,5] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_5, "SEC_MIXER_4_5", 0.0f); // Value of the custom mixer at element [4,5] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_5, "SEC_MIXER_5_5", 0.0f); // Value of the custom mixer at element [5,5] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_6, "SEC_MIXER_0_6", 0.0f); // Value of the custom mixer at element [0,6] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_6, "SEC_MIXER_1_6", 0.0f); // Value of the custom mixer at element [1,6] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_6, "SEC_MIXER_2_6", 0.0f); // Value of the custom mixer at element [2,6] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_6, "SEC_MIXER_3_6", 0.0f); // Value of the custom mixer at element [3,6] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_6, "SEC_MIXER_4_6", 0.0f); // Value of the custom mixer at element [4,6] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_6, "SEC_MIXER_5_6", 0.0f); // Value of the custom mixer at element [5,6] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_7, "SEC_MIXER_0_7", 0.0f); // Value of the custom mixer at element [0,7] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_7, "SEC_MIXER_1_7", 0.0f); // Value of the custom mixer at element [1,7] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_7, "SEC_MIXER_2_7", 0.0f); // Value of the custom mixer at element [2,7] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_7, "SEC_MIXER_3_7", 0.0f); // Value of the custom mixer at element [3,7] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_7, "SEC_MIXER_4_7", 0.0f); // Value of the custom mixer at element [4,7] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_7, "SEC_MIXER_5_7", 0.0f); // Value of the custom mixer at element [5,7] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_8, "SEC_MIXER_0_8", 0.0f); // Value of the custom mixer at element [0,8] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_8, "SEC_MIXER_1_8", 0.0f); // Value of the custom mixer at element [1,8] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_8, "SEC_MIXER_2_8", 0.0f); // Value of the custom mixer at element [2,8] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_8, "SEC_MIXER_3_8", 0.0f); // Value of the custom mixer at element [3,8] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_8, "SEC_MIXER_4_8", 0.0f); // Value of the custom mixer at element [4,8] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_8, "SEC_MIXER_5_8", 0.0f); // Value of the custom mixer at element [5,8] | -inf | inf 
+                                                                                                                                 
+  init_param_float(PARAM_SECONDARY_MIXER_0_9, "SEC_MIXER_0_9", 0.0f); // Value of the custom mixer at element [0,9] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_1_9, "SEC_MIXER_1_9", 0.0f); // Value of the custom mixer at element [1,9] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_2_9, "SEC_MIXER_2_9", 0.0f); // Value of the custom mixer at element [2,9] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_3_9, "SEC_MIXER_3_9", 0.0f); // Value of the custom mixer at element [3,9] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_4_9, "SEC_MIXER_4_9", 0.0f); // Value of the custom mixer at element [4,9] | -inf | inf 
+  init_param_float(PARAM_SECONDARY_MIXER_5_9, "SEC_MIXER_5_9", 0.0f); // Value of the custom mixer at element [5,9] | -inf | inf 
 
   /*****************************/
   /*** MAVLINK CONFIGURATION ***/
@@ -290,25 +290,25 @@ void Params::set_defaults(void)
   /********************************/
   /*** CONTROLLER CONFIGURATION ***/
   /********************************/
-  init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 1.5f); // Roll Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_RATE_P, "PID_ROLL_RATE_P", 0.070f); // Roll Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_RATE_I, "PID_ROLL_RATE_I", 0.000f); // Roll Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_RATE_D, "PID_ROLL_RATE_D", 0.000f); // Roll Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_PITCH_RATE_P, "PID_PITCH_RATE_P", 1.5f);  // Pitch Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_RATE_P, "PID_PITCH_RATE_P", 0.070f);  // Pitch Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_RATE_I, "PID_PITCH_RATE_I", 0.0000f); // Pitch Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_RATE_D, "PID_PITCH_RATE_D", 0.0000f); // Pitch Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 5.5f);   // Yaw Rate Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_YAW_RATE_P, "PID_YAW_RATE_P", 0.25f);   // Yaw Rate Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_YAW_RATE_I, "PID_YAW_RATE_I", 0.0f);  // Yaw Rate Integral Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_YAW_RATE_D, "PID_YAW_RATE_D", 0.0f);  // Yaw Rate Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 1.2f);   // Roll Angle Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_ANGLE_P, "PID_ROLL_ANG_P", 0.15f);   // Roll Angle Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_ROLL_ANGLE_I, "PID_ROLL_ANG_I", 0.0f);   // Roll Angle Integral Gain | 0.0 | 1000.0
-  init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.4f);  // Roll Angle Derivative Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_ROLL_ANGLE_D, "PID_ROLL_ANG_D", 0.05f);  // Roll Angle Derivative Gain | 0.0 | 1000.0
 
-  init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 1.2f);  // Pitch Angle Proportional Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_ANGLE_P, "PID_PITCH_ANG_P", 0.15f);  // Pitch Angle Proportional Gain | 0.0 | 1000.0
   init_param_float(PARAM_PID_PITCH_ANGLE_I, "PID_PITCH_ANG_I", 0.0f);  // Pitch Angle Integral Gain | 0.0 | 1000.0
-  init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.4f); // Pitch Angle Derivative Gain | 0.0 | 1000.0
+  init_param_float(PARAM_PID_PITCH_ANGLE_D, "PID_PITCH_ANG_D", 0.05f); // Pitch Angle Derivative Gain | 0.0 | 1000.0
 
   init_param_float(PARAM_X_EQ_TORQUE, "X_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on x axis | -1.0 | 1.0
   init_param_float(PARAM_Y_EQ_TORQUE, "Y_EQ_TORQUE", 0.0f); // Equilibrium torque added to output of controller on y axis | -1.0 | 1.0
@@ -428,10 +428,10 @@ void Params::set_defaults(void)
   /*****************************/
   /*** BATTERY MONITOR SETUP ***/
   /*****************************/
-  init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f);
-  init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f);
-  init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f);
-  init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f);
+  init_param_float(PARAM_BATTERY_VOLTAGE_MULTIPLIER, "BATT_VOLT_MULT", 0.0f); // Multiplier for the voltage sensor | 0 | inf
+  init_param_float(PARAM_BATTERY_CURRENT_MULTIPLIER, "BATT_CURR_MULT", 0.0f); // Multiplier for the current sensor | 0 | inf
+  init_param_float(PARAM_BATTERY_VOLTAGE_ALPHA, "BATT_VOLT_ALPHA", 0.995f);   // Alpha value for the low pass filter on the reported battery voltage | 0 | 1
+  init_param_float(PARAM_BATTERY_CURRENT_ALPHA, "BATT_CURR_ALPHA", 0.995f);   // Alpha value for the low pass filter on the reported battery current | 0 | 1
 
   /************************/
   /*** OFFBOARD CONTROL ***/

From 9416b66f1f84f2b255474136ff3d18c8177cedde Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 16 Dec 2024 14:48:26 -0700
Subject: [PATCH 35/41] changed eigen dependency to be fetch content so it is
 more platform independent

---
 .github/workflows/varmint_firmware.yml |  2 +-
 CMakeLists.txt                         | 14 ++++++++++----
 2 files changed, 11 insertions(+), 5 deletions(-)

diff --git a/.github/workflows/varmint_firmware.yml b/.github/workflows/varmint_firmware.yml
index 9a782e3c..1f894760 100644
--- a/.github/workflows/varmint_firmware.yml
+++ b/.github/workflows/varmint_firmware.yml
@@ -12,7 +12,7 @@ jobs:
     - name: checkout submodules
       run: git submodule update --init --recursive
     - name: install toolchain
-      run: sudo apt -y install gcc-arm-none-eabi libeigen3-dev
+      run: sudo apt -y install gcc-arm-none-eabi
     - name: check toolchain
       run: arm-none-eabi-gcc --version
     - name: build varmint
diff --git a/CMakeLists.txt b/CMakeLists.txt
index c535ebb8..799163d0 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -28,10 +28,16 @@ if("${GIT_VERSION_HASH}" STREQUAL "")
   set(GIT_VERSION_HASH "0")
 endif()
 
-### source files ###
-find_package(Eigen3 REQUIRED)
-include_directories(${EIGEN3_INCLUDE_DIRS})
+### Install Eigen dependency ###
+include(FetchContent)
+FetchContent_Declare(
+  Eigen3
+  GIT_REPOSITORY https://gitlab.com/libeigen/eigen.git
+  GIT_TAG origin/3.4
+)
+FetchContent_MakeAvaliable(Eigen3)
 
+### source files ###
 include_directories(
     include
     include/interface
@@ -39,7 +45,7 @@ include_directories(
     comms/mavlink
     comms/mavlink/v1.0
     comms/mavlink/v1.0/common
-    comms/mavlink/v1.0/rosflight
+    ${eigen3_SOURCE_DIR}
 )
 
 file(GLOB_RECURSE ROSFLIGHT_SOURCES

From 0ee7dde8d9f5cc5b18f9a9c65abfa5dc981b23f9 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Mon, 16 Dec 2024 14:49:50 -0700
Subject: [PATCH 36/41] removed iostream dependency and dead function calls

---
 include/mixer.h |   2 -
 src/mixer.cpp   | 110 +++---------------------------------------------
 2 files changed, 5 insertions(+), 107 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index 6dc17af6..d884c445 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -121,8 +121,6 @@ class Mixer : public ParamListenerInterface
   aux_command_t aux_command_;
   output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS];
 
-  void write_motor(uint8_t index, float value);
-  void write_servo(uint8_t index, float value);
   void add_header_to_mixer(mixer_t* mixer);
   void load_primary_mixer_values();
   void load_secondary_mixer_values();
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 89ac96d1..7606905c 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -34,9 +34,6 @@
 #include "rosflight.h"
 
 #include <cstdint>
-// We don't have a method to print the mixer currently being used for the user's benefit.
-// Keeping iostream is a temporary patch that is very useful when using the mixer.
-#include <iostream>
 
 namespace rosflight_firmware
 {
@@ -260,7 +257,7 @@ void Mixer::init_mixing()
     if (array_of_mixers_[mixer_choice] == &custom_mixing) {
       // Load the custom mixer for the primary mixer based off the saved parameters.
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
-                            "Loading saved custom mixer values for the primary mixer...");
+                            "Loading saved custom values to primary mixer...");
 
       load_primary_mixer_values();
 
@@ -293,13 +290,15 @@ void Mixer::init_mixing()
 
   if (mixer_choice >= NUM_MIXERS) {
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
-        "Invalid mixer selected for secondary mixer, secondary mixer defaulting to primary");
+        "Invalid mixer selected for secondary mixer!");
+    RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
+        "Secondary mixer defaulting to primary!");
     
     secondary_mixer_ = primary_mixer_;
   } else if (array_of_mixers_[mixer_choice] == &custom_mixing) {
     // Load the custom mixer for the secondary mixer based off the saved parameters.
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
-                          "Loading saved custom mixer values for the secondary mixer...");
+                          "Loading saved custom values to secondary mixer");
 
     load_secondary_mixer_values();
   } else if (array_of_mixers_[mixer_choice] != &fixedwing_mixing &&
@@ -315,76 +314,6 @@ void Mixer::init_mixing()
     secondary_mixer_ = *array_of_mixers_[mixer_choice];
   }
 
-  if (mixer_to_use_.primary_mixer_ptr != nullptr) {
-    std::cout << "Primary Mixer: " << std::endl;
-    for (auto i : primary_mixer_.output_type) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.default_pwm_rate) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Fx) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Fy) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Fz) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Qx) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Qy) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : primary_mixer_.Qz) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-
-    std::cout << "Secondary Mixer: " << std::endl;
-    for (auto i : secondary_mixer_.output_type) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.default_pwm_rate) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Fx) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Fy) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Fz) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Qx) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Qy) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-    for (auto i : secondary_mixer_.Qz) {
-      std::cout << i << " ";
-    }
-    std::cout << std::endl;
-  }
-
   init_PWM();
 
   for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
@@ -643,35 +572,6 @@ void Mixer::init_PWM()
   }
 }
 
-void Mixer::write_motor(uint8_t index, float value)
-{
-  if (RF_.state_manager_.state().armed) {
-    if (value > 1.0) {
-      value = 1.0;
-    } else if (value < RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE)
-               && RF_.params_.get_param_int(PARAM_SPIN_MOTORS_WHEN_ARMED)) {
-      value = RF_.params_.get_param_float(PARAM_MOTOR_IDLE_THROTTLE);
-    } else if (value < 0.0) {
-      value = 0.0;
-    }
-  } else {
-    value = 0.0;
-  }
-  raw_outputs_[index] = value;
-  RF_.board_.pwm_write(index, raw_outputs_[index]);
-}
-
-void Mixer::write_servo(uint8_t index, float value)
-{
-  if (value > 1.0) {
-    value = 1.0;
-  } else if (value < -1.0) {
-    value = -1.0;
-  }
-  raw_outputs_[index] = value;
-  RF_.board_.pwm_write(index, raw_outputs_[index] * 0.5 + 0.5);
-}
-
 void Mixer::set_new_aux_command(aux_command_t new_aux_command)
 {
   for (uint8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {

From 4173d5913906dc2707b045c2978473a555cb2652 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 17 Dec 2024 08:31:27 -0700
Subject: [PATCH 37/41] removed air density as a parameter. Using ideal gas law

---
 include/mixer.h    | 1 -
 include/param.h    | 2 --
 include/sensors.h  | 3 +++
 src/controller.cpp | 4 +---
 src/mixer.cpp      | 6 +++---
 src/param.cpp      | 2 --
 src/sensors.cpp    | 2 ++
 7 files changed, 9 insertions(+), 11 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index d884c445..b1a53562 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -274,7 +274,6 @@ class Mixer : public ParamListenerInterface
 
   // Define parameters for the mixer
   float R_;                      // Motor resistance
-  float rho_;                    // Air density
   float K_V_;                    // Motor back-emf constant
   float K_Q_;                    // Motor torque constant
   float i_0_;                    // Motor no-load current
diff --git a/include/param.h b/include/param.h
index 1a61933b..3a082b93 100644
--- a/include/param.h
+++ b/include/param.h
@@ -62,8 +62,6 @@ enum : uint16_t
   PARAM_BAUD_RATE = 0,
   PARAM_SERIAL_DEVICE,
 
-  PARAM_AIR_DENSITY,
-
   PARAM_NUM_MOTORS,
   PARAM_MOTOR_RESISTANCE,
   PARAM_MOTOR_KV,
diff --git a/include/sensors.h b/include/sensors.h
index 6f41f1e4..526fef4c 100644
--- a/include/sensors.h
+++ b/include/sensors.h
@@ -177,6 +177,7 @@ class Sensors : public ParamListenerInterface
   Sensors(ROSflight & rosflight);
 
   inline const Data & data() const { return data_; }
+  inline float rho() { return rho_; }
   void get_filtered_IMU(turbomath::Vector & accel, turbomath::Vector & gyro, uint64_t & stamp_us);
 
   // function declarations
@@ -227,6 +228,8 @@ class Sensors : public ParamListenerInterface
   float accel_[3] = {0, 0, 0};
   float gyro_[3] = {0, 0, 0};
 
+  float rho_ = 1.225;
+
   bool calibrating_acc_flag_ = false;
   bool calibrating_gyro_flag_ = false;
   uint8_t next_sensor_to_update_ = BAROMETER;
diff --git a/src/controller.cpp b/src/controller.cpp
index 4d642017..a2f8c584 100644
--- a/src/controller.cpp
+++ b/src/controller.cpp
@@ -79,7 +79,7 @@ void Controller::calculate_max_thrust()
 {
   float R = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE);
   float D = RF_.params_.get_param_float(PARAM_PROP_DIAMETER);
-  float rho = RF_.params_.get_param_float(PARAM_AIR_DENSITY);
+  float rho = RF_.sensors_.rho();
   float CQ = RF_.params_.get_param_float(PARAM_PROP_CQ);
   float CT = RF_.params_.get_param_float(PARAM_PROP_CT);
   float KV = RF_.params_.get_param_float(PARAM_MOTOR_KV);
@@ -210,8 +210,6 @@ void Controller::param_change_callback(uint16_t param_id)
     case PARAM_PID_YAW_RATE_D:
     case PARAM_MOTOR_RESISTANCE:
     case PARAM_PROP_DIAMETER:
-    case PARAM_AIR_DENSITY:
-    case PARAM_PROP_CQ:
     case PARAM_PROP_CT:
     case PARAM_MOTOR_KV:
     case PARAM_NO_LOAD_CURRENT:
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 7606905c..459f0526 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -219,7 +219,6 @@ void Mixer::param_change_callback(uint16_t param_id)
       init_mixing();
       break;
     case PARAM_MOTOR_RESISTANCE:
-    case PARAM_AIR_DENSITY:
     case PARAM_MOTOR_KV:
     case PARAM_NO_LOAD_CURRENT:
     case PARAM_PROP_DIAMETER:
@@ -325,7 +324,6 @@ void Mixer::init_mixing()
 void Mixer::update_parameters()
 {
   R_ = RF_.params_.get_param_float(PARAM_MOTOR_RESISTANCE);
-  rho_ = RF_.params_.get_param_float(PARAM_AIR_DENSITY);
   K_V_ = RF_.params_.get_param_float(PARAM_MOTOR_KV);
   K_Q_ = K_V_;
   i_0_ = RF_.params_.get_param_float(PARAM_NO_LOAD_CURRENT);
@@ -610,6 +608,8 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
   // Mix the inputs
   float max_output = 1.0;
 
+  float rho = RF_.sensors_.rho();
+
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
     if ((*mixer_to_use_.output_type)[i] == M) {
       // Matrix multiply to mix outputs for Motor type
@@ -625,7 +625,7 @@ float Mixer::mix_multirotor_with_motor_parameters(Controller::Output commands)
 
       // Ch. 4, setting equation for torque produced by a propeller equal to Eq. 4.19
       // Note that we assume constant advance ratio, leading to constant torque and thrust constants.
-      float V_in = rho_ * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
+      float V_in = rho * pow(D_, 5.0) / (4.0 * pow(M_PI, 2.0)) * omega_squared * C_Q_ * R_ / K_Q_
         + R_ * i_0_ + K_V_ * sqrt(omega_squared);
 
       // Convert desired V_in setting to a throttle setting
diff --git a/src/param.cpp b/src/param.cpp
index c693ec22..bb80f718 100644
--- a/src/param.cpp
+++ b/src/param.cpp
@@ -108,8 +108,6 @@ void Params::set_defaults(void)
   init_param_int(PARAM_BAUD_RATE, "BAUD_RATE", 921600); // Baud rate of MAVlink communication with companion computer | 9600 | 921600
   init_param_int(PARAM_SERIAL_DEVICE, "SERIAL_DEVICE", 0); // Serial Port (for supported devices) | 0 | 3
 
-  init_param_float(PARAM_AIR_DENSITY, "AIR_DENSITY", 1.225f); // Density of the air (kg/m^3) | 0 | 1000.0
-
   init_param_int(PARAM_NUM_MOTORS, "NUM_MOTORS", 4); // Number of vertical-facing motors on the vehicle | 1 | 8
   init_param_float(PARAM_MOTOR_RESISTANCE, "MOTOR_RESISTANCE", 0.042f); // Electrical resistance of the motor windings (ohms) | 0 | 1000.0
   init_param_float(PARAM_MOTOR_KV, "MOTOR_KV", 0.01706f); // Back emf constant of the motor in SI units (V/rad/s) | 0 | 1000.0
diff --git a/src/sensors.cpp b/src/sensors.cpp
index fb491836..6dad39dd 100644
--- a/src/sensors.cpp
+++ b/src/sensors.cpp
@@ -174,6 +174,8 @@ got_flags Sensors::run()
       got.baro = true;
       rf_.board_.baro_read(&data_.baro_pressure, &data_.baro_temperature);
       correct_baro();
+
+      rho_ = (0.0289644 * data_.baro_pressure) / (8.31432 * data_.baro_temperature);
     }
   }
 

From ff543828bb607ea4e1f729c666df8e115204a063 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 17 Dec 2024 09:19:19 -0700
Subject: [PATCH 38/41] renamed NONE to AUX since it is only used for AUX
 channels

---
 include/mixer.h      | 28 ++++++++++++++--------------
 src/comm_manager.cpp |  2 +-
 src/mixer.cpp        |  8 ++++----
 3 files changed, 19 insertions(+), 19 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index b1a53562..bd253bc0 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -51,7 +51,7 @@ class Mixer : public ParamListenerInterface
   static constexpr uint8_t NUM_TOTAL_OUTPUTS = 14;
   static constexpr uint8_t NUM_MIXER_OUTPUTS = 10;
 
-  enum
+  typedef enum
   {
     ESC_CALIBRATION = 0,
     QUADCOPTER_PLUS = 1,
@@ -67,11 +67,11 @@ class Mixer : public ParamListenerInterface
     CUSTOM = 11,
     NUM_MIXERS,
     INVALID_MIXER = 255
-  };
+  } mixer_type_t;
 
   typedef enum
   {
-    NONE, // None
+    AUX, // None
     S,    // Servo
     M,    // Motor
     G     // GPIO
@@ -141,7 +141,7 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix
 
   const mixer_t quadcopter_plus_mixing = {
-    {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE},            // output type
+    {M, M, M, M, AUX, AUX, AUX, AUX, AUX, AUX},            // output type
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},  // F_y Mix
@@ -151,7 +151,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}}; // Q_z Mix 
 
   const mixer_t quadcopter_x_mixing = {
-    {M, M, M, M, NONE, NONE, NONE, NONE, NONE, NONE},             // output type
+    {M, M, M, M, AUX, AUX, AUX, AUX, AUX, AUX},             // output type
     {490, 490, 490, 490, 50, 50, 50, 50, 50, 50},                 // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0, 0, 0},   // F_y Mix
@@ -161,7 +161,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0, 0, 0}};  // Q_z Mix
 
   const mixer_t hex_plus_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {M, M, M, M, M, M, AUX, AUX, AUX, AUX},                                // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
@@ -171,7 +171,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t hex_x_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {M, M, M, M, M, M, AUX, AUX, AUX, AUX},                                // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
@@ -181,7 +181,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t octocopter_plus_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {M, M, M, M, M, M, M, M, AUX, AUX},                                                    // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
@@ -191,7 +191,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t octocopter_x_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {M, M, M, M, M, M, M, M, AUX, AUX},                                                    // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
@@ -201,7 +201,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t Y6_mixing = {
-    {M, M, M, M, M, M, NONE, NONE, NONE, NONE},                                // output type
+    {M, M, M, M, M, M, AUX, AUX, AUX, AUX},                                // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                          // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0, 0, 0},  // F_y Mix
@@ -211,7 +211,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0, 0, 0}}; // Q_z Mix
 
   const mixer_t X8_mixing = {
-    {M, M, M, M, M, M, M, M, NONE, NONE},                                                    // output type
+    {M, M, M, M, M, M, M, M, AUX, AUX},                                                    // output type
     {490, 490, 490, 490, 490, 490, 490, 490, 50, 50},                                        // Rate (Hz)
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_x Mix
     { 0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f,  0.0000f, 0, 0},  // F_y Mix
@@ -221,7 +221,7 @@ class Mixer : public ParamListenerInterface
     { 1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f,  1.0000f, -1.0000f, 0, 0}}; // Q_z Mix
 
   const mixer_t fixedwing_mixing = {
-    {   S,    S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // output type
+    {   S,    S,    S, AUX,    M, AUX, AUX, AUX, AUX, AUX},  // output type
     { 50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
     {0.0f, 0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
@@ -231,7 +231,7 @@ class Mixer : public ParamListenerInterface
     {0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t fixedwing_inverted_vtail_mixing = {
-    {   S,     S,    S, NONE,    M, NONE, NONE, NONE, NONE, NONE},  // Ailerons, LRuddervator, RRuddervator, Motor
+    {   S,     S,    S, AUX,    M, AUX, AUX, AUX, AUX, AUX},  // Ailerons, LRuddervator, RRuddervator, Motor
     {  50,    50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz)
     {0.0f,  0.0f, 0.0f, 0.0f, 1.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix 
     {0.0f,  0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
@@ -241,7 +241,7 @@ class Mixer : public ParamListenerInterface
     {0.0f,  0.5f, 0.5f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f}}; // Q_z Mix 
 
   const mixer_t custom_mixing = {
-    {NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE, NONE},  // output type
+    {AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX, AUX},  // output type
     {  50,   50,   50,   50,   50,   50,   50,   50,   50,   50},  // Rate (Hz or kHz)
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_x Mix
     {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f},  // F_y Mix
diff --git a/src/comm_manager.cpp b/src/comm_manager.cpp
index 0f81ebb4..aad134f6 100644
--- a/src/comm_manager.cpp
+++ b/src/comm_manager.cpp
@@ -293,7 +293,7 @@ void CommManager::aux_command_callback(const CommLinkInterface::AuxCommand & com
     switch (command.cmd_array[i].type) {
       case CommLinkInterface::AuxCommand::Type::DISABLED:
         // Channel is either not used or is controlled by the mixer
-        new_aux_command.channel[i].type = Mixer::NONE;
+        new_aux_command.channel[i].type = Mixer::AUX;
         new_aux_command.channel[i].value = 0;
         break;
       case CommLinkInterface::AuxCommand::Type::SERVO:
diff --git a/src/mixer.cpp b/src/mixer.cpp
index 459f0526..a274eef0 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -43,7 +43,7 @@ Mixer::Mixer(ROSflight & _rf)
   mixer_to_use_.primary_mixer_ptr = nullptr;
 
   for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
-    aux_command_.channel[i].type = NONE;
+    aux_command_.channel[i].type = AUX;
     aux_command_.channel[i].value = 0.0f;
   }
 }
@@ -584,7 +584,7 @@ float Mixer::mix_multirotor_without_motor_parameters(Controller::Output commands
   float max_output = 1.0;
 
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if ((*mixer_to_use_.output_type)[i] != NONE) {
+    if ((*mixer_to_use_.output_type)[i] != AUX) {
       // Matrix multiply to mix outputs
       outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
                     commands.Fy * (*mixer_to_use_.Fy)[i] +
@@ -711,7 +711,7 @@ void Mixer::mix_fixedwing()
 
   // Mix the outputs
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if ((*mixer_to_use_.output_type)[i] != NONE) {
+    if ((*mixer_to_use_.output_type)[i] != AUX) {
       // Matrix multiply to mix outputs
       outputs_[i] = commands.Fx * (*mixer_to_use_.Fx)[i] +
                     commands.Fy * (*mixer_to_use_.Fy)[i] +
@@ -761,7 +761,7 @@ void Mixer::mix_output()
   // For the first NUM_MIXER_OUTPUTS channels, only write aux_command to channels the mixer is not
   // using
   for (uint8_t i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    if ((*mixer_to_use_.output_type)[i] == NONE) {
+    if ((*mixer_to_use_.output_type)[i] == AUX) {
       outputs_[i] = aux_command_.channel[i].value;
       combined_output_type_[i] = aux_command_.channel[i].type;
     } else {

From e0eee6340d37ba2b3e5e770ecbfb0089934207ab Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 17 Dec 2024 09:33:51 -0700
Subject: [PATCH 39/41] added error checking for RC F axis selection

---
 src/command_manager.cpp | 19 ++++++++++++++++++-
 src/mixer.cpp           |  5 +++++
 2 files changed, 23 insertions(+), 1 deletion(-)

diff --git a/src/command_manager.cpp b/src/command_manager.cpp
index 663ae048..ed1bc4ff 100644
--- a/src/command_manager.cpp
+++ b/src/command_manager.cpp
@@ -99,7 +99,12 @@ void CommandManager::init_failsafe()
     case Y_AXIS:
       multirotor_failsafe_command_.Fy.value = failsafe_thr_param;
       break;
+    case Z_AXIS:
+      multirotor_failsafe_command_.Fz.value = failsafe_thr_param;
+      break;
     default:
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
+          "Invalid RC F axis. Defaulting to z-axis.");
       multirotor_failsafe_command_.Fz.value = failsafe_thr_param;
       break;
   }
@@ -130,7 +135,14 @@ void CommandManager::interpret_rc(void)
       rc_command_.Fy.value = RF_.rc_.stick(RC::STICK_F);
       rc_command_.Fz.value = 0.0;
       break;
-    default:   // RC F = Z axis
+    case Z_AXIS:
+      rc_command_.Fx.value = 0.0;
+      rc_command_.Fy.value = 0.0;
+      rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
+      break;
+    default:
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
+          "Invalid RC F axis. Defaulting to z-axis.");
       rc_command_.Fx.value = 0.0;
       rc_command_.Fy.value = 0.0;
       rc_command_.Fz.value = RF_.rc_.stick(RC::STICK_F);
@@ -232,7 +244,12 @@ bool CommandManager::do_throttle_muxing(void)
     case Y_AXIS: 
       selected_channel = MUX_FY;
       break;
+    case Z_AXIS:
+      selected_channel = MUX_FZ;
+      break;
     default:
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
+          "Invalid RC F axis. Defaulting to z-axis.");
       selected_channel = MUX_FZ;
       break;
   }
diff --git a/src/mixer.cpp b/src/mixer.cpp
index a274eef0..ec954c30 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -661,7 +661,12 @@ void Mixer::mix_multirotor()
     case Y_AXIS:
       throttle_command = commands.Fy;
       break;
+    case Z_AXIS:
+      throttle_command = commands.Fz;
+      break;
     default:
+      RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_WARNING,
+          "Invalid RC F axis. Defaulting to z-axis.");
       throttle_command = commands.Fz;
       break;
   }

From 1f01b7350c5dbba590274a79650be2ad75c8db3a Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 17 Dec 2024 12:16:00 -0700
Subject: [PATCH 40/41] reworked the way mixers were assigned to use the enums
 instead of the array of pointers

---
 include/mixer.h |  34 ++++++-------
 src/mixer.cpp   | 131 +++++++++++++++++++++++++-----------------------
 2 files changed, 85 insertions(+), 80 deletions(-)

diff --git a/include/mixer.h b/include/mixer.h
index bd253bc0..3c3e1a98 100644
--- a/include/mixer.h
+++ b/include/mixer.h
@@ -91,7 +91,6 @@ class Mixer : public ParamListenerInterface
 
   typedef struct 
   {
-    mixer_t* primary_mixer_ptr;
     output_type_t (*output_type)[NUM_MIXER_OUTPUTS];
     float (*default_pwm_rate)[NUM_MIXER_OUTPUTS];
     float (*Fx)[NUM_MIXER_OUTPUTS];
@@ -121,12 +120,12 @@ class Mixer : public ParamListenerInterface
   aux_command_t aux_command_;
   output_type_t combined_output_type_[NUM_TOTAL_OUTPUTS];
 
-  void add_header_to_mixer(mixer_t* mixer);
   void load_primary_mixer_values();
   void load_secondary_mixer_values();
-  mixer_t invert_mixer(const mixer_t* mixer_to_invert);
+  mixer_t invert_mixer(const mixer_t mixer_to_invert);
   float mix_multirotor_with_motor_parameters(Controller::Output commands);
   float mix_multirotor_without_motor_parameters(Controller::Output commands);
+  void select_primary_or_secondary_mixer();
 
   // clang-format off
 
@@ -254,20 +253,21 @@ class Mixer : public ParamListenerInterface
   mixer_t secondary_mixer_;
 
   mixer_selection_t mixer_to_use_;
-
-  const mixer_t* array_of_mixers_[NUM_MIXERS] = {
-    &esc_calibration_mixing,
-    &quadcopter_plus_mixing,
-    &quadcopter_x_mixing,
-    &hex_plus_mixing,
-    &hex_x_mixing,
-    &octocopter_plus_mixing,
-    &octocopter_x_mixing,
-    &Y6_mixing,
-    &X8_mixing,
-    &fixedwing_mixing,
-    &fixedwing_inverted_vtail_mixing,
-    &custom_mixing,
+  bool primary_mixer_is_selected_ = false;
+
+  const mixer_t array_of_mixers_[NUM_MIXERS] = {
+    esc_calibration_mixing,
+    quadcopter_plus_mixing,
+    quadcopter_x_mixing,
+    hex_plus_mixing,
+    hex_x_mixing,
+    octocopter_plus_mixing,
+    octocopter_x_mixing,
+    Y6_mixing,
+    X8_mixing,
+    fixedwing_mixing,
+    fixedwing_inverted_vtail_mixing,
+    custom_mixing,
   };
 
   // clang-format on
diff --git a/src/mixer.cpp b/src/mixer.cpp
index ec954c30..9ac4e6d5 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -40,8 +40,6 @@ namespace rosflight_firmware
 Mixer::Mixer(ROSflight & _rf)
     : RF_(_rf)
 {
-  mixer_to_use_.primary_mixer_ptr = nullptr;
-
   for (int8_t i = 0; i < NUM_TOTAL_OUTPUTS; i++) {
     aux_command_.channel[i].type = AUX;
     aux_command_.channel[i].value = 0.0f;
@@ -63,7 +61,6 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PRIMARY_MIXER_OUTPUT_7:
     case PARAM_PRIMARY_MIXER_OUTPUT_8:
     case PARAM_PRIMARY_MIXER_OUTPUT_9:
-
     case PARAM_PRIMARY_MIXER_PWM_RATE_0:
     case PARAM_PRIMARY_MIXER_PWM_RATE_1:
     case PARAM_PRIMARY_MIXER_PWM_RATE_2:
@@ -74,77 +71,66 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_PRIMARY_MIXER_PWM_RATE_7:
     case PARAM_PRIMARY_MIXER_PWM_RATE_8:
     case PARAM_PRIMARY_MIXER_PWM_RATE_9:
-
     case PARAM_PRIMARY_MIXER_0_0:
     case PARAM_PRIMARY_MIXER_1_0:
     case PARAM_PRIMARY_MIXER_2_0:
     case PARAM_PRIMARY_MIXER_3_0:
     case PARAM_PRIMARY_MIXER_4_0:
     case PARAM_PRIMARY_MIXER_5_0:
-
     case PARAM_PRIMARY_MIXER_0_1:
     case PARAM_PRIMARY_MIXER_1_1:
     case PARAM_PRIMARY_MIXER_2_1:
     case PARAM_PRIMARY_MIXER_3_1:
     case PARAM_PRIMARY_MIXER_4_1:
     case PARAM_PRIMARY_MIXER_5_1:
-
     case PARAM_PRIMARY_MIXER_0_2:
     case PARAM_PRIMARY_MIXER_1_2:
     case PARAM_PRIMARY_MIXER_2_2:
     case PARAM_PRIMARY_MIXER_3_2:
     case PARAM_PRIMARY_MIXER_4_2:
     case PARAM_PRIMARY_MIXER_5_2:
-
     case PARAM_PRIMARY_MIXER_0_3:
     case PARAM_PRIMARY_MIXER_1_3:
     case PARAM_PRIMARY_MIXER_2_3:
     case PARAM_PRIMARY_MIXER_3_3:
     case PARAM_PRIMARY_MIXER_4_3:
     case PARAM_PRIMARY_MIXER_5_3:
-
     case PARAM_PRIMARY_MIXER_0_4:
     case PARAM_PRIMARY_MIXER_1_4:
     case PARAM_PRIMARY_MIXER_2_4:
     case PARAM_PRIMARY_MIXER_3_4:
     case PARAM_PRIMARY_MIXER_4_4:
     case PARAM_PRIMARY_MIXER_5_4:
-
     case PARAM_PRIMARY_MIXER_0_5:
     case PARAM_PRIMARY_MIXER_1_5:
     case PARAM_PRIMARY_MIXER_2_5:
     case PARAM_PRIMARY_MIXER_3_5:
     case PARAM_PRIMARY_MIXER_4_5:
     case PARAM_PRIMARY_MIXER_5_5:
-
     case PARAM_PRIMARY_MIXER_0_6:
     case PARAM_PRIMARY_MIXER_1_6:
     case PARAM_PRIMARY_MIXER_2_6:
     case PARAM_PRIMARY_MIXER_3_6:
     case PARAM_PRIMARY_MIXER_4_6:
     case PARAM_PRIMARY_MIXER_5_6:
-
     case PARAM_PRIMARY_MIXER_0_7:
     case PARAM_PRIMARY_MIXER_1_7:
     case PARAM_PRIMARY_MIXER_2_7:
     case PARAM_PRIMARY_MIXER_3_7:
     case PARAM_PRIMARY_MIXER_4_7:
     case PARAM_PRIMARY_MIXER_5_7:
-
     case PARAM_PRIMARY_MIXER_0_8:
     case PARAM_PRIMARY_MIXER_1_8:
     case PARAM_PRIMARY_MIXER_2_8:
     case PARAM_PRIMARY_MIXER_3_8:
     case PARAM_PRIMARY_MIXER_4_8:
     case PARAM_PRIMARY_MIXER_5_8:
-
     case PARAM_PRIMARY_MIXER_0_9:
     case PARAM_PRIMARY_MIXER_1_9:
     case PARAM_PRIMARY_MIXER_2_9:
     case PARAM_PRIMARY_MIXER_3_9:
     case PARAM_PRIMARY_MIXER_4_9:
     case PARAM_PRIMARY_MIXER_5_9:
-    case PARAM_PRIMARY_MIXER:
 
     case PARAM_SECONDARY_MIXER_0_0:
     case PARAM_SECONDARY_MIXER_1_0:
@@ -152,69 +138,62 @@ void Mixer::param_change_callback(uint16_t param_id)
     case PARAM_SECONDARY_MIXER_3_0:
     case PARAM_SECONDARY_MIXER_4_0:
     case PARAM_SECONDARY_MIXER_5_0:
-
     case PARAM_SECONDARY_MIXER_0_1:
     case PARAM_SECONDARY_MIXER_1_1:
     case PARAM_SECONDARY_MIXER_2_1:
     case PARAM_SECONDARY_MIXER_3_1:
     case PARAM_SECONDARY_MIXER_4_1:
     case PARAM_SECONDARY_MIXER_5_1:
-
     case PARAM_SECONDARY_MIXER_0_2:
     case PARAM_SECONDARY_MIXER_1_2:
     case PARAM_SECONDARY_MIXER_2_2:
     case PARAM_SECONDARY_MIXER_3_2:
     case PARAM_SECONDARY_MIXER_4_2:
     case PARAM_SECONDARY_MIXER_5_2:
-
     case PARAM_SECONDARY_MIXER_0_3:
     case PARAM_SECONDARY_MIXER_1_3:
     case PARAM_SECONDARY_MIXER_2_3:
     case PARAM_SECONDARY_MIXER_3_3:
     case PARAM_SECONDARY_MIXER_4_3:
     case PARAM_SECONDARY_MIXER_5_3:
-
     case PARAM_SECONDARY_MIXER_0_4:
     case PARAM_SECONDARY_MIXER_1_4:
     case PARAM_SECONDARY_MIXER_2_4:
     case PARAM_SECONDARY_MIXER_3_4:
     case PARAM_SECONDARY_MIXER_4_4:
     case PARAM_SECONDARY_MIXER_5_4:
-
     case PARAM_SECONDARY_MIXER_0_5:
     case PARAM_SECONDARY_MIXER_1_5:
     case PARAM_SECONDARY_MIXER_2_5:
     case PARAM_SECONDARY_MIXER_3_5:
     case PARAM_SECONDARY_MIXER_4_5:
     case PARAM_SECONDARY_MIXER_5_5:
-
     case PARAM_SECONDARY_MIXER_0_6:
     case PARAM_SECONDARY_MIXER_1_6:
     case PARAM_SECONDARY_MIXER_2_6:
     case PARAM_SECONDARY_MIXER_3_6:
     case PARAM_SECONDARY_MIXER_4_6:
     case PARAM_SECONDARY_MIXER_5_6:
-
     case PARAM_SECONDARY_MIXER_0_7:
     case PARAM_SECONDARY_MIXER_1_7:
     case PARAM_SECONDARY_MIXER_2_7:
     case PARAM_SECONDARY_MIXER_3_7:
     case PARAM_SECONDARY_MIXER_4_7:
     case PARAM_SECONDARY_MIXER_5_7:
-
     case PARAM_SECONDARY_MIXER_0_8:
     case PARAM_SECONDARY_MIXER_1_8:
     case PARAM_SECONDARY_MIXER_2_8:
     case PARAM_SECONDARY_MIXER_3_8:
     case PARAM_SECONDARY_MIXER_4_8:
     case PARAM_SECONDARY_MIXER_5_8:
-
     case PARAM_SECONDARY_MIXER_0_9:
     case PARAM_SECONDARY_MIXER_1_9:
     case PARAM_SECONDARY_MIXER_2_9:
     case PARAM_SECONDARY_MIXER_3_9:
     case PARAM_SECONDARY_MIXER_4_9:
     case PARAM_SECONDARY_MIXER_5_9:
+
+    case PARAM_PRIMARY_MIXER:
     case PARAM_SECONDARY_MIXER:
       init_mixing();
       break;
@@ -244,16 +223,16 @@ void Mixer::init_mixing()
   RF_.state_manager_.clear_error(StateManager::ERROR_INVALID_MIXER);
 
   // Set up the primary mixer, used by the RC safety pilot
-  uint8_t mixer_choice = RF_.params_.get_param_int(PARAM_PRIMARY_MIXER);
+  mixer_type_t mixer_choice = static_cast<mixer_type_t>(RF_.params_.get_param_int(PARAM_PRIMARY_MIXER));
 
   if (mixer_choice >= NUM_MIXERS) {
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_ERROR, "Invalid mixer choice for primary mixer");
 
     // set the invalid mixer flag
     RF_.state_manager_.set_error(StateManager::ERROR_INVALID_MIXER);
-    mixer_to_use_.primary_mixer_ptr = nullptr;
+    primary_mixer_is_selected_ = false;
   } else {
-    if (array_of_mixers_[mixer_choice] == &custom_mixing) {
+    if (mixer_choice == CUSTOM) {
       // Load the custom mixer for the primary mixer based off the saved parameters.
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                             "Loading saved custom values to primary mixer...");
@@ -262,30 +241,35 @@ void Mixer::init_mixing()
 
       // Update the motor parameters that will be used
       update_parameters();
-    } else if (array_of_mixers_[mixer_choice] != &fixedwing_mixing &&
-                array_of_mixers_[mixer_choice] != &fixedwing_inverted_vtail_mixing) {
-      // Don't invert the fixedwing mixers
-
+    } else if (mixer_choice != FIXEDWING ||
+               mixer_choice != INVERTED_VTAIL) {
+      // Invert the selected "canned" matrix
       RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                             "Inverting selected mixing matrix...");
-
-      // Otherwise, invert the selected "canned" matrix
       primary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]);
     } else {
-      primary_mixer_ = *array_of_mixers_[mixer_choice];
+      // Don't invert the fixedwing mixers
+      primary_mixer_ = array_of_mixers_[mixer_choice];
     }
 
-    // Save a pointer to the primary mixer for checks later
-    mixer_to_use_.primary_mixer_ptr = &primary_mixer_;
-
     // Load the primary mixer header to the mixer_to_use_ header. Note that both the primary and 
     // secondary mixers will use the header for the primary mixer
     mixer_to_use_.output_type = &primary_mixer_.output_type;
     mixer_to_use_.default_pwm_rate = &primary_mixer_.default_pwm_rate;
+
+    // Load the primary mixing values into the mixer_to_use_ by default
+    mixer_to_use_.Fx = &primary_mixer_.Fx;
+    mixer_to_use_.Fy = &primary_mixer_.Fy;
+    mixer_to_use_.Fz = &primary_mixer_.Fz;
+    mixer_to_use_.Qx = &primary_mixer_.Qx;
+    mixer_to_use_.Qx = &primary_mixer_.Qx;
+    mixer_to_use_.Qx = &primary_mixer_.Qx;
+
+    primary_mixer_is_selected_ = true;
   }
 
   // Compute or load the secondary mixer, used by the offboard control
-  mixer_choice = RF_.params_.get_param_int(PARAM_SECONDARY_MIXER);
+  mixer_choice = static_cast<mixer_type_t>(RF_.params_.get_param_int(PARAM_SECONDARY_MIXER));
 
   if (mixer_choice >= NUM_MIXERS) {
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
@@ -294,23 +278,22 @@ void Mixer::init_mixing()
         "Secondary mixer defaulting to primary!");
     
     secondary_mixer_ = primary_mixer_;
-  } else if (array_of_mixers_[mixer_choice] == &custom_mixing) {
+  } else if (mixer_choice == CUSTOM) {
     // Load the custom mixer for the secondary mixer based off the saved parameters.
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                           "Loading saved custom values to secondary mixer");
 
     load_secondary_mixer_values();
-  } else if (array_of_mixers_[mixer_choice] != &fixedwing_mixing &&
-              array_of_mixers_[mixer_choice] != &fixedwing_inverted_vtail_mixing) {
-    // Don't invert the fixedwing mixers
-
+  } else if (mixer_choice != FIXEDWING ||
+             mixer_choice != INVERTED_VTAIL) {
+    // Invert the selected "canned" matrix
     RF_.comm_manager_.log(CommLinkInterface::LogSeverity::LOG_INFO,
                           "Inverting selected mixing matrix...");
 
-    // Otherwise, invert the selected "canned" matrix
     secondary_mixer_ = invert_mixer(array_of_mixers_[mixer_choice]);
   } else {
-    secondary_mixer_ = *array_of_mixers_[mixer_choice];
+    // Don't invert the fixedwing mixers
+    secondary_mixer_ = array_of_mixers_[mixer_choice];
   }
 
   init_PWM();
@@ -334,19 +317,19 @@ void Mixer::update_parameters()
   V_max_ = RF_.params_.get_param_float(PARAM_VOLT_MAX);
 }
 
-Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
+Mixer::mixer_t Mixer::invert_mixer(const mixer_t mixer_to_invert)
 {
   Eigen::Matrix<float, 6, NUM_MIXER_OUTPUTS> mixer_matrix;
   mixer_matrix.setZero();
 
   // Convert the mixer_t to an Eigen matrix
   for (int i=0; i<NUM_MIXER_OUTPUTS; i++) {
-    mixer_matrix(0, i) = mixer_to_invert->Fx[i];
-    mixer_matrix(1, i) = mixer_to_invert->Fy[i];
-    mixer_matrix(2, i) = mixer_to_invert->Fz[i];
-    mixer_matrix(3, i) = mixer_to_invert->Qx[i];
-    mixer_matrix(4, i) = mixer_to_invert->Qy[i];
-    mixer_matrix(5, i) = mixer_to_invert->Qz[i];
+    mixer_matrix(0, i) = mixer_to_invert.Fx[i];
+    mixer_matrix(1, i) = mixer_to_invert.Fy[i];
+    mixer_matrix(2, i) = mixer_to_invert.Fz[i];
+    mixer_matrix(3, i) = mixer_to_invert.Qx[i];
+    mixer_matrix(4, i) = mixer_to_invert.Qy[i];
+    mixer_matrix(5, i) = mixer_to_invert.Qz[i];
   }
 
   // Calculate the pseudoinverse of the mixing matrix using the SVD
@@ -372,8 +355,8 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
   mixer_t inverted_mixer;
 
   for (int i = 0; i < NUM_MIXER_OUTPUTS; i++) {
-    inverted_mixer.output_type[i] = mixer_to_invert->output_type[i];
-    inverted_mixer.default_pwm_rate[i] = mixer_to_invert->default_pwm_rate[i];
+    inverted_mixer.output_type[i] = mixer_to_invert.output_type[i];
+    inverted_mixer.default_pwm_rate[i] = mixer_to_invert.default_pwm_rate[i];
     inverted_mixer.Fx[i] = mixer_matrix_pinv(i, 0);
     inverted_mixer.Fy[i] = mixer_matrix_pinv(i, 1);
     inverted_mixer.Fz[i] = mixer_matrix_pinv(i, 2);
@@ -388,7 +371,6 @@ Mixer::mixer_t Mixer::invert_mixer(const mixer_t* mixer_to_invert)
 void Mixer::load_primary_mixer_values()
 {
   // Load the mixer header values
-  // TODO: Is there a better way to do this?
   primary_mixer_.output_type[0] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_0);
   primary_mixer_.output_type[1] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_1);
   primary_mixer_.output_type[2] = (output_type_t) RF_.params_.get_param_int(PARAM_PRIMARY_MIXER_OUTPUT_2);
@@ -410,8 +392,18 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.default_pwm_rate[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_7);
   primary_mixer_.default_pwm_rate[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_8);
   primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9);
-                
+
   // Load the mixer values from the firmware parameters
+  // for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
+  //   // This assumes the parameters are stored in order in the param enum
+  //   int param_index = (int) PARAM_PRIMARY_MIXER_0_0 + 6 * i;
+  //   primary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
+  //   primary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
+  //   primary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
+  //   primary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
+  //   primary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
+  //   primary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
+  // }
   primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_0);
   primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_0);
   primary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_0);
@@ -485,11 +477,18 @@ void Mixer::load_primary_mixer_values()
 
 void Mixer::load_secondary_mixer_values()
 {
-  // Load the mixer header values
-  // TODO: Is there a better way to do this?
-
   // Load the mixer values from the firmware parameters
   // The header values will be the same as the primary mixer
+  // for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
+  //   // This assumes the parameters are stored in order in the param enum
+  //   int param_index = (int) PARAM_SECONDARY_MIXER_0_0 + 6 * i;
+  //   secondary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
+  //   secondary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
+  //   secondary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
+  //   secondary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
+  //   secondary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
+  //   secondary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
+  // }
   secondary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_0);
   secondary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_0);
   secondary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_0);
@@ -563,7 +562,7 @@ void Mixer::load_secondary_mixer_values()
 
 void Mixer::init_PWM()
 {
-  if (mixer_to_use_.primary_mixer_ptr != nullptr) {
+  if (primary_mixer_is_selected_) {
     RF_.board_.pwm_init_multi(primary_mixer_.default_pwm_rate, NUM_MIXER_OUTPUTS);
   } else {
     RF_.board_.pwm_init_multi(esc_calibration_mixing.default_pwm_rate, NUM_MIXER_OUTPUTS);
@@ -652,7 +651,7 @@ void Mixer::mix_multirotor()
 {
   Controller::Output commands = RF_.controller_.output();
 
-  // Test the throttle command based on the axis corresponding to the RC F channel
+  // Check the throttle command based on the axis corresponding to the RC F channel
   float throttle_command = 0.0;
   switch (static_cast<rc_f_axis_t>(RF_.params_.get_param_int(PARAM_RC_F_AXIS))) {
     case X_AXIS:
@@ -728,10 +727,8 @@ void Mixer::mix_fixedwing()
   }
 }
 
-void Mixer::mix_output()
+void Mixer::select_primary_or_secondary_mixer()
 {
-  if (mixer_to_use_.primary_mixer_ptr == nullptr) { return; }
-
   // Check if under RC control. If RC throttle or attitude override is active,
   // adjust the mixer_to_use_ accordingly.
   if (RF_.command_manager_.rc_throttle_override_active()) {
@@ -753,6 +750,14 @@ void Mixer::mix_output()
     mixer_to_use_.Qy = &secondary_mixer_.Qy;
     mixer_to_use_.Qz = &secondary_mixer_.Qz;
   }
+}
+
+void Mixer::mix_output()
+{
+  if (!primary_mixer_is_selected_) { return; }
+
+  // Select the primary or secondary mixer based on the RC override status
+  select_primary_or_secondary_mixer();
 
   // Mix according to airframe type
   if (RF_.params_.get_param_int(PARAM_FIXED_WING)) {

From 100fe3337e0a65feba337ec1dac532c7780c4642 Mon Sep 17 00:00:00 2001
From: Jacob Moore <jacobmoor2@gmail.com>
Date: Tue, 17 Dec 2024 12:20:55 -0700
Subject: [PATCH 41/41] replaced the load parameter methods with a loop

---
 src/mixer.cpp | 178 ++++++--------------------------------------------
 1 file changed, 20 insertions(+), 158 deletions(-)

diff --git a/src/mixer.cpp b/src/mixer.cpp
index 9ac4e6d5..b50342f1 100644
--- a/src/mixer.cpp
+++ b/src/mixer.cpp
@@ -394,170 +394,32 @@ void Mixer::load_primary_mixer_values()
   primary_mixer_.default_pwm_rate[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_PWM_RATE_9);
 
   // Load the mixer values from the firmware parameters
-  // for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
-  //   // This assumes the parameters are stored in order in the param enum
-  //   int param_index = (int) PARAM_PRIMARY_MIXER_0_0 + 6 * i;
-  //   primary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
-  //   primary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
-  //   primary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
-  //   primary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
-  //   primary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
-  //   primary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
-  // }
-  primary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_0);
-  primary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_0);
-  primary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_0);
-  primary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_0);
-  primary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_0);
-  primary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_0);
-                
-  primary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_1);
-  primary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_1);
-  primary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_1);
-  primary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_1);
-  primary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_1);
-  primary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_1);
-                
-  primary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_2);
-  primary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_2);
-  primary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_2);
-  primary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_2);
-  primary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_2);
-  primary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_2);
-                
-  primary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_3);
-  primary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_3);
-  primary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_3);
-  primary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_3);
-  primary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_3);
-  primary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_3);
-                
-  primary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_4);
-  primary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_4);
-  primary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_4);
-  primary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_4);
-  primary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_4);
-  primary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_4);
-                
-  primary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_5);
-  primary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_5);
-  primary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_5);
-  primary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_5);
-  primary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_5);
-  primary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_5);
-                
-  primary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_6);
-  primary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_6);
-  primary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_6);
-  primary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_6);
-  primary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_6);
-  primary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_6);
-                
-  primary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_7);
-  primary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_7);
-  primary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_7);
-  primary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_7);
-  primary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_7);
-  primary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_7);
-                
-  primary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_8);
-  primary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_8);
-  primary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_8);
-  primary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_8);
-  primary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_8);
-  primary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_8);
-                
-  primary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_0_9);
-  primary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_1_9);
-  primary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_2_9);
-  primary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_3_9);
-  primary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_4_9);
-  primary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_PRIMARY_MIXER_5_9);
+  for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
+    // This assumes the parameters are stored in order in the param enum
+    int param_index = (int) PARAM_PRIMARY_MIXER_0_0 + 6 * i;
+    primary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
+    primary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
+    primary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
+    primary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
+    primary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
+    primary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
+  }
 }
 
 void Mixer::load_secondary_mixer_values()
 {
   // Load the mixer values from the firmware parameters
   // The header values will be the same as the primary mixer
-  // for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
-  //   // This assumes the parameters are stored in order in the param enum
-  //   int param_index = (int) PARAM_SECONDARY_MIXER_0_0 + 6 * i;
-  //   secondary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
-  //   secondary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
-  //   secondary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
-  //   secondary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
-  //   secondary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
-  //   secondary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
-  // }
-  secondary_mixer_.Fx[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_0);
-  secondary_mixer_.Fy[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_0);
-  secondary_mixer_.Fz[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_0);
-  secondary_mixer_.Qx[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_0);
-  secondary_mixer_.Qy[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_0);
-  secondary_mixer_.Qz[0] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_0);
-                
-  secondary_mixer_.Fx[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_1);
-  secondary_mixer_.Fy[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_1);
-  secondary_mixer_.Fz[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_1);
-  secondary_mixer_.Qx[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_1);
-  secondary_mixer_.Qy[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_1);
-  secondary_mixer_.Qz[1] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_1);
-                
-  secondary_mixer_.Fx[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_2);
-  secondary_mixer_.Fy[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_2);
-  secondary_mixer_.Fz[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_2);
-  secondary_mixer_.Qx[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_2);
-  secondary_mixer_.Qy[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_2);
-  secondary_mixer_.Qz[2] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_2);
-                
-  secondary_mixer_.Fx[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_3);
-  secondary_mixer_.Fy[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_3);
-  secondary_mixer_.Fz[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_3);
-  secondary_mixer_.Qx[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_3);
-  secondary_mixer_.Qy[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_3);
-  secondary_mixer_.Qz[3] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_3);
-                
-  secondary_mixer_.Fx[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_4);
-  secondary_mixer_.Fy[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_4);
-  secondary_mixer_.Fz[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_4);
-  secondary_mixer_.Qx[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_4);
-  secondary_mixer_.Qy[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_4);
-  secondary_mixer_.Qz[4] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_4);
-                
-  secondary_mixer_.Fx[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_5);
-  secondary_mixer_.Fy[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_5);
-  secondary_mixer_.Fz[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_5);
-  secondary_mixer_.Qx[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_5);
-  secondary_mixer_.Qy[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_5);
-  secondary_mixer_.Qz[5] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_5);
-                
-  secondary_mixer_.Fx[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_6);
-  secondary_mixer_.Fy[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_6);
-  secondary_mixer_.Fz[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_6);
-  secondary_mixer_.Qx[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_6);
-  secondary_mixer_.Qy[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_6);
-  secondary_mixer_.Qz[6] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_6);
-                
-  secondary_mixer_.Fx[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_7);
-  secondary_mixer_.Fy[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_7);
-  secondary_mixer_.Fz[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_7);
-  secondary_mixer_.Qx[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_7);
-  secondary_mixer_.Qy[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_7);
-  secondary_mixer_.Qz[7] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_7);
-                
-  secondary_mixer_.Fx[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_8);
-  secondary_mixer_.Fy[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_8);
-  secondary_mixer_.Fz[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_8);
-  secondary_mixer_.Qx[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_8);
-  secondary_mixer_.Qy[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_8);
-  secondary_mixer_.Qz[8] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_8);
-                
-  secondary_mixer_.Fx[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_0_9);
-  secondary_mixer_.Fy[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_1_9);
-  secondary_mixer_.Fz[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_2_9);
-  secondary_mixer_.Qx[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_3_9);
-  secondary_mixer_.Qy[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_4_9);
-  secondary_mixer_.Qz[9] = RF_.params_.get_param_float(PARAM_SECONDARY_MIXER_5_9);
+  for (int i=0; i<NUM_MIXER_OUTPUTS; ++i) {
+    // This assumes the parameters are stored in order in the param enum
+    int param_index = (int) PARAM_SECONDARY_MIXER_0_0 + 6 * i;
+    secondary_mixer_.Fx[i] = RF_.params_.get_param_float(param_index++);
+    secondary_mixer_.Fy[i] = RF_.params_.get_param_float(param_index++);
+    secondary_mixer_.Fz[i] = RF_.params_.get_param_float(param_index++);
+    secondary_mixer_.Qx[i] = RF_.params_.get_param_float(param_index++);
+    secondary_mixer_.Qy[i] = RF_.params_.get_param_float(param_index++);
+    secondary_mixer_.Qz[i] = RF_.params_.get_param_float(param_index);
+  }
 }
 
 void Mixer::init_PWM()