Closes #625: Implement IDLE motor thrust parameter

bitcraze · Oct 7, 2020 · f061327 · f061327
1 parent 46cf8db
commit f061327
Showing 1 changed file with 23 additions and 0 deletions.
diff --git a/src/modules/src/power_distribution_stock.c b/src/modules/src/power_distribution_stock.c
@@ -51,6 +51,12 @@ static struct {
   uint16_t m4;
 } motorPowerSet;
 
+#ifndef DEFAULT_IDLE_THRUST
+#define DEFAULT_IDLE_THRUST 0
+#endif
+
+static uint32_t idleThrust = DEFAULT_IDLE_THRUST;
+
 void powerDistributionInit(void)
 {
   motorsInit(platformConfigGetMotorMapping());
@@ -104,6 +110,19 @@ void powerDistribution(const control_t *control)
   }
   else
   {
+    if (control->thrust != 0 && motorPower.m1 < idleThrust) {
+      motorPower.m1 = idleThrust;
+    }
+    if (control->thrust != 0 && motorPower.m2 < idleThrust) {
+      motorPower.m2 = idleThrust;
+    }
+    if (control->thrust != 0 && motorPower.m3 < idleThrust) {
+      motorPower.m3 = idleThrust;
+    }
+    if (control->thrust != 0 && motorPower.m4 < idleThrust) {
+      motorPower.m4 = idleThrust;
+    }
+
     motorsSetRatio(MOTOR_M1, motorPower.m1);
     motorsSetRatio(MOTOR_M2, motorPower.m2);
     motorsSetRatio(MOTOR_M3, motorPower.m3);
@@ -119,6 +138,10 @@ PARAM_ADD(PARAM_UINT16, m3, &motorPowerSet.m3)
 PARAM_ADD(PARAM_UINT16, m4, &motorPowerSet.m4)
 PARAM_GROUP_STOP(motorPowerSet)
 
+PARAM_GROUP_START(powerDist)
+PARAM_ADD(PARAM_UINT32, idleThrust, &idleThrust)
+PARAM_GROUP_STOP(powerDist)
+
 LOG_GROUP_START(motor)
 LOG_ADD(LOG_UINT32, m1, &motorPower.m1)
 LOG_ADD(LOG_UINT32, m2, &motorPower.m2)