Adding a parameter to make tilt compensation configurable.

This closes #106
bitcraze · May 22, 2016 · cadd553 · cadd553
1 parent 6a6b368
commit cadd553
Showing 1 changed file with 15 additions and 1 deletion.
diff --git a/src/modules/src/controller_pid.c b/src/modules/src/controller_pid.c
@@ -7,10 +7,13 @@
 #include "position_controller.h"
 
 #include "log.h"
+#include "param.h"
 
 #define ATTITUDE_RATE RATE_500_HZ
 #define POSITION_RATE RATE_100_HZ
 
+static bool tiltCompensationEnabled = true;
+
 static attitude_t attitudeDesired;
 static attitude_t rateDesired;
 static float actuatorThrust;
@@ -83,7 +86,14 @@ void stateController(control_t *control, const sensorData_t *sensors,
     control->yaw = -control->yaw;
   }
 
-  control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
+  if (tiltCompensationEnabled)
+  {
+    control->thrust = actuatorThrust / sensfusion6GetInvThrustCompensationForTilt();
+  }
+  else
+  {
+    control->thrust = actuatorThrust;
+  }
 
   if (control->thrust == 0)
   {
@@ -106,3 +116,7 @@ LOG_ADD(LOG_FLOAT, roll, &attitudeDesired.roll)
 LOG_ADD(LOG_FLOAT, pitch, &attitudeDesired.pitch)
 LOG_ADD(LOG_FLOAT, yaw, &attitudeDesired.yaw)
 LOG_GROUP_STOP(controller)
+
+PARAM_GROUP_START(controller)
+PARAM_ADD(PARAM_UINT8, tiltComp, &tiltCompensationEnabled)
+PARAM_GROUP_STOP(controller)