From ea15615d9d29fc16231a8f26e537276fb78f488b Mon Sep 17 00:00:00 2001 From: Bengt Date: Tue, 3 May 2016 18:15:04 +0200 Subject: [PATCH 1/4] A simpler calculation of tri_tail_motor_thrustfactor --- src/main/flight/mixer_tricopter.c | 32 ++++--------------------------- 1 file changed, 4 insertions(+), 28 deletions(-) diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index 4f4fa46e236..2a32c2095d3 100644 --- a/src/main/flight/mixer_tricopter.c +++ b/src/main/flight/mixer_tricopter.c @@ -499,38 +499,14 @@ 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 > 0.5f && averageServoAngle < 40.f) { // TODO: Too wide? or totally unnecessary? + averageServoAngle *= RAD; + gpMixerConfig->tri_tail_motor_thrustfactor = rint(10.0 / tan(averageServoAngle)); saveConfigAndNotify(); pTT->state = TT_DONE; - } - else - { + } else { pTT->state = TT_FAIL; } pTT->timestamp_ms = millis(); From 34de21219f435640d41e7a1afcdd34406ed7e212 Mon Sep 17 00:00:00 2001 From: Bengt Date: Tue, 3 May 2016 22:45:29 +0200 Subject: [PATCH 2/4] Fixed coordinate bug --- src/main/flight/mixer_tricopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index 2a32c2095d3..b3861c61fc3 100644 --- a/src/main/flight/mixer_tricopter.c +++ b/src/main/flight/mixer_tricopter.c @@ -498,7 +498,7 @@ STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool case TT_WAIT_FOR_DISARM: if (!ARMING_FLAG(ARMED)) { - float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf; + float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf - 90.0f; if (averageServoAngle > 0.5f && averageServoAngle < 40.f) { // TODO: Too wide? or totally unnecessary? averageServoAngle *= RAD; gpMixerConfig->tri_tail_motor_thrustfactor = rint(10.0 / tan(averageServoAngle)); From 941dfb019e5ad861d0b30cacfd097bce61d7804a Mon Sep 17 00:00:00 2001 From: Bengt Date: Wed, 4 May 2016 21:23:35 +0200 Subject: [PATCH 3/4] Change limits and make more clear --- src/main/flight/mixer_tricopter.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index b3861c61fc3..635edb9bed3 100644 --- a/src/main/flight/mixer_tricopter.c +++ b/src/main/flight/mixer_tricopter.c @@ -498,8 +498,9 @@ STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool case TT_WAIT_FOR_DISARM: if (!ARMING_FLAG(ARMED)) { - float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf - 90.0f; - if (averageServoAngle > 0.5f && averageServoAngle < 40.f) { // TODO: Too wide? or totally unnecessary? + float averageServoAngle = pTT->servoAvgAngle.sum / 10.0f / pTT->servoAvgAngle.numOf; + if (averageServoAngle > 90.5f && averageServoAngle < 130.f) { + averageServoAngle -= 90.0f; averageServoAngle *= RAD; gpMixerConfig->tri_tail_motor_thrustfactor = rint(10.0 / tan(averageServoAngle)); From c37371edda3678619be6ebb2a937cc1cc789b5c6 Mon Sep 17 00:00:00 2001 From: Bengt Date: Thu, 5 May 2016 11:48:34 +0200 Subject: [PATCH 4/4] 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 --- src/main/flight/mixer_tricopter.c | 8 +++++--- src/test/unit/mixer_tricopter_unittest.cc | 21 +++++++++++++++++++++ 2 files changed, 26 insertions(+), 3 deletions(-) diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index 635edb9bed3..19c59ef7273 100644 --- 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 index 642a9eced2e..0a2bb15ad1b 100644 --- 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" {