Merge pull request #28 from Bengt-M/SimplerThrustTorque

A simpler calculation of tri_tail_motor_thrustfactor
lkaino · May 5, 2016 · 961cbfd · 961cbfd
2 parents ce7c9be + c37371e
commit 961cbfd
Show file tree

Hide file tree

Showing 2 changed files with 25 additions and 25 deletions.
diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c
@@ -499,31 +499,10 @@ STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool
         if (!ARMING_FLAG(ARMED))
         {
             float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf;
-
-            // Find out the factor that gives least yaw force at the average angle
-            float factor = TAIL_THRUST_FACTOR_MIN_FLOAT;
-            const float angleRad = DEGREES_TO_RADIANS(averageServoAngle);
-            float minAbsForce = FLT_MAX;
-            float minFactor = TAIL_THRUST_FACTOR_MIN_FLOAT;
-            _Bool done = false;
-            for (factor = TAIL_THRUST_FACTOR_MIN_FLOAT; (done == false) && (factor < TAIL_THRUST_FACTOR_MAX_FLOAT); factor += 0.1f)
-            {
-                float absForceAtAngle = fabsf(-factor * cosf(angleRad) - sinf(angleRad)) * getPitchCorrectionAtTailAngle(angleRad, factor);
-
-                if (absForceAtAngle < minAbsForce)
-                {
-                    minAbsForce = absForceAtAngle;
-                    minFactor = factor;
-                }
-                else
-                {
-                    done = true;
-                }
-            }
-
-            if (done && minFactor > TAIL_THRUST_FACTOR_MIN_FLOAT + 0.01f)
-            {
-                gpMixerConfig->tri_tail_motor_thrustfactor = minFactor * 10.0f;
+            if (averageServoAngle > 90.5f && averageServoAngle < 120.f) {
+                averageServoAngle -= 90.0f;
+                averageServoAngle *= RAD;
+                gpMixerConfig->tri_tail_motor_thrustfactor = 10.0f * cos_approx(averageServoAngle) / sin_approx(averageServoAngle);
 
                 saveConfigAndNotify();
 

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" {