src/test/unit/navigation_unittest.cc.txt

   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
  20 #include <limits.h>
  21
  22 #define SERIAL_PORT_COUNT   1
  23
  24 extern "C" {
  25     #include "debug.h"
  26
  27     #include "common/axis.h"
  28     #include "common/color.h"
  29     #include "common/maths.h"
  30
  31     #include "drivers/sensor.h"
  32     #include "drivers/accgyro.h"
  33     #include "drivers/serial.h"
  34
  35     //#include "drivers/pwm_rx.h"
  36     typedef enum {
  37         INPUT_FILTERING_DISABLED = 0,
  38         INPUT_FILTERING_ENABLED
  39     } inputFilteringMode_e;
  40
  41
  42     #include "sensors/sensors.h"
  43     #include "sensors/acceleration.h"
  44     #include "sensors/barometer.h"
  45     #include "sensors/gyro.h"
  46
  47     #include "sensors/battery.h"
  48     #include "sensors/boardalignment.h"
  49
  50     #include "io/escservo.h"
  51     #include "io/rc_controls.h"
  52     #include "io/serial.h"
  53
  54     #include "telemetry/telemetry.h"
  55
  56     #include "rx/rx.h"
  57
  58     #include "flight/pid.h"
  59     #include "flight/imu.h"
  60     #include "flight/mixer.h"
  61     #include "flight/failsafe.h"
  62     #include "flight/gps_conversion.h"
  63     #include "flight/navigation_rewrite.h"
  64
  65     #include "config/runtime_config.h"
  66     #include "config/config.h"
  67     #include "config/config_profile.h"
  68     #include "config/config_master.h"
  69 }
  70
  71 #include "unittest_macros.h"
  72 #include "gtest/gtest.h"
  73
  74 typedef struct coordConversionExpectation_s {
  75     gpsLocation_t llh;
  76     t_fp_vector pos;
  77 } coordConversionExpectation_t;
  78
  79 TEST(NavigationTest, CoordinateConversion)
  80 {
  81     // given
  82     coordConversionExpectation_t testExpectations[] = {
  83         { {505498090, 1370165690, 10},      {0.0f, 0.0f, 0.0f} },    // this would be origin
  84         { {505498100, 1370165700, 20},      {11.131949f, 7.0733223f, 10.0f} },
  85         { {505498080, 1370165680, 0},       {-11.131949f, -7.0733223f, -10.0f} },
  86     };
  87
  88     uint8_t testIterationCount = sizeof(testExpectations) / sizeof(testExpectations[0]);
  89     gpsOrigin_s origin;
  90     origin.valid = false;
  91
  92     // expect
  93     for (uint8_t index = 0; index < testIterationCount; index ++) {
  94         coordConversionExpectation_t * testExpectation = &testExpectations[index];
  95
  96         t_fp_vector pos;
  97         gpsConvertGeodeticToLocal(&origin, &testExpectation->llh, &pos);
  98
  99         EXPECT_FLOAT_EQ(testExpectation->pos.V.X, pos.V.X);
 100         EXPECT_FLOAT_EQ(testExpectation->pos.V.Y, pos.V.Y);
 101         EXPECT_FLOAT_EQ(testExpectation->pos.V.Z, pos.V.Z);
 102
 103         gpsLocation_t llh;
 104         gpsConvertLocalToGeodetic(&origin, &testExpectation->pos, &llh);
 105
 106         EXPECT_EQ(testExpectation->llh.lat, llh.lat);
 107         EXPECT_EQ(testExpectation->llh.lon, llh.lon);
 108         EXPECT_EQ(testExpectation->llh.alt, llh.alt);
 109     }
 110
 111 }
 112
 113
 114
 115
 116 // STUBS
 117 extern "C" {
 118
 119 master_t masterConfig;
 120
 121 uint32_t rcModeActivationMask;
 122 int16_t rcCommand[4];
 123 int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT];
 124
 125 uint32_t accTimeSum ;        // keep track for integration of acc
 126 int accSumCount;
 127 float accVelScale;
 128
 129 uint16_t acc_1G;
 130 int16_t heading;
 131 gyro_t gyro;
 132
 133 int32_t accSum[XYZ_AXIS_COUNT];
 134 int16_t accADC[XYZ_AXIS_COUNT];
 135 int16_t gyroADC[XYZ_AXIS_COUNT];
 136 int16_t magADC[XYZ_AXIS_COUNT];
 137 int32_t debug[DEBUG32_VALUE_COUNT];
 138
 139 uint8_t stateFlags;
 140 uint16_t flightModeFlags;
 141 uint8_t armingFlags;
 142
 143 uint8_t GPS_numSat;
 144
 145 bool persistentFlag(uint8_t mask)
 146 {
 147     UNUSED(mask);
 148     return true;
 149 }
 150
 151 uint16_t enableFlightMode(flightModeFlags_e mask)
 152 {
 153     return flightModeFlags |= (mask);
 154 }
 155
 156 uint16_t disableFlightMode(flightModeFlags_e mask)
 157 {
 158     return flightModeFlags &= ~(mask);
 159 }
 160
 161 void gyroUpdate(void) {};
 162
 163 bool sensors(uint32_t mask)
 164 {
 165     UNUSED(mask);
 166     return false;
 167 };
 168
 169 void updateAccelerationReadings(rollAndPitchTrims_t *rollAndPitchTrims)
 170 {
 171     UNUSED(rollAndPitchTrims);
 172 }
 173
 174 uint32_t micros(void) { return 0; }
 175
 176 bool isBaroCalibrationComplete(void) { return true; }
 177
 178 int32_t baroCalculateAltitude(void) { return 0; }
 179 }