Changed limit check and added tests for err cases

1/tan also replaced by cos_approx/sin_approx to save code size Now it will build and pass the unit test Not flight tested yet
lkaino · May 5, 2016 · c37371e · c37371e
1 parent 941dfb0
commit c37371e
Show file tree

Hide file tree

Showing 2 changed files with 26 additions and 3 deletions.
diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c
@@ -499,15 +499,17 @@ STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool
         if (!ARMING_FLAG(ARMED))
         {
             float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf;
-            if (averageServoAngle > 90.5f && averageServoAngle < 130.f) {
+            if (averageServoAngle > 90.5f && averageServoAngle < 120.f) {
                 averageServoAngle -= 90.0f;
                 averageServoAngle *= RAD;
-                gpMixerConfig->tri_tail_motor_thrustfactor = rint(10.0 / tan(averageServoAngle));
+                gpMixerConfig->tri_tail_motor_thrustfactor = 10.0f * cos_approx(averageServoAngle) / sin_approx(averageServoAngle);
 
                 saveConfigAndNotify();
 
                 pTT->state = TT_DONE;
-            } else {
+            }
+            else
+            {
                 pTT->state = TT_FAIL;
             }
             pTT->timestamp_ms = millis();

diff --git a/src/test/unit/mixer_tricopter_unittest.cc b/src/test/unit/mixer_tricopter_unittest.cc
@@ -73,6 +73,7 @@ class ThrustFactorCalculationTest: public ::testing::Test {
             servo[i] = DEFAULT_SERVO_MIDDLE;
         }
 
+        mixerConfig.tri_tail_motor_thrustfactor = 123; // so we can check it's unchanged on TT_FAIL
         triInitMixer(&servoConf, &servo[5], &mixerConfig);
         tailTune.mode = TT_MODE_THRUST_TORQUE;
         tailTune.tt.state = TT_WAIT_FOR_DISARM;
@@ -120,6 +121,26 @@ TEST_F(ThrustFactorCalculationTest, 80) {
     EXPECT_EQ(tailTune.tt.state, TT_DONE);
 }
 
+TEST_F(ThrustFactorCalculationTest, err90) {
+    // given
+    tailTune.tt.servoAvgAngle.sum = 270000;
+    // and
+    tailTuneModeThrustTorque(&tailTune.tt, true);
+    // then
+    EXPECT_EQ(123, mixerConfig.tri_tail_motor_thrustfactor);
+    EXPECT_EQ(tailTune.tt.state, TT_FAIL);
+}
+
+TEST_F(ThrustFactorCalculationTest, err130) {
+    // given
+    tailTune.tt.servoAvgAngle.sum = 390000;
+    // and
+    tailTuneModeThrustTorque(&tailTune.tt, true);
+    // then
+    EXPECT_EQ(123, mixerConfig.tri_tail_motor_thrustfactor);
+    EXPECT_EQ(tailTune.tt.state, TT_FAIL);
+}
+
 //STUBS
 extern "C" {