src/test/unit/telemetry_hott_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 <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include <limits.h>
  23
  24 extern "C" {
  25     #include "platform.h"
  26
  27     #include "common/axis.h"
  28
  29     #include "drivers/system.h"
  30     #include "drivers/serial.h"
  31
  32     #include "sensors/sensors.h"
  33     #include "sensors/battery.h"
  34
  35     #include "io/serial.h"
  36     #include "io/gps.h"
  37
  38     #include "telemetry/telemetry.h"
  39     #include "telemetry/hott.h"
  40
  41     #include "flight/pid.h"
  42     #include "flight/gps_conversion.h"
  43
  44     #include "config/runtime_config.h"
  45 }
  46
  47 #include "unittest_macros.h"
  48 #include "gtest/gtest.h"
  49
  50 extern "C" {
  51     void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude);
  52 }
  53 // See http://en.wikipedia.org/wiki/Geographic_coordinate_conversion
  54
  55 HOTT_GPS_MSG_t hottGPSMessage;
  56
  57 HOTT_GPS_MSG_t *getGPSMessageForTest(void)
  58 {
  59     memset(&hottGPSMessage, 0, sizeof(hottGPSMessage));
  60     return &hottGPSMessage;
  61 }
  62
  63 TEST(TelemetryHottTest, UpdateGPSCoordinates1)
  64 {
  65     // given
  66     HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
  67
  68     // Mayrhofen, Austria
  69     uint32_t longitude = GPS_coord_to_degrees("4710.5186");
  70     uint32_t latitude = GPS_coord_to_degrees("1151.4252");
  71
  72     // when
  73     addGPSCoordinates(hottGPSMessage, latitude, longitude);
  74
  75     // then
  76     EXPECT_EQ(hottGPSMessage->pos_NS, 0);
  77     EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 1151);
  78     EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L), 4251);
  79
  80     EXPECT_EQ(hottGPSMessage->pos_EW, 0);
  81     EXPECT_EQ(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L, 4710);
  82     EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), 5186);
  83 }
  84
  85 TEST(TelemetryHottTest, UpdateGPSCoordinates2)
  86 {
  87     // given
  88     HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
  89
  90     // Hampstead Heath, London
  91     // 51.563886, -0.159960
  92     uint32_t longitude = GPS_coord_to_degrees("5156.3886");
  93     uint32_t latitude  = -GPS_coord_to_degrees("015.9960");
  94
  95     // when
  96     addGPSCoordinates(hottGPSMessage, longitude, latitude);
  97
  98     // then
  99     EXPECT_EQ(hottGPSMessage->pos_NS, 0);
 100     EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 5156);
 101     EXPECT_EQ(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L, 3886);
 102
 103     EXPECT_EQ(hottGPSMessage->pos_EW, 1);
 104     EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L), -15);
 105     EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), -9960);
 106 }
 107
 108
 109 TEST(TelemetryHottTest, UpdateGPSCoordinates3)
 110 {
 111     // given
 112     HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
 113
 114     int32_t longitude = -GPS_coord_to_degrees("17999.9999");
 115     int32_t latitude = GPS_coord_to_degrees("8999.9999");
 116
 117     // when
 118     addGPSCoordinates(hottGPSMessage, longitude, latitude);
 119
 120     // then
 121     EXPECT_EQ(hottGPSMessage->pos_NS, 1);
 122     EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L), -18039);
 123     EXPECT_EQ((int16_t)(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L), -9999);
 124
 125     EXPECT_EQ(hottGPSMessage->pos_EW, 0);
 126     EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L), 9039);
 127     EXPECT_EQ((int16_t)(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L), 9999);
 128 }
 129
 130 TEST(TelemetryHottTest, PrepareGPSMessage_Altitude1m)
 131 {
 132     // given
 133     HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest();
 134
 135     stateFlags = GPS_FIX;
 136     uint16_t altitudeInMeters = 1;
 137     GPS_altitude = altitudeInMeters * (1 / 0.1f); // 1 = 0.1m
 138
 139     // when
 140     hottPrepareGPSResponse(hottGPSMessage);
 141
 142     // then
 143     EXPECT_EQ((int16_t)(hottGPSMessage->altitude_H << 8 | hottGPSMessage->altitude_L), 1 + HOTT_GPS_ALTITUDE_OFFSET);
 144 }
 145
 146
 147 // STUBS
 148
 149 extern "C" {
 150
 151 int16_t debug[4];
 152
 153 uint8_t stateFlags;
 154
 155
 156 uint8_t GPS_numSat;
 157 int32_t GPS_coord[2];
 158 uint16_t GPS_speed;                 // speed in 0.1m/s
 159 uint16_t GPS_distanceToHome;        // distance to home point in meters
 160 uint16_t GPS_altitude;              // altitude in 0.1m
 161 uint8_t vbat;
 162 int16_t GPS_directionToHome;        // direction to home or hol point in degrees
 163
 164 int32_t amperage;
 165 int32_t mAhDrawn;
 166
 167 uint32_t micros(void) { return 0; }
 168
 169 uint8_t serialTotalBytesWaiting(serialPort_t *instance) {
 170     UNUSED(instance);
 171     return 0;
 172 }
 173
 174 uint8_t serialRead(serialPort_t *instance) {
 175     UNUSED(instance);
 176     return 0;
 177 }
 178
 179 void serialWrite(serialPort_t *instance, uint8_t ch) {
 180     UNUSED(instance);
 181     UNUSED(ch);
 182 }
 183
 184 void serialSetMode(serialPort_t *instance, portMode_t mode) {
 185     UNUSED(instance);
 186     UNUSED(mode);
 187 }
 188
 189 void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) {
 190     UNUSED(instance);
 191     UNUSED(baudRate);
 192 }
 193
 194 void beginSerialPortFunction(serialPort_t *port, serialPortFunction_e function) {
 195     UNUSED(port);
 196     UNUSED(function);
 197 }
 198
 199 void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function) {
 200     UNUSED(port);
 201     UNUSED(function);
 202 }
 203
 204 serialPort_t *openSerialPort(serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion) {
 205     UNUSED(functionMask);
 206     UNUSED(baudRate);
 207     UNUSED(callback);
 208     UNUSED(mode);
 209     UNUSED(inversion);
 210
 211     return NULL;
 212 }
 213
 214 serialPort_t *findOpenSerialPort(uint16_t functionMask) {
 215     UNUSED(functionMask);
 216     return NULL;
 217 }
 218
 219 bool sensors(uint32_t mask) {
 220     UNUSED(mask);
 221     return false;
 222 }
 223
 224 }
 225