From d92c720559cf19d5c4356d702a0dad6e7b1dd5b1 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 3 Nov 2022 14:39:09 +0100
Subject: [PATCH 1/7] Reduce power of all motors when limiting overload

---
 .../src/power_distribution_quadrotor.c        | 49 ++++++++++++-------
 1 file changed, 32 insertions(+), 17 deletions(-)

diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index 5b8dd68732..54e7e0e881 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -50,7 +50,7 @@ uint16_t powerDistributionStopRatio(uint32_t id)
 {
   return 0;
 }
-  
+
 void powerDistributionInit(void)
 {
 }
@@ -61,29 +61,44 @@ bool powerDistributionTest(void)
   return pass;
 }
 
-#define limitThrust(VAL) limitUint16(VAL)
+static uint16_t limitThrust(float thrust, uint32_t minThrust) {
+  if (thrust < minThrust) {
+    return minThrust;
+  }
+
+  return thrust;
+}
 
 void powerDistribution(motors_thrust_t* motorPower, const control_t *control)
 {
+  const int32_t maxThrust = UINT16_MAX;
+
   int16_t r = control->roll / 2.0f;
   int16_t p = control->pitch / 2.0f;
-  motorPower->m1 = limitThrust(control->thrust - r + p + control->yaw);
-  motorPower->m2 = limitThrust(control->thrust - r - p - control->yaw);
-  motorPower->m3 =  limitThrust(control->thrust + r - p + control->yaw);
-  motorPower->m4 =  limitThrust(control->thrust + r + p - control->yaw);
 
-  if (motorPower->m1 < idleThrust) {
-    motorPower->m1 = idleThrust;
-  }
-  if (motorPower->m2 < idleThrust) {
-    motorPower->m2 = idleThrust;
-  }
-  if (motorPower->m3 < idleThrust) {
-    motorPower->m3 = idleThrust;
-  }
-  if (motorPower->m4 < idleThrust) {
-    motorPower->m4 = idleThrust;
+  int32_t m1 = control->thrust - r + p + control->yaw;
+  int32_t m2 = control->thrust - r - p - control->yaw;
+  int32_t m3 = control->thrust + r - p + control->yaw;
+  int32_t m4 = control->thrust + r + p - control->yaw;
+
+  int32_t maxM = m1;
+  if (m2 > maxM) { maxM = m2; }
+  if (m3 > maxM) { maxM = m3; }
+  if (m4 > maxM) { maxM = m4; }
+
+  if (maxM > maxThrust) {
+    float reduction = maxM - maxThrust;
+
+    m1 -= reduction;
+    m2 -= reduction;
+    m3 -= reduction;
+    m4 -= reduction;
   }
+
+  motorPower->m1 = limitThrust(m1, idleThrust);
+  motorPower->m2 = limitThrust(m2, idleThrust);
+  motorPower->m3 = limitThrust(m3, idleThrust);
+  motorPower->m4 = limitThrust(m4, idleThrust);
 }
 
 /**

From 96a23724ab38b50f85b536cc7d6dd2be5869209b Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 9 Nov 2022 14:10:42 +0100
Subject: [PATCH 2/7] Reworked power distribution Moved all thrust manipulation
 to the stabilizer loop for better overview. Added a platform specific cap
 function to allow a platform to cap thrust in more meaningful way.

---
 docs/development/create_platform.md           |   3 +-
 src/drivers/interface/motors.h                |  12 +-
 src/drivers/src/motors.c                      | 104 ++++++------------
 src/modules/interface/power_distribution.h    |  19 +++-
 src/modules/interface/stabilizer_types.h      |   7 --
 src/modules/src/power_distribution_flapper.c  |  73 ++++++------
 .../src/power_distribution_quadrotor.c        |  53 +++++----
 src/modules/src/stabilizer.c                  |  64 +++++++++--
 src/modules/src/supervisor.c                  |   4 +-
 9 files changed, 195 insertions(+), 144 deletions(-)

diff --git a/docs/development/create_platform.md b/docs/development/create_platform.md
index 7c8a00d5f0..044958fd64 100644
--- a/docs/development/create_platform.md
+++ b/docs/development/create_platform.md
@@ -270,7 +270,8 @@ endchoice
 
 The next step is to add an implementation of the power distribution function. Copy
 `power_distribution_quadrotor.c` into a new file, `power_distribution_car.c` and modify the
-`powerDistribution()` function to fit your needs.
+`powerDistribution()` function to fit your needs. Also modify the `powerDistributionCap()`, this function is responsible
+for limiting the thrust to the valid range [0 - UINT16_MAX].
 
 The final step is to add the c file to the build. Open `src/modules/src/Kbuild` and add your new file.
 ```Makefile
diff --git a/src/drivers/interface/motors.h b/src/drivers/interface/motors.h
index 1604be9c54..98050b7490 100644
--- a/src/drivers/interface/motors.h
+++ b/src/drivers/interface/motors.h
@@ -351,5 +351,15 @@ void motorsBeep(int id, bool enable, uint16_t frequency, uint16_t ratio);
  */
 const MotorHealthTestDef* motorsGetHealthTestSettings(uint32_t id);
 
-#endif /* __MOTORS_H__ */
+/**
+ * @brief Utility function to calculate thrust (actually PWM ratio), compensated for the battery state
+ * Note: both input and output may be outside the valid PWM range.
+ *
+ * @param id The id of the motor
+ * @param ithrust The desired thrust
+ * @param supplyVoltage The battery voltage
+ * @return float The PWM ratio required to get the desired thrust given the battery state.
+ */
+float motorsCompensateBatteryVoltage(uint32_t id, float iThrust, float supplyVoltage);
 
+#endif /* __MOTORS_H__ */
diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c
index 410571f36e..bc5bc23503 100644
--- a/src/drivers/src/motors.c
+++ b/src/drivers/src/motors.c
@@ -47,7 +47,6 @@
 #include "param.h"
 
 static bool motorSetEnable = false;
-static uint32_t motorPower[] = {0, 0, 0, 0};    // user-requested PWM signals
 static uint16_t motorPowerSet[] = {0, 0, 0, 0}; // user-requested PWM signals (overrides)
 static uint32_t motor_ratios[] = {0, 0, 0, 0};  // actual PWM signals
 
@@ -164,32 +163,39 @@ GPIO_InitTypeDef GPIO_PassthroughOutput =
 //
 // => p = -0.00062390   0.08835522   0.06865956
 //
-// We will not use the contant term, since we want zero thrust to equal
+// We will not use the constant term, since we want zero thrust to equal
 // zero PWM.
 //
 // And to get the PWM as a percentage we would need to divide the
 // Voltage needed with the Supply voltage.
-static uint16_t motorsCompensateBatteryVoltage(uint16_t ithrust)
+float motorsCompensateBatteryVoltage(uint32_t id, float iThrust, float supplyVoltage)
 {
-  float supply_voltage = pmGetBatteryVoltage();
-  /*
-   * A LiPo battery is supposed to be 4.2V charged, 3.7V mid-charge and 3V
-   * discharged.
-   *
-   * A suiteble sanity check for disabiling the voltage compensation would be
-   * under 2V. That would suggest a damaged battery. This protects against
-   * rushing the motors on bugs and invalid voltage levels.
-   */
-  if (supply_voltage < 2.0f)
+  #ifdef ENABLE_THRUST_BAT_COMPENSATED
+  ASSERT(id < NBR_OF_MOTORS);
+
+  if (motorMap[id]->drvType == BRUSHED)
   {
-    return ithrust;
+    /*
+    * A LiPo battery is supposed to be 4.2V charged, 3.7V mid-charge and 3V
+    * discharged.
+    *
+    * A suitable sanity check for disabling the voltage compensation would be
+    * under 2V. That would suggest a damaged battery. This protects against
+    * rushing the motors on bugs and invalid voltage levels.
+    */
+    if (supplyVoltage < 2.0f)
+    {
+      return iThrust;
+    }
+
+    float thrust = (iThrust / 65536.0f) * 60;
+    float volts = -0.0006239f * thrust * thrust + 0.088f * thrust;
+    float ratio = volts / supplyVoltage;
+    return UINT16_MAX * ratio;
   }
+  #endif
 
-  float thrust = ((float) ithrust / 65536.0f) * 60;
-  float volts = -0.0006239f * thrust * thrust + 0.088f * thrust;
-  float percentage = volts / supply_voltage;
-  percentage = percentage > 1.0f ? 1.0f : percentage;
-  return percentage * UINT16_MAX;
+  return iThrust;
 }
 
 /* Public functions */
@@ -453,23 +459,11 @@ void motorsBurstDshot()
 void motorsSetRatio(uint32_t id, uint16_t ithrust)
 {
   if (isInit) {
-    uint16_t ratio = ithrust;
-
     ASSERT(id < NBR_OF_MOTORS);
 
-    motorPower[id] = ithrust;
-
-#ifdef ENABLE_THRUST_BAT_COMPENSATED
-    if (motorMap[id]->drvType == BRUSHED)
-    {
-      // To make sure we provide the correct PWM given current supply voltage
-      // from the battery, we do calculations based on measurements of PWM,
-      // voltage and thrust. See comment at function definition for details.
-      ratio = motorsCompensateBatteryVoltage(ithrust);
-    }
-#endif
-
+    uint16_t ratio = ithrust;
     motor_ratios[id] = ratio;
+
     if (motorSetEnable) {
       ratio = motorPowerSet[id];
     }
@@ -710,51 +704,25 @@ PARAM_ADD_CORE(PARAM_UINT16, m4, &motorPowerSet[3])
 
 PARAM_GROUP_STOP(motorPowerSet)
 
+
 /**
  * Motor output related log variables.
  */
 LOG_GROUP_START(motor)
 /**
- * @brief Requested motor power (PWM value) for M1 [0 - UINT16_MAX]
+ * @brief Motor power (PWM value) for M1 [0 - UINT16_MAX]
  */
-LOG_ADD_CORE(LOG_UINT32, m1, &motorPower[0])
+LOG_ADD_CORE(LOG_UINT32, m1, &motor_ratios[MOTOR_M1])
 /**
- * @brief Requested motor power (PWM value) for M2 [0 - UINT16_MAX]
+ * @brief Motor power (PWM value) for M2 [0 - UINT16_MAX]
  */
-LOG_ADD_CORE(LOG_UINT32, m2, &motorPower[1])
+LOG_ADD_CORE(LOG_UINT32, m2, &motor_ratios[MOTOR_M2])
 /**
- * @brief Requested motor power (PWM value) for M3 [0 - UINT16_MAX]
+ * @brief Motor power (PWM value) for M3 [0 - UINT16_MAX]
  */
-LOG_ADD_CORE(LOG_UINT32, m3, &motorPower[2])
+LOG_ADD_CORE(LOG_UINT32, m3, &motor_ratios[MOTOR_M3])
 /**
- * @brief Requested motor power (PWM value) for M4 [0 - UINT16_MAX]
+ * @brief Motor power (PWM value) for M4 [0 - UINT16_MAX]
  */
-LOG_ADD_CORE(LOG_UINT32, m4, &motorPower[3])
+LOG_ADD_CORE(LOG_UINT32, m4, &motor_ratios[MOTOR_M4])
 LOG_GROUP_STOP(motor)
-
-
-/**
- * Logging variables of the motors PWM output
- */
-LOG_GROUP_START(pwm)
-/**
- * @brief Current motor 1 PWM output
- */
-LOG_ADD(LOG_UINT32, m1_pwm, &motor_ratios[0])
-/**
- * @brief Current motor 2 PWM output
- */
-LOG_ADD(LOG_UINT32, m2_pwm, &motor_ratios[1])
-/**
- * @brief Current motor 3 PWM output
- */
-LOG_ADD(LOG_UINT32, m3_pwm, &motor_ratios[2])
-/**
- * @brief Current motor 4 PWM output
- */
-LOG_ADD(LOG_UINT32, m4_pwm, &motor_ratios[3])
-/**
- * @brief Cycle time of M1 output in microseconds
- */
-LOG_ADD(LOG_UINT32, cycletime, &cycleTime)
-LOG_GROUP_STOP(pwm)
diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h
index 6a7a17405b..cd2cd48a1c 100644
--- a/src/modules/interface/power_distribution.h
+++ b/src/modules/interface/power_distribution.h
@@ -31,7 +31,24 @@
 
 void powerDistributionInit(void);
 bool powerDistributionTest(void);
-void powerDistribution(motors_thrust_t* motorPower, const control_t *control);
+
+/**
+ * @brief Calculate the power (thrust) of each motor based on the output from the controller
+ *
+ * @param control Data from the controller
+ * @param motorThrustUncapped The desired thrust
+ */
+void powerDistribution(const control_t *control, int32_t motorThrustUncapped[]);
+
+/**
+ * @brief Cap the thrust for the motors when out side of the valid range [0 - UINT16_MAX]. The platform specific
+ * implementation can chose to cap the trust in a way that provides graceful degradation, for instance prioritizing
+ * attitude over thrust.
+ *
+ * @param motorThrustBatCompUncapped The desired thrust for the motors
+ * @param motorPwm The capped thrust
+ */
+void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[]);
 
 /**
  * Returns a 1 when motor 'id' gives thrust, returns 0 otherwise
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index b7d3ac0be4..6640b70a21 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -174,13 +174,6 @@ typedef struct control_s {
   float thrust;
 } control_t;
 
-typedef struct motors_thrust_s {
-  uint16_t m1;  // PWM ratio
-  uint16_t m2;  // PWM ratio
-  uint16_t m3;  // PWM ratio
-  uint16_t m4;  // PWM ratio
-} motors_thrust_t;
-
 typedef enum mode_e {
   modeDisable = 0,
   modeAbs,
diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c
index 0713355bdc..bb6f3b9bf9 100644
--- a/src/modules/src/power_distribution_flapper.c
+++ b/src/modules/src/power_distribution_flapper.c
@@ -33,6 +33,7 @@
 #include "num.h"
 #include "autoconf.h"
 #include "config.h"
+#include "motors.h"
 
 #include "debug.h"
 #include "math.h"
@@ -104,7 +105,7 @@ int powerDistributionMotorType(uint32_t id)
 {
   int type = 1;
   if (id == idPitch || id == idYaw)
-  { 
+  {
     type = 0;
   }
 
@@ -114,18 +115,18 @@ int powerDistributionMotorType(uint32_t id)
 uint16_t powerDistributionStopRatio(uint32_t id)
 {
   uint16_t stopRatio = 0;
-  if (id == idPitch) 
+  if (id == idPitch)
   {
     stopRatio = flapperConfig.pitchServoNeutral*act_max/100.0f;
   }
-  else if (id == idYaw) 
+  else if (id == idYaw)
   {
     stopRatio = flapperConfig.yawServoNeutral*act_max/100.0f;
   }
 
   return stopRatio;
 }
-  
+
 void powerDistributionInit(void)
 {
   #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
@@ -133,7 +134,7 @@ void powerDistributionInit(void)
   #else
     DEBUG_PRINT("Using Flapper power distribution | PCB revD (2022) or newer\n");
   #endif
-  
+
 }
 
 bool powerDistributionTest(void)
@@ -142,40 +143,46 @@ bool powerDistributionTest(void)
   return pass;
 }
 
-#define limitThrust(VAL) limitUint16(VAL)
+uint16_t limitThrust(int32_t value, int32_t min, int32_t max)
+{
+  if (value < min) { return min; }
+  if (value > max) { return max; }
+  return value;
+}
 
-void powerDistribution(motors_thrust_t* motorPower, const control_t *control)
+void powerDistribution(const control_t *control, int32_t motorThrustUncapped[])
 {
   thrust = fmin(control->thrust, flapperConfig.maxThrust);
-  
+
   flapperConfig.pitchServoNeutral=limitServoNeutral(flapperConfig.pitchServoNeutral);
   flapperConfig.yawServoNeutral=limitServoNeutral(flapperConfig.yawServoNeutral);
   flapperConfig.rollBias=limitRollBias(flapperConfig.rollBias);
 
   #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
-    motorPower->m2 = limitThrust(flapperConfig.pitchServoNeutral*act_max/100.0f + pitch_ampl*control->pitch); // pitch servo
-    motorPower->m3 = limitThrust(flapperConfig.yawServoNeutral*act_max/100.0f - control->yaw); // yaw servo
-    motorPower->m1 = limitThrust( 0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias/100.0f) ); // left motor
-    motorPower->m4 = limitThrust(-0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias/100.0f) ); // right motor
-    
-    if (motorPower->m1 < idleThrust) {
-      motorPower->m1 = idleThrust;
-    }
-    if (motorPower->m4 < idleThrust) {
-      motorPower->m4 = idleThrust;
-    }
+    motorThrustUncapped[MOTOR_M2] = flapperConfig.pitchServoNeutral*act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
+    motorThrustUncapped[MOTOR_M3] = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
+    motorThrustUncapped[MOTOR_M1] =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
+    motorThrustUncapped[MOTOR_M4] = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
+  #else
+    motorThrustUncapped[MOTOR_M1] = flapperConfig.pitchServoNeutral * act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
+    motorThrustUncapped[MOTOR_M3] = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
+    motorThrustUncapped[MOTOR_M2] =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
+    motorThrustUncapped[MOTOR_M4] = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
+  #endif
+}
+
+void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[])
+{
+  #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
+    motorPwm[MOTOR_M2] = limitThrust(motorThrustBatCompUncapped[MOTOR_M2], 0, UINT16_MAX); // pitch servo
+    motorPwm[MOTOR_M3] = limitThrust(motorThrustBatCompUncapped[MOTOR_M3], 0, UINT16_MAX); // yaw servo
+    motorPwm[MOTOR_M1] = limitThrust(motorThrustBatCompUncapped[MOTOR_M1], idleThrust, UINT16_MAX); // left motor
+    motorPwm[MOTOR_M4] = limitThrust(motorThrustBatCompUncapped[MOTOR_M4], idleThrust, UINT16_MAX); // left motor
   #else
-    motorPower->m1 = limitThrust(flapperConfig.pitchServoNeutral*act_max/100.0f + pitch_ampl*control->pitch); // pitch servo
-    motorPower->m3 = limitThrust(flapperConfig.yawServoNeutral*act_max/100.0f - control->yaw); // yaw servo
-    motorPower->m2 = limitThrust( 0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias/100.0f) ); // left motor
-    motorPower->m4 = limitThrust(-0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias/100.0f) ); // right motor
-    
-    if (motorPower->m2 < idleThrust) {
-      motorPower->m2 = idleThrust;
-    }
-    if (motorPower->m4 < idleThrust) {
-      motorPower->m4 = idleThrust;
-    }
+    motorPwm[MOTOR_M1] = limitThrust(motorThrustBatCompUncapped[MOTOR_M1], 0, UINT16_MAX); // pitch servo
+    motorPwm[MOTOR_M3] = limitThrust(motorThrustBatCompUncapped[MOTOR_M3], 0, UINT16_MAX); // yaw servo
+    motorPwm[MOTOR_M2] = limitThrust(motorThrustBatCompUncapped[MOTOR_M2], idleThrust, UINT16_MAX); // left motor
+    motorPwm[MOTOR_M4] = limitThrust(motorThrustBatCompUncapped[MOTOR_M4], idleThrust, UINT16_MAX); // right motor
   #endif
 }
 
@@ -202,7 +209,7 @@ PARAM_GROUP_START(flapper)
  * @brief Roll bias <-25%; 25%> (default 0%)
  *
  * This parameter can be used if uneven performance of the left and right flapping mechanaisms and/or wings
- * is observed, which in flight results in a drift in roll/sideways flight. Positive values make the drone roll 
+ * is observed, which in flight results in a drift in roll/sideways flight. Positive values make the drone roll
  * more to the right, negative values to the left.
  */
 PARAM_ADD(PARAM_INT8 | PARAM_PERSISTENT, motBiasRoll, &flapperConfig.rollBias)
@@ -217,14 +224,14 @@ PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servPitchNeutr, &flapperConfig.pitchSe
 /**
  * @brief Yaw servo neutral <25%; 75%> (default 50%)
  *
- * The parameter sets the neutral position of the yaw servo, such that the yaw control arm is pointed spanwise. If in flight 
+ * The parameter sets the neutral position of the yaw servo, such that the yaw control arm is pointed spanwise. If in flight
  * you observe drift in the clock-wise direction, increase this parameter and vice-versa if the drift is counter-clock-wise.
  */
 PARAM_ADD(PARAM_UINT8 | PARAM_PERSISTENT, servYawNeutr, &flapperConfig.yawServoNeutral)
 /**
  * @brief Yaw servo neutral <25%; 75%> (default 50%)
  *
- * The parameter sets the neutral position of the yaw servo, such that the yaw control arm is pointed spanwise. If in flight 
+ * The parameter sets the neutral position of the yaw servo, such that the yaw control arm is pointed spanwise. If in flight
  * you observe drift in the clock-wise direction, increase this parameter and vice-versa if the drift is counter-clock-wise.
  */
 PARAM_ADD(PARAM_UINT16 | PARAM_PERSISTENT, flapperMaxThrust, &flapperConfig.maxThrust)
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index 54e7e0e881..a80367713c 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -32,6 +32,7 @@
 #include "num.h"
 #include "autoconf.h"
 #include "config.h"
+#include "motors.h"
 
 #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST
 #  define DEFAULT_IDLE_THRUST 0
@@ -61,7 +62,7 @@ bool powerDistributionTest(void)
   return pass;
 }
 
-static uint16_t limitThrust(float thrust, uint32_t minThrust) {
+static uint16_t capMinThrust(float thrust, uint32_t minThrust) {
   if (thrust < minThrust) {
     return minThrust;
   }
@@ -69,36 +70,42 @@ static uint16_t limitThrust(float thrust, uint32_t minThrust) {
   return thrust;
 }
 
-void powerDistribution(motors_thrust_t* motorPower, const control_t *control)
+void powerDistribution(const control_t *control, int32_t motorThrustUncapped[])
 {
-  const int32_t maxThrust = UINT16_MAX;
-
   int16_t r = control->roll / 2.0f;
   int16_t p = control->pitch / 2.0f;
 
-  int32_t m1 = control->thrust - r + p + control->yaw;
-  int32_t m2 = control->thrust - r - p - control->yaw;
-  int32_t m3 = control->thrust + r - p + control->yaw;
-  int32_t m4 = control->thrust + r + p - control->yaw;
-
-  int32_t maxM = m1;
-  if (m2 > maxM) { maxM = m2; }
-  if (m3 > maxM) { maxM = m3; }
-  if (m4 > maxM) { maxM = m4; }
+  motorThrustUncapped[MOTOR_M1] = control->thrust - r + p + control->yaw;
+  motorThrustUncapped[MOTOR_M2] = control->thrust - r - p - control->yaw;
+  motorThrustUncapped[MOTOR_M3] = control->thrust + r - p + control->yaw;
+  motorThrustUncapped[MOTOR_M4] = control->thrust + r + p - control->yaw;
+}
 
-  if (maxM > maxThrust) {
-    float reduction = maxM - maxThrust;
+void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[])
+{
+  const int32_t maxAllowedThrust = UINT16_MAX;
+
+  // Find highest thrust
+  int32_t highestThrustFound = 0;
+  for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+  {
+    if (motorThrustBatCompUncapped[motor] > highestThrustFound)
+    {
+      highestThrustFound = motorThrustBatCompUncapped[motor];
+    }
+  }
 
-    m1 -= reduction;
-    m2 -= reduction;
-    m3 -= reduction;
-    m4 -= reduction;
+  int32_t reduction = 0;
+  if (highestThrustFound > maxAllowedThrust)
+  {
+    reduction = highestThrustFound - maxAllowedThrust;
   }
 
-  motorPower->m1 = limitThrust(m1, idleThrust);
-  motorPower->m2 = limitThrust(m2, idleThrust);
-  motorPower->m3 = limitThrust(m3, idleThrust);
-  motorPower->m4 = limitThrust(m4, idleThrust);
+  for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+  {
+    int32_t thrustCappedUpper = motorThrustBatCompUncapped[motor] - reduction;
+    motorPwm[motor] = capMinThrust(thrustCappedUpper, idleThrust);
+  }
 }
 
 /**
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 01b1c8fd61..0d355b7316 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -68,7 +68,11 @@ static setpoint_t setpoint;
 static sensorData_t sensorData;
 static state_t state;
 static control_t control;
-static motors_thrust_t motorPower;
+
+static int32_t motorThrustUncapped[NBR_OF_MOTORS];
+static int32_t motorThrustBatCompUncapped[NBR_OF_MOTORS];
+static uint16_t motorPwm[NBR_OF_MOTORS];
+
 // For scratch storage - never logged or passed to other subsystems.
 static setpoint_t tempSetpoint;
 
@@ -211,11 +215,28 @@ static void checkEmergencyStopTimeout()
   }
 }
 
-/* The stabilizer loop runs at 1kHz (stock) or 500Hz (kalman). It is the
+static void batteryCompensation(int32_t motorThrustUncapped[], uint32_t motorThrustBatCompUncapped[])
+{
+    float supplyVoltage = pmGetBatteryVoltage();
+
+    for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+    {
+      motorThrustBatCompUncapped[motor] = motorsCompensateBatteryVoltage(motor, motorThrustUncapped[motor], supplyVoltage);
+    }
+}
+
+static void setMotorRatios(const uint16_t motorPwm[])
+{
+    for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+    {
+      motorsSetRatio(motor, motorPwm[motor]);
+    }
+}
+
+/* The stabilizer loop runs at 1kHz. It is the
  * responsibility of the different functions to run slower by skipping call
  * (ie. returning without modifying the output structure).
  */
-
 static void stabilizerTask(void* param)
 {
   uint32_t tick;
@@ -285,11 +306,10 @@ static void stabilizerTask(void* param)
       if (emergencyStop || (systemIsArmed() == false)) {
         motorsStop();
       } else {
-        powerDistribution(&motorPower, &control);
-        motorsSetRatio(MOTOR_M1, motorPower.m1);
-        motorsSetRatio(MOTOR_M2, motorPower.m2);
-        motorsSetRatio(MOTOR_M3, motorPower.m3);
-        motorsSetRatio(MOTOR_M4, motorPower.m4);
+        powerDistribution(&control, motorThrustUncapped);
+        batteryCompensation(motorThrustUncapped, motorThrustBatCompUncapped);
+        powerDistributionCap(motorThrustBatCompUncapped, motorPwm);
+        setMotorRatios(motorPwm);
       }
 
 #ifdef CONFIG_DECK_USD
@@ -793,3 +813,31 @@ LOG_ADD(LOG_INT16, ratePitch, &stateCompressed.ratePitch)
  */
 LOG_ADD(LOG_INT16, rateYaw, &stateCompressed.rateYaw)
 LOG_GROUP_STOP(stateEstimateZ)
+
+
+LOG_GROUP_START(motor)
+
+/**
+ * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
+ * and may have values outside the [0 - UINT16_MAX] range.
+ */
+LOG_ADD(LOG_INT32, m1, &motorThrustBatCompUncapped[MOTOR_M1])
+
+/**
+ * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
+ * and may have values outside the [0 - UINT16_MAX] range.
+ */
+LOG_ADD(LOG_INT32, m2, &motorThrustBatCompUncapped[MOTOR_M2])
+
+/**
+ * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
+ * and may have values outside the [0 - UINT16_MAX] range.
+ */
+LOG_ADD(LOG_INT32, m3, &motorThrustBatCompUncapped[MOTOR_M3])
+
+/**
+ * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
+ * and may have values outside the [0 - UINT16_MAX] range.
+ */
+LOG_ADD(LOG_INT32, m4, &motorThrustBatCompUncapped[MOTOR_M4])
+LOG_GROUP_STOP(motor)
diff --git a/src/modules/src/supervisor.c b/src/modules/src/supervisor.c
index 59cba6c2ad..f9d6065656 100644
--- a/src/modules/src/supervisor.c
+++ b/src/modules/src/supervisor.c
@@ -74,14 +74,14 @@ static bool canFlyCheck()
 }
 
 //
-// We say we are flying if the sum of the ratios of all motors giving thrust 
+// We say we are flying if the sum of the ratios of all motors giving thrust
 // is above a certain threshold.
 //
 static bool isFlyingCheck()
 {
   int sumRatio = 0;
   for (int i = 0; i < NBR_OF_MOTORS; ++i) {
-    sumRatio += powerDistributionMotorType(i)*motorsGetRatio(i);
+    sumRatio += powerDistributionMotorType(i) * motorsGetRatio(i);
   }
 
   return sumRatio > SUPERVISOR_FLIGHT_THRESHOLD;

From c6ac726d34e8d87be96470f3f5602c2e5bcc6ec4 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 9 Nov 2022 14:15:38 +0100
Subject: [PATCH 3/7] Set logs also when overriding motor power

---
 src/drivers/src/motors.c | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/src/drivers/src/motors.c b/src/drivers/src/motors.c
index bc5bc23503..66decfbc9a 100644
--- a/src/drivers/src/motors.c
+++ b/src/drivers/src/motors.c
@@ -462,12 +462,13 @@ void motorsSetRatio(uint32_t id, uint16_t ithrust)
     ASSERT(id < NBR_OF_MOTORS);
 
     uint16_t ratio = ithrust;
-    motor_ratios[id] = ratio;
 
     if (motorSetEnable) {
       ratio = motorPowerSet[id];
     }
 
+    motor_ratios[id] = ratio;
+
     if (motorMap[id]->drvType == BRUSHLESS)
     {
 #ifdef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT

From ffd39a71b84bdfa82bc55d8f87945203e29dcbdd Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 9 Nov 2022 14:58:06 +0100
Subject: [PATCH 4/7] Reworked motor thrust types

---
 src/modules/interface/power_distribution.h    |  4 +-
 src/modules/interface/stabilizer_types.h      | 23 ++++++++++
 src/modules/src/power_distribution_flapper.c  | 37 ++++++++--------
 .../src/power_distribution_quadrotor.c        | 26 +++++------
 src/modules/src/stabilizer.c                  | 44 +++++++++----------
 test_python/test_power_distribution.py        | 14 +++---
 6 files changed, 85 insertions(+), 63 deletions(-)

diff --git a/src/modules/interface/power_distribution.h b/src/modules/interface/power_distribution.h
index cd2cd48a1c..1577ff1983 100644
--- a/src/modules/interface/power_distribution.h
+++ b/src/modules/interface/power_distribution.h
@@ -38,7 +38,7 @@ bool powerDistributionTest(void);
  * @param control Data from the controller
  * @param motorThrustUncapped The desired thrust
  */
-void powerDistribution(const control_t *control, int32_t motorThrustUncapped[]);
+void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped);
 
 /**
  * @brief Cap the thrust for the motors when out side of the valid range [0 - UINT16_MAX]. The platform specific
@@ -48,7 +48,7 @@ void powerDistribution(const control_t *control, int32_t motorThrustUncapped[]);
  * @param motorThrustBatCompUncapped The desired thrust for the motors
  * @param motorPwm The capped thrust
  */
-void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[]);
+void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm);
 
 /**
  * Returns a 1 when motor 'id' gives thrust, returns 0 otherwise
diff --git a/src/modules/interface/stabilizer_types.h b/src/modules/interface/stabilizer_types.h
index 6640b70a21..d5dfeebe48 100644
--- a/src/modules/interface/stabilizer_types.h
+++ b/src/modules/interface/stabilizer_types.h
@@ -174,6 +174,29 @@ typedef struct control_s {
   float thrust;
 } control_t;
 
+
+#define STABILIZER_NR_OF_MOTORS 4
+
+typedef union {
+  int32_t list[STABILIZER_NR_OF_MOTORS];
+  struct {
+    int32_t m1;
+    int32_t m2;
+    int32_t m3;
+    int32_t m4;
+  } motors;
+} motors_thrust_uncapped_t;
+
+typedef union {
+  uint16_t list[STABILIZER_NR_OF_MOTORS];
+  struct {
+    uint16_t m1;  // PWM ratio
+    uint16_t m2;  // PWM ratio
+    uint16_t m3;  // PWM ratio
+    uint16_t m4;  // PWM ratio
+  } motors;
+} motors_thrust_pwm_t;
+
 typedef enum mode_e {
   modeDisable = 0,
   modeAbs,
diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c
index bb6f3b9bf9..9f0faf2f18 100644
--- a/src/modules/src/power_distribution_flapper.c
+++ b/src/modules/src/power_distribution_flapper.c
@@ -33,7 +33,6 @@
 #include "num.h"
 #include "autoconf.h"
 #include "config.h"
-#include "motors.h"
 
 #include "debug.h"
 #include "math.h"
@@ -150,7 +149,7 @@ uint16_t limitThrust(int32_t value, int32_t min, int32_t max)
   return value;
 }
 
-void powerDistribution(const control_t *control, int32_t motorThrustUncapped[])
+void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped)
 {
   thrust = fmin(control->thrust, flapperConfig.maxThrust);
 
@@ -159,30 +158,30 @@ void powerDistribution(const control_t *control, int32_t motorThrustUncapped[])
   flapperConfig.rollBias=limitRollBias(flapperConfig.rollBias);
 
   #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
-    motorThrustUncapped[MOTOR_M2] = flapperConfig.pitchServoNeutral*act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
-    motorThrustUncapped[MOTOR_M3] = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
-    motorThrustUncapped[MOTOR_M1] =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
-    motorThrustUncapped[MOTOR_M4] = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
+    motorThrustUncapped->motors.m2 = flapperConfig.pitchServoNeutral*act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
+    motorThrustUncapped->motors.m3 = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
+    motorThrustUncapped->motors.m1 =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
+    motorThrustUncapped->motors.m4 = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
   #else
-    motorThrustUncapped[MOTOR_M1] = flapperConfig.pitchServoNeutral * act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
-    motorThrustUncapped[MOTOR_M3] = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
-    motorThrustUncapped[MOTOR_M2] =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
-    motorThrustUncapped[MOTOR_M4] = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
+    motorThrustUncapped->motors.m1 = flapperConfig.pitchServoNeutral * act_max / 100.0f + pitch_ampl * control->pitch; // pitch servo
+    motorThrustUncapped->motors.m3 = flapperConfig.yawServoNeutral*act_max / 100.0f - control->yaw; // yaw servo
+    motorThrustUncapped->motors.m2 =  0.5f * control->roll + thrust * (1.0f + flapperConfig.rollBias / 100.0f); // left motor
+    motorThrustUncapped->motors.m4 = -0.5f * control->roll + thrust * (1.0f - flapperConfig.rollBias / 100.0f); // right motor
   #endif
 }
 
-void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[])
+void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm)
 {
   #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
-    motorPwm[MOTOR_M2] = limitThrust(motorThrustBatCompUncapped[MOTOR_M2], 0, UINT16_MAX); // pitch servo
-    motorPwm[MOTOR_M3] = limitThrust(motorThrustBatCompUncapped[MOTOR_M3], 0, UINT16_MAX); // yaw servo
-    motorPwm[MOTOR_M1] = limitThrust(motorThrustBatCompUncapped[MOTOR_M1], idleThrust, UINT16_MAX); // left motor
-    motorPwm[MOTOR_M4] = limitThrust(motorThrustBatCompUncapped[MOTOR_M4], idleThrust, UINT16_MAX); // left motor
+    motorPwm->motors.m2 = limitThrust(motorThrustBatCompUncapped->motors.m2, 0, UINT16_MAX); // pitch servo
+    motorPwm->motors.m3 = limitThrust(motorThrustBatCompUncapped->motors.m3, 0, UINT16_MAX); // yaw servo
+    motorPwm->motors.m1 = limitThrust(motorThrustBatCompUncapped->motors.m1, idleThrust, UINT16_MAX); // left motor
+    motorPwm->motors.m4 = limitThrust(motorThrustBatCompUncapped->motors.m4, idleThrust, UINT16_MAX); // left motor
   #else
-    motorPwm[MOTOR_M1] = limitThrust(motorThrustBatCompUncapped[MOTOR_M1], 0, UINT16_MAX); // pitch servo
-    motorPwm[MOTOR_M3] = limitThrust(motorThrustBatCompUncapped[MOTOR_M3], 0, UINT16_MAX); // yaw servo
-    motorPwm[MOTOR_M2] = limitThrust(motorThrustBatCompUncapped[MOTOR_M2], idleThrust, UINT16_MAX); // left motor
-    motorPwm[MOTOR_M4] = limitThrust(motorThrustBatCompUncapped[MOTOR_M4], idleThrust, UINT16_MAX); // right motor
+    motorPwm->motors.m1 = limitThrust(motorThrustBatCompUncapped->motors.m1, 0, UINT16_MAX); // pitch servo
+    motorPwm->motors.m3 = limitThrust(motorThrustBatCompUncapped->motors.m3, 0, UINT16_MAX); // yaw servo
+    motorPwm->motors.m2 = limitThrust(motorThrustBatCompUncapped->motors.m2, idleThrust, UINT16_MAX); // left motor
+    motorPwm->motors.m4 = limitThrust(motorThrustBatCompUncapped->motors.m4, idleThrust, UINT16_MAX); // right motor
   #endif
 }
 
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
index a80367713c..55fc1b798a 100644
--- a/src/modules/src/power_distribution_quadrotor.c
+++ b/src/modules/src/power_distribution_quadrotor.c
@@ -32,7 +32,6 @@
 #include "num.h"
 #include "autoconf.h"
 #include "config.h"
-#include "motors.h"
 
 #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST
 #  define DEFAULT_IDLE_THRUST 0
@@ -70,28 +69,29 @@ static uint16_t capMinThrust(float thrust, uint32_t minThrust) {
   return thrust;
 }
 
-void powerDistribution(const control_t *control, int32_t motorThrustUncapped[])
+void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped)
 {
   int16_t r = control->roll / 2.0f;
   int16_t p = control->pitch / 2.0f;
 
-  motorThrustUncapped[MOTOR_M1] = control->thrust - r + p + control->yaw;
-  motorThrustUncapped[MOTOR_M2] = control->thrust - r - p - control->yaw;
-  motorThrustUncapped[MOTOR_M3] = control->thrust + r - p + control->yaw;
-  motorThrustUncapped[MOTOR_M4] = control->thrust + r + p - control->yaw;
+  motorThrustUncapped->motors.m1 = control->thrust - r + p + control->yaw;
+  motorThrustUncapped->motors.m2 = control->thrust - r - p - control->yaw;
+  motorThrustUncapped->motors.m3 = control->thrust + r - p + control->yaw;
+  motorThrustUncapped->motors.m4 = control->thrust + r + p - control->yaw;
 }
 
-void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPwm[])
+void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm)
 {
   const int32_t maxAllowedThrust = UINT16_MAX;
 
   // Find highest thrust
   int32_t highestThrustFound = 0;
-  for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+  for (int motorIndex = 0; motorIndex < STABILIZER_NR_OF_MOTORS; motorIndex++)
   {
-    if (motorThrustBatCompUncapped[motor] > highestThrustFound)
+    const int32_t thrust = motorThrustBatCompUncapped->list[motorIndex];
+    if (thrust > highestThrustFound)
     {
-      highestThrustFound = motorThrustBatCompUncapped[motor];
+      highestThrustFound = thrust;
     }
   }
 
@@ -101,10 +101,10 @@ void powerDistributionCap(int32_t motorThrustBatCompUncapped[], uint16_t motorPw
     reduction = highestThrustFound - maxAllowedThrust;
   }
 
-  for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
+  for (int motorIndex = 0; motorIndex < STABILIZER_NR_OF_MOTORS; motorIndex++)
   {
-    int32_t thrustCappedUpper = motorThrustBatCompUncapped[motor] - reduction;
-    motorPwm[motor] = capMinThrust(thrustCappedUpper, idleThrust);
+    int32_t thrustCappedUpper = motorThrustBatCompUncapped->list[motorIndex] - reduction;
+    motorPwm->list[motorIndex] = capMinThrust(thrustCappedUpper, idleThrust);
   }
 }
 
diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 0d355b7316..379452cffd 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -69,9 +69,9 @@ static sensorData_t sensorData;
 static state_t state;
 static control_t control;
 
-static int32_t motorThrustUncapped[NBR_OF_MOTORS];
-static int32_t motorThrustBatCompUncapped[NBR_OF_MOTORS];
-static uint16_t motorPwm[NBR_OF_MOTORS];
+static motors_thrust_uncapped_t motorThrustUncapped;
+static motors_thrust_uncapped_t motorThrustBatCompUncapped;
+static motors_thrust_pwm_t motorPwm;
 
 // For scratch storage - never logged or passed to other subsystems.
 static setpoint_t tempSetpoint;
@@ -215,22 +215,22 @@ static void checkEmergencyStopTimeout()
   }
 }
 
-static void batteryCompensation(int32_t motorThrustUncapped[], uint32_t motorThrustBatCompUncapped[])
+static void batteryCompensation(const motors_thrust_uncapped_t* motorThrustUncapped, motors_thrust_uncapped_t* motorThrustBatCompUncapped)
 {
-    float supplyVoltage = pmGetBatteryVoltage();
+  float supplyVoltage = pmGetBatteryVoltage();
 
-    for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
-    {
-      motorThrustBatCompUncapped[motor] = motorsCompensateBatteryVoltage(motor, motorThrustUncapped[motor], supplyVoltage);
-    }
+  for (int motor = 0; motor < STABILIZER_NR_OF_MOTORS; motor++)
+  {
+    motorThrustBatCompUncapped->list[motor] = motorsCompensateBatteryVoltage(motor, motorThrustUncapped->list[motor], supplyVoltage);
+  }
 }
 
-static void setMotorRatios(const uint16_t motorPwm[])
+static void setMotorRatios(const motors_thrust_pwm_t* motorPwm)
 {
-    for (int motor = 0; motor < NBR_OF_MOTORS; motor++)
-    {
-      motorsSetRatio(motor, motorPwm[motor]);
-    }
+  motorsSetRatio(MOTOR_M1, motorPwm->motors.m1);
+  motorsSetRatio(MOTOR_M2, motorPwm->motors.m2);
+  motorsSetRatio(MOTOR_M3, motorPwm->motors.m3);
+  motorsSetRatio(MOTOR_M4, motorPwm->motors.m4);
 }
 
 /* The stabilizer loop runs at 1kHz. It is the
@@ -306,10 +306,10 @@ static void stabilizerTask(void* param)
       if (emergencyStop || (systemIsArmed() == false)) {
         motorsStop();
       } else {
-        powerDistribution(&control, motorThrustUncapped);
-        batteryCompensation(motorThrustUncapped, motorThrustBatCompUncapped);
-        powerDistributionCap(motorThrustBatCompUncapped, motorPwm);
-        setMotorRatios(motorPwm);
+        powerDistribution(&control, &motorThrustUncapped);
+        batteryCompensation(&motorThrustUncapped, &motorThrustBatCompUncapped);
+        powerDistributionCap(&motorThrustBatCompUncapped, &motorPwm);
+        setMotorRatios(&motorPwm);
       }
 
 #ifdef CONFIG_DECK_USD
@@ -821,23 +821,23 @@ LOG_GROUP_START(motor)
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m1, &motorThrustBatCompUncapped[MOTOR_M1])
+LOG_ADD(LOG_INT32, m1, &motorThrustBatCompUncapped.motors.m1)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m2, &motorThrustBatCompUncapped[MOTOR_M2])
+LOG_ADD(LOG_INT32, m2, &motorThrustBatCompUncapped.motors.m2)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m3, &motorThrustBatCompUncapped[MOTOR_M3])
+LOG_ADD(LOG_INT32, m3, &motorThrustBatCompUncapped.motors.m3)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m4, &motorThrustBatCompUncapped[MOTOR_M4])
+LOG_ADD(LOG_INT32, m4, &motorThrustBatCompUncapped.motors.m4)
 LOG_GROUP_STOP(motor)
diff --git a/test_python/test_power_distribution.py b/test_python/test_power_distribution.py
index 802392c717..bde60b1eef 100644
--- a/test_python/test_power_distribution.py
+++ b/test_python/test_power_distribution.py
@@ -2,20 +2,20 @@
 
 import cffirmware
 
-def test_controller_mellinger():
+def test_basic_power_distribution():
 
     cffirmware.controllerMellingerInit()
 
-    motorPower = cffirmware.motors_thrust_t()
+    motorPower = cffirmware.motors_thrust_uncapped_t()
     control = cffirmware.control_t()
     control.thrust = 10
     control.roll = 0
     control.pitch = 0
     control.yaw = 0
 
-    cffirmware.powerDistribution(motorPower, control)
+    cffirmware.powerDistribution(control, motorPower)
     # control.thrust will be at a (tuned) hover-state
-    assert motorPower.m1 == control.thrust
-    assert motorPower.m2 == control.thrust
-    assert motorPower.m3 == control.thrust
-    assert motorPower.m4 == control.thrust
+    assert motorPower.motors.m1 == control.thrust
+    assert motorPower.motors.m2 == control.thrust
+    assert motorPower.motors.m3 == control.thrust
+    assert motorPower.motors.m4 == control.thrust

From 3b9a6c0147c96c271702f18db42d8a1cbb0511a0 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 9 Nov 2022 15:24:32 +0100
Subject: [PATCH 5/7] Added tests for power distribution

---
 test_python/test_power_distribution.py | 178 +++++++++++++++++++++++--
 1 file changed, 168 insertions(+), 10 deletions(-)

diff --git a/test_python/test_power_distribution.py b/test_python/test_power_distribution.py
index bde60b1eef..d115833d2e 100644
--- a/test_python/test_power_distribution.py
+++ b/test_python/test_power_distribution.py
@@ -2,20 +2,178 @@
 
 import cffirmware
 
-def test_basic_power_distribution():
-
-    cffirmware.controllerMellingerInit()
-
-    motorPower = cffirmware.motors_thrust_uncapped_t()
+def test_power_distribution_for_hover():
+    # Fixture
     control = cffirmware.control_t()
     control.thrust = 10
     control.roll = 0
     control.pitch = 0
     control.yaw = 0
 
-    cffirmware.powerDistribution(control, motorPower)
+    actual = cffirmware.motors_thrust_uncapped_t()
+
+    # Test
+    cffirmware.powerDistribution(control, actual)
+
+    # Assert
     # control.thrust will be at a (tuned) hover-state
-    assert motorPower.motors.m1 == control.thrust
-    assert motorPower.motors.m2 == control.thrust
-    assert motorPower.motors.m3 == control.thrust
-    assert motorPower.motors.m4 == control.thrust
+    assert actual.motors.m1 == control.thrust
+    assert actual.motors.m2 == control.thrust
+    assert actual.motors.m3 == control.thrust
+    assert actual.motors.m4 == control.thrust
+
+def test_power_distribution_for_roll():
+    # Fixture
+    control = cffirmware.control_t()
+    control.thrust = 0
+    control.roll = 10
+    control.pitch = 0
+    control.yaw = 0
+
+    actual = cffirmware.motors_thrust_uncapped_t()
+
+    # Test
+    cffirmware.powerDistribution(control, actual)
+
+    # Assert
+    # control.thrust will be at a (tuned) hover-state
+    assert actual.motors.m1 == -control.roll / 2
+    assert actual.motors.m2 == -control.roll / 2
+    assert actual.motors.m3 == control.roll / 2
+    assert actual.motors.m4 == control.roll / 2
+
+def test_power_distribution_for_pitch():
+    # Fixture
+    control = cffirmware.control_t()
+    control.thrust = 0
+    control.roll = 0
+    control.pitch = 12
+    control.yaw = 0
+
+    actual = cffirmware.motors_thrust_uncapped_t()
+
+    # Test
+    cffirmware.powerDistribution(control, actual)
+
+    # Assert
+    # control.thrust will be at a (tuned) hover-state
+    assert actual.motors.m1 == control.pitch / 2
+    assert actual.motors.m2 == -control.pitch / 2
+    assert actual.motors.m3 == -control.pitch / 2
+    assert actual.motors.m4 == control.pitch / 2
+
+def test_power_distribution_for_yaw():
+    # Fixture
+    control = cffirmware.control_t()
+    control.thrust = 0
+    control.roll = 0
+    control.pitch = 0
+    control.yaw = 20
+
+    actual = cffirmware.motors_thrust_uncapped_t()
+
+    # Test
+    cffirmware.powerDistribution(control, actual)
+
+    # Assert
+    # control.thrust will be at a (tuned) hover-state
+    assert actual.motors.m1 == control.yaw
+    assert actual.motors.m2 == -control.yaw
+    assert actual.motors.m3 == control.yaw
+    assert actual.motors.m4 == -control.yaw
+
+def test_power_distribution_cap_when_in_range():
+    # Fixture
+    input = cffirmware.motors_thrust_uncapped_t()
+    input.motors.m1 = 1000
+    input.motors.m2 = 2000
+    input.motors.m3 = 3000
+    input.motors.m4 = 4000
+
+    actual = cffirmware.motors_thrust_pwm_t()
+
+    # Test
+    cffirmware.powerDistributionCap(input, actual)
+
+    # Assert
+    # control.thrust will be at a (tuned) hover-state
+    assert actual.motors.m1 == input.motors.m1
+    assert actual.motors.m2 == input.motors.m2
+    assert actual.motors.m3 == input.motors.m3
+    assert actual.motors.m4 == input.motors.m4
+
+def test_power_distribution_cap_when_all_negative():
+    # Fixture
+    input = cffirmware.motors_thrust_uncapped_t()
+    input.motors.m1 = -1000
+    input.motors.m2 = -2000
+    input.motors.m3 = -3000
+    input.motors.m4 = -4000
+
+    actual = cffirmware.motors_thrust_pwm_t()
+
+    # Test
+    cffirmware.powerDistributionCap(input, actual)
+
+    # Assert
+    assert actual.motors.m1 == 0
+    assert actual.motors.m2 == 0
+    assert actual.motors.m3 == 0
+    assert actual.motors.m4 == 0
+
+def test_power_distribution_cap_when_all_above_range():
+    # Fixture
+    input = cffirmware.motors_thrust_uncapped_t()
+    input.motors.m1 = 0xffff + 1
+    input.motors.m2 = 0xffff + 1
+    input.motors.m3 = 0xffff + 1
+    input.motors.m4 = 0xffff + 1
+
+    actual = cffirmware.motors_thrust_pwm_t()
+
+    # Test
+    cffirmware.powerDistributionCap(input, actual)
+
+    # Assert
+    assert actual.motors.m1 == 0xffff
+    assert actual.motors.m2 == 0xffff
+    assert actual.motors.m3 == 0xffff
+    assert actual.motors.m4 == 0xffff
+
+def test_power_distribution_cap_reduces_thrust_equally_much():
+    # Fixture
+    input = cffirmware.motors_thrust_uncapped_t()
+    input.motors.m1 = 0xffff + 1
+    input.motors.m2 = 0xffff + 5
+    input.motors.m3 = 0xffff + 10
+    input.motors.m4 = 0xffff + 15
+
+    actual = cffirmware.motors_thrust_pwm_t()
+
+    # Test
+    cffirmware.powerDistributionCap(input, actual)
+
+    # Assert
+    assert actual.motors.m1 == 0xffff - 14
+    assert actual.motors.m2 == 0xffff - 10
+    assert actual.motors.m3 == 0xffff - 5
+    assert actual.motors.m4 == 0xffff - 0
+
+def test_power_distribution_cap_reduces_thrust_equally_much_with_lower_cap():
+    # Fixture
+    input = cffirmware.motors_thrust_uncapped_t()
+    input.motors.m1 = 0
+    input.motors.m2 = 5
+    input.motors.m3 = 1000
+    input.motors.m4 = 0xffff + 10
+
+    actual = cffirmware.motors_thrust_pwm_t()
+
+    # Test
+    cffirmware.powerDistributionCap(input, actual)
+
+    # Assert
+    assert actual.motors.m1 == 0
+    assert actual.motors.m2 == 0
+    assert actual.motors.m3 == 1000 - 10
+    assert actual.motors.m4 == 0xffff

From a0afba5b23f81db87ea6ec8f43d862724c2e65ac Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Wed, 9 Nov 2022 16:24:24 +0100
Subject: [PATCH 6/7] Corrected log names

---
 src/modules/src/stabilizer.c | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

diff --git a/src/modules/src/stabilizer.c b/src/modules/src/stabilizer.c
index 379452cffd..455ca2e8c3 100644
--- a/src/modules/src/stabilizer.c
+++ b/src/modules/src/stabilizer.c
@@ -821,23 +821,23 @@ LOG_GROUP_START(motor)
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m1, &motorThrustBatCompUncapped.motors.m1)
+LOG_ADD(LOG_INT32, m1req, &motorThrustBatCompUncapped.motors.m1)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m2, &motorThrustBatCompUncapped.motors.m2)
+LOG_ADD(LOG_INT32, m2req, &motorThrustBatCompUncapped.motors.m2)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m3, &motorThrustBatCompUncapped.motors.m3)
+LOG_ADD(LOG_INT32, m3req, &motorThrustBatCompUncapped.motors.m3)
 
 /**
  * @brief Requested motor power for m1, including battery compensation. Same scale as the motor PWM but uncapped
  * and may have values outside the [0 - UINT16_MAX] range.
  */
-LOG_ADD(LOG_INT32, m4, &motorThrustBatCompUncapped.motors.m4)
+LOG_ADD(LOG_INT32, m4req, &motorThrustBatCompUncapped.motors.m4)
 LOG_GROUP_STOP(motor)

From 3a2166d525161b9145dac0b755227ffe8bad8f28 Mon Sep 17 00:00:00 2001
From: Kristoffer Richardsson <kristoffer@bitcraze.io>
Date: Thu, 10 Nov 2022 13:58:28 +0100
Subject: [PATCH 7/7] Corrected comment

---
 src/modules/src/power_distribution_flapper.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/src/power_distribution_flapper.c b/src/modules/src/power_distribution_flapper.c
index 9f0faf2f18..848daaa259 100644
--- a/src/modules/src/power_distribution_flapper.c
+++ b/src/modules/src/power_distribution_flapper.c
@@ -176,7 +176,7 @@ void powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUnca
     motorPwm->motors.m2 = limitThrust(motorThrustBatCompUncapped->motors.m2, 0, UINT16_MAX); // pitch servo
     motorPwm->motors.m3 = limitThrust(motorThrustBatCompUncapped->motors.m3, 0, UINT16_MAX); // yaw servo
     motorPwm->motors.m1 = limitThrust(motorThrustBatCompUncapped->motors.m1, idleThrust, UINT16_MAX); // left motor
-    motorPwm->motors.m4 = limitThrust(motorThrustBatCompUncapped->motors.m4, idleThrust, UINT16_MAX); // left motor
+    motorPwm->motors.m4 = limitThrust(motorThrustBatCompUncapped->motors.m4, idleThrust, UINT16_MAX); // right motor
   #else
     motorPwm->motors.m1 = limitThrust(motorThrustBatCompUncapped->motors.m1, 0, UINT16_MAX); // pitch servo
     motorPwm->motors.m3 = limitThrust(motorThrustBatCompUncapped->motors.m3, 0, UINT16_MAX); // yaw servo