src/test/unit/altitude_hold_unittest.cc

   1 /*
   2  * This file is part of Cleanflight.
   3  *
   4  * Cleanflight is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * Cleanflight is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdint.h>
  19 #include <stdbool.h>
  20
  21 #include <limits.h>
  22
  23 //#define DEBUG_ALTITUDE_HOLD
  24
  25 #define BARO
  26
  27 extern "C" {
  28     #include "debug.h"
  29
  30     #include "common/axis.h"
  31     #include "common/maths.h"
  32
  33     #include "drivers/sensor.h"
  34     #include "drivers/accgyro.h"
  35
  36     #include "sensors/sensors.h"
  37     #include "sensors/acceleration.h"
  38     #include "sensors/barometer.h"
  39
  40     #include "io/escservo.h"
  41     #include "io/rc_controls.h"
  42
  43     #include "rx/rx.h"
  44
  45     #include "flight/mixer.h"
  46     #include "flight/pid.h"
  47     #include "flight/imu.h"
  48     #include "flight/altitudehold.h"
  49
  50     #include "config/runtime_config.h"
  51
  52 }
  53
  54 #include "unittest_macros.h"
  55 #include "gtest/gtest.h"
  56
  57 #define DOWNWARDS_THRUST true
  58 #define UPWARDS_THRUST false
  59
  60
  61 extern "C" {
  62     bool isThrustFacingDownwards(rollAndPitchInclination_t *inclinations);
  63     uint16_t calculateTiltAngle(rollAndPitchInclination_t *inclinations);
  64 }
  65
  66 typedef struct inclinationExpectation_s {
  67     rollAndPitchInclination_t inclination;
  68     bool expectDownwardsThrust;
  69 } inclinationExpectation_t;
  70
  71 TEST(AltitudeHoldTest, IsThrustFacingDownwards)
  72 {
  73     // given
  74
  75     inclinationExpectation_t inclinationExpectations[] = {
  76             { {{    0,    0 }}, DOWNWARDS_THRUST },
  77             { {{  799,  799 }}, DOWNWARDS_THRUST },
  78             { {{  800,  799 }}, UPWARDS_THRUST },
  79             { {{  799,  800 }}, UPWARDS_THRUST },
  80             { {{  800,  800 }}, UPWARDS_THRUST },
  81             { {{  801,  801 }}, UPWARDS_THRUST },
  82             { {{ -799, -799 }}, DOWNWARDS_THRUST },
  83             { {{ -800, -799 }}, UPWARDS_THRUST },
  84             { {{ -799, -800 }}, UPWARDS_THRUST },
  85             { {{ -800, -800 }}, UPWARDS_THRUST },
  86             { {{ -801, -801 }}, UPWARDS_THRUST }
  87     };
  88     uint8_t testIterationCount = sizeof(inclinationExpectations) / sizeof(inclinationExpectation_t);
  89
  90     // expect
  91
  92     for (uint8_t index = 0; index < testIterationCount; index ++) {
  93         inclinationExpectation_t *angleInclinationExpectation = &inclinationExpectations[index];
  94 #ifdef DEBUG_ALTITUDE_HOLD
  95         printf("iteration: %d\n", index);
  96 #endif
  97         bool result = isThrustFacingDownwards(&angleInclinationExpectation->inclination);
  98         EXPECT_EQ(angleInclinationExpectation->expectDownwardsThrust, result);
  99     }
 100 }
 101
 102 typedef struct inclinationAngleExpectations_s {
 103     rollAndPitchInclination_t inclination;
 104     uint16_t expected_angle;
 105 } inclinationAngleExpectations_t;
 106
 107 TEST(AltitudeHoldTest, TestCalculateTiltAngle)
 108 {
 109     inclinationAngleExpectations_t inclinationAngleExpectations[] = {
 110         { {{ 0,  0}}, 0},
 111         { {{ 1,  0}}, 1},
 112         { {{ 0,  1}}, 1},
 113         { {{ 0, -1}}, 1},
 114         { {{-1,  0}}, 1},
 115         { {{-1, -2}}, 2},
 116         { {{-2, -1}}, 2},
 117         { {{ 1,  2}}, 2},
 118         { {{ 2,  1}}, 2}
 119     };
 120
 121     rollAndPitchInclination_t inclination = {{0, 0}};
 122     uint16_t tilt_angle = calculateTiltAngle(&inclination);
 123     EXPECT_EQ(tilt_angle, 0);
 124
 125     for (uint8_t i = 0; i < 9; i++) {
 126         inclinationAngleExpectations_t *expectation = &inclinationAngleExpectations[i];
 127         uint16_t result = calculateTiltAngle(&expectation->inclination);
 128         EXPECT_EQ(expectation->expected_angle, result);
 129     }
 130 }
 131
 132 // STUBS
 133
 134 extern "C" {
 135 uint32_t rcModeActivationMask;
 136 int16_t rcCommand[4];
 137 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 138
 139 uint32_t accTimeSum ;        // keep track for integration of acc
 140 int accSumCount;
 141 float accVelScale;
 142
 143 rollAndPitchInclination_t inclination;
 144
 145 //uint16_t acc_1G;
 146 //int16_t heading;
 147 //gyro_t gyro;
 148 int32_t accSum[XYZ_AXIS_COUNT];
 149 //int16_t magADC[XYZ_AXIS_COUNT];
 150 int32_t BaroAlt;
 151 int16_t debug[DEBUG16_VALUE_COUNT];
 152
 153 uint8_t stateFlags;
 154 uint16_t flightModeFlags;
 155 uint8_t armingFlags;
 156
 157 int32_t sonarAlt;
 158
 159
 160 uint16_t enableFlightMode(flightModeFlags_e mask)
 161 {
 162     return flightModeFlags |= (mask);
 163 }
 164
 165 uint16_t disableFlightMode(flightModeFlags_e mask)
 166 {
 167     return flightModeFlags &= ~(mask);
 168 }
 169
 170 void gyroUpdate(void) {};
 171 bool sensors(uint32_t mask)
 172 {
 173     UNUSED(mask);
 174     return false;
 175 };
 176 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 177 {
 178     UNUSED(rollAndPitchTrims);
 179 }
 180
 181 void imuResetAccelerationSum(void) {};
 182
 183 int32_t applyDeadband(int32_t, int32_t) { return 0; }
 184 uint32_t micros(void) { return 0; }
 185 bool isBaroCalibrationComplete(void) { return true; }
 186 void performBaroCalibrationCycle(void) {}
 187 int32_t baroCalculateAltitude(void) { return 0; }
 188 int constrain(int amt, int low, int high)
 189 {
 190     UNUSED(amt);
 191     UNUSED(low);
 192     UNUSED(high);
 193     return 0;
 194 }
 195
 196 }