From c37371edda3678619be6ebb2a937cc1cc789b5c6 Mon Sep 17 00:00:00 2001 From: Bengt Date: Thu, 5 May 2016 11:48:34 +0200 Subject: [PATCH] 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" {