Better handling of motor power distribution

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

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__ */

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,27 +459,16 @@ 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];
     }
 
+    motor_ratios[id] = ratio;
+
     if (motorMap[id]->drvType == BRUSHLESS)
     {
 #ifdef CONFIG_MOTORS_ESC_PROTOCOL_DSHOT
@@ -710,51 +705,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)

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, 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
+ * 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(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm);
 
 /**
  * Returns a 1 when motor 'id' gives thrust, returns 0 otherwise

src/modules/interface/stabilizer_types.h

@@ -174,12 +174,28 @@ 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;
+
+#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,

src/modules/src/power_distribution_flapper.c

@@ -104,7 +104,7 @@ int powerDistributionMotorType(uint32_t id)
 {
   int type = 1;
   if (id == idPitch || id == idYaw)
-  { 
+  {
     type = 0;
   }
 
@@ -114,26 +114,26 @@ 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
     DEBUG_PRINT("Using Flapper power distribution | PCB revB (2021)\n");
   #else
     DEBUG_PRINT("Using Flapper power distribution | PCB revD (2022) or newer\n");
   #endif
-  
+
 }
 
 bool powerDistributionTest(void)
@@ -142,40 +142,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, motors_thrust_uncapped_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->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->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(const motors_thrust_uncapped_t* motorThrustBatCompUncapped, motors_thrust_pwm_t* motorPwm)
+{
+  #if CONFIG_POWER_DISTRIBUTION_FLAPPER_REVB
+    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); // right 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->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
 }
 
@@ -202,7 +208,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 +223,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)