Merge pull request #1384 from bitcraze/toverumar/mutual_excl_idlethrust

Toverumar/mutual excl idlethrust
bitcraze · Jun 11, 2024 · d8fd72f · d8fd72f
2 parents fe48d59 + aa03faa
commit d8fd72f
Showing 1 changed file with 18 additions and 3 deletions.
diff --git a/src/modules/src/power_distribution_quadrotor.c b/src/modules/src/power_distribution_quadrotor.c
@@ -24,9 +24,11 @@
  * power_distribution_quadrotor.c - Crazyflie stock power distribution code
  */
 
+
 #include "power_distribution.h"
 
 #include <string.h>
+#include "debug.h"
 #include "log.h"
 #include "param.h"
 #include "num.h"
@@ -35,6 +37,9 @@
 #include "math.h"
 #include "platform_defaults.h"
 
+#if (!defined(CONFIG_MOTORS_REQUIRE_ARMING) || (CONFIG_MOTORS_REQUIRE_ARMING == 0)) && defined(CONFIG_MOTORS_DEFAULT_IDLE_THRUST) && (CONFIG_MOTORS_DEFAULT_IDLE_THRUST > 0)
+    #error "CONFIG_MOTORS_REQUIRE_ARMING must be defined and not set to 0 if CONFIG_MOTORS_DEFAULT_IDLE_THRUST is greater than 0"
+#endif
 #ifndef CONFIG_MOTORS_DEFAULT_IDLE_THRUST
 #  define DEFAULT_IDLE_THRUST 0
 #else
@@ -63,6 +68,11 @@ uint16_t powerDistributionStopRatio(uint32_t id)
 
 void powerDistributionInit(void)
 {
+  #if (!defined(CONFIG_MOTORS_REQUIRE_ARMING) || (CONFIG_MOTORS_REQUIRE_ARMING == 0))
+  if(idleThrust > 0) {
+    DEBUG_PRINT("WARNING: idle thrust will be overridden with value 0. Autoarming can not be on while idle thrust is higher than 0. If you want to use idle thust please use use arming\n");
+  }
+  #endif
 }
 
 bool powerDistributionTest(void)
@@ -163,14 +173,19 @@ bool powerDistributionCap(const motors_thrust_uncapped_t* motorThrustBatCompUnca
   for (int motorIndex = 0; motorIndex < STABILIZER_NR_OF_MOTORS; motorIndex++)
   {
     int32_t thrustCappedUpper = motorThrustBatCompUncapped->list[motorIndex] - reduction;
-    motorPwm->list[motorIndex] = capMinThrust(thrustCappedUpper, idleThrust);
+    motorPwm->list[motorIndex] = capMinThrust(thrustCappedUpper, powerDistributionGetIdleThrust());
   }
 
   return isCapped;
 }
 
-uint32_t powerDistributionGetIdleThrust() {
-  return idleThrust;
+uint32_t powerDistributionGetIdleThrust()
+{
+  int32_t thrust = idleThrust;
+  #if (!defined(CONFIG_MOTORS_REQUIRE_ARMING) || (CONFIG_MOTORS_REQUIRE_ARMING == 0))
+    thrust = 0;
+  #endif
+  return thrust;
 }
 
 float powerDistributionGetMaxThrust() {