Merge pull request #1543 from FinlaySanders/master

Implements powerDistributionForce for force control mode

Author: tobbeanton

src/modules/src/power_distribution_quadrotor.c

@@ -116,8 +116,27 @@ static void powerDistributionForceTorque(const control_t *control, motors_thrust
   }
 }
 
+/**
+ * @brief Allows for direct control of motor power with clipping
+ *
+ * This function applies clipping to the motor values, which is different to
+ * the "capping" behaviour found in powerDistributionForceTorque() - which
+ * instead prioritizes stability rather than thrust.
+ */
 static void powerDistributionForce(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped) {
-  // Not implemented yet
+  for (int i = 0; i < STABILIZER_NR_OF_MOTORS; i++) {
+    float f = control->normalizedForces[i];
+
+    if (f < 0.0f) {
+      f = 0.0f;
+    }
+
+    if (f > 1.0f) {
+      f = 1.0f;
+    }
+
+    motorThrustUncapped->list[i] = f * UINT16_MAX;
+  }
 }
 
 void powerDistribution(const control_t *control, motors_thrust_uncapped_t* motorThrustUncapped)
@@ -195,4 +214,4 @@ PARAM_GROUP_START(powerDist)
  * common value is between 3000 - 6000.
  */
 PARAM_ADD_CORE(PARAM_UINT32 | PARAM_PERSISTENT, idleThrust, &idleThrust)
-PARAM_GROUP_STOP(powerDist)
+PARAM_GROUP_STOP(powerDist)