From 10279c0178d93ca137f79cd222409103c009377d Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sat, 24 May 2014 21:09:07 +0100 Subject: [PATCH] Updated GPS logitude/latitude calculation. The existing implementation that was ported over was trying to work with different values. By writing a unit test for the code that stores values in GPS_coord it was possible to have known values which could then also be used to write a unit test for the HoTT telemetry which finally enabled production code to be written. Hopefully it will work, unable to test further since my GPS unit is playing up. --- src/drivers/serial_common.c | 2 +- src/drivers/serial_common.h | 10 +-- src/telemetry_hott.c | 113 +++++++++++++------------ test/Makefile | 13 ++- test/gps_conversion_unittest.cc | 12 +-- test/telemetry_hott_unittest.cc | 141 ++++++++++++++++++++++++++++++++ 6 files changed, 225 insertions(+), 66 deletions(-) create mode 100644 test/telemetry_hott_unittest.cc diff --git a/src/drivers/serial_common.c b/src/drivers/serial_common.c index 766e15080f..d55be05e5d 100644 --- a/src/drivers/serial_common.c +++ b/src/drivers/serial_common.c @@ -43,7 +43,7 @@ bool isSerialTransmitBufferEmpty(serialPort_t *instance) return instance->vTable->isSerialTransmitBufferEmpty(instance); } -inline void serialSetMode(serialPort_t *instance, portMode_t mode) +void serialSetMode(serialPort_t *instance, portMode_t mode) { instance->vTable->setMode(instance, mode); } diff --git a/src/drivers/serial_common.h b/src/drivers/serial_common.h index 04ade08d4a..fa9bab146b 100644 --- a/src/drivers/serial_common.h +++ b/src/drivers/serial_common.h @@ -51,11 +51,11 @@ struct serialPortVTable { void (*setMode)(serialPort_t *instance, portMode_t mode); }; -inline void serialWrite(serialPort_t *instance, uint8_t ch); -inline uint8_t serialTotalBytesWaiting(serialPort_t *instance); -inline uint8_t serialRead(serialPort_t *instance); -inline void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); +void serialWrite(serialPort_t *instance, uint8_t ch); +uint8_t serialTotalBytesWaiting(serialPort_t *instance); +uint8_t serialRead(serialPort_t *instance); +void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate); void serialSetMode(serialPort_t *instance, portMode_t mode); -inline bool isSerialTransmitBufferEmpty(serialPort_t *instance); +bool isSerialTransmitBufferEmpty(serialPort_t *instance); void serialPrint(serialPort_t *instance, const char *str); uint32_t serialGetBaudRate(serialPort_t *instance); diff --git a/src/telemetry_hott.c b/src/telemetry_hott.c index 070af29eca..a4a51b226f 100644 --- a/src/telemetry_hott.c +++ b/src/telemetry_hott.c @@ -68,7 +68,7 @@ extern int16_t debug[4]; #define HOTT_TX_DELAY_US 3000 static uint32_t lastHoTTRequestCheckAt = 0; -static uint32_t lastMessagePreparedAt = 0; +static uint32_t lastMessagesPreparedAt = 0; static bool hottIsSending = false; @@ -119,74 +119,79 @@ static void initialiseMessages(void) initialiseGPSMessage(&hottGPSMessage, sizeof(hottGPSMessage)); } -void hottFormatGPSResponse(void) +void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude) { - hottGPSMessage.gps_satelites = GPS_numSat; + uint8_t deg = latitude / 10000000UL; + uint32_t sec = (latitude - (deg * 10000000UL)) * 6; + uint8_t min = sec / 1000000UL; + sec = (sec % 1000000UL) / 100UL; + uint16_t degMin = (deg * 100) + min; + + hottGPSMessage->pos_NS = (latitude < 0); + hottGPSMessage->pos_NS_dm_L = degMin; + hottGPSMessage->pos_NS_dm_H = degMin >> 8; + hottGPSMessage->pos_NS_sec_L = sec; + hottGPSMessage->pos_NS_sec_H = sec >> 8; + + deg = longitude / 10000000UL; + sec = (longitude - (deg * 10000000UL)) * 6; + min = sec / 1000000UL; + sec = (sec % 1000000UL) / 100UL; + degMin = (deg * 100) + min; + + hottGPSMessage->pos_EW = (longitude < 0); + hottGPSMessage->pos_EW_dm_L = degMin; + hottGPSMessage->pos_EW_dm_H = degMin >> 8; + hottGPSMessage->pos_EW_sec_L = sec; + hottGPSMessage->pos_EW_sec_H = sec >> 8; +} + +void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) +{ + hottGPSMessage->gps_satelites = GPS_numSat; if (!f.GPS_FIX) { - hottGPSMessage.gps_fix_char = GPS_FIX_CHAR_NONE; + hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } if (GPS_numSat >= 5) { - hottGPSMessage.gps_fix_char = GPS_FIX_CHAR_3D; + hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D; } else { - hottGPSMessage.gps_fix_char = GPS_FIX_CHAR_2D; + hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D; } - // latitude - hottGPSMessage.pos_NS = (GPS_coord[LAT] < 0); - uint8_t deg = GPS_coord[LAT] / 100000; - uint32_t sec = (GPS_coord[LAT] - (deg * 100000)) * 6; - uint8_t min = sec / 10000; - sec = sec % 10000; - uint16_t degMin = (deg * 100) + min; - hottGPSMessage.pos_NS_dm_L = degMin; - hottGPSMessage.pos_NS_dm_H = degMin >> 8; - hottGPSMessage.pos_NS_sec_L = sec; - hottGPSMessage.pos_NS_sec_H = sec >> 8; - - // longitude - hottGPSMessage.pos_EW = (GPS_coord[LON] < 0); - deg = GPS_coord[LON] / 100000; - sec = (GPS_coord[LON] - (deg * 100000)) * 6; - min = sec / 10000; - sec = sec % 10000; - degMin = (deg * 100) + min; - hottGPSMessage.pos_EW_dm_L = degMin; - hottGPSMessage.pos_EW_dm_H = degMin >> 8; - hottGPSMessage.pos_EW_sec_L = sec; - hottGPSMessage.pos_EW_sec_H = sec >> 8; + addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]); // GPS Speed in km/h - uint16_t speed = (GPS_speed / 100) * 36; // 0.1m/s * 0.36 = km/h - hottGPSMessage.gps_speed_L = speed & 0x00FF; - hottGPSMessage.gps_speed_H = speed >> 8; + uint16_t speed = (GPS_speed / 100) * 36; // 0->1m/s * 0->36 = km/h + hottGPSMessage->gps_speed_L = speed & 0x00FF; + hottGPSMessage->gps_speed_H = speed >> 8; - hottGPSMessage.home_distance_L = GPS_distanceToHome & 0x00FF; - hottGPSMessage.home_distance_H = GPS_distanceToHome >> 8; + hottGPSMessage->home_distance_L = GPS_distanceToHome & 0x00FF; + hottGPSMessage->home_distance_H = GPS_distanceToHome >> 8; - hottGPSMessage.altitude_L = GPS_altitude & 0x00FF; - hottGPSMessage.altitude_H = GPS_altitude >> 8; + hottGPSMessage->altitude_L = GPS_altitude & 0x00FF; + hottGPSMessage->altitude_H = GPS_altitude >> 8; - hottGPSMessage.home_direction = GPS_directionToHome; + hottGPSMessage->home_direction = GPS_directionToHome; } -static inline void hottEAMUpdateBattery(void) +static inline void hottEAMUpdateBattery(HOTT_EAM_MSG_t *hottEAMMessage) { - hottEAMMessage.main_voltage_L = vbat & 0xFF; - hottEAMMessage.main_voltage_H = vbat >> 8; - hottEAMMessage.batt1_voltage_L = vbat & 0xFF; - hottEAMMessage.batt1_voltage_H = vbat >> 8; + hottEAMMessage->main_voltage_L = vbat & 0xFF; + hottEAMMessage->main_voltage_H = vbat >> 8; + hottEAMMessage->batt1_voltage_L = vbat & 0xFF; + hottEAMMessage->batt1_voltage_H = vbat >> 8; } -void hottFormatEAMResponse(void) +void hottPrepareEAMResponse(HOTT_EAM_MSG_t *hottEAMMessage) { // Reset alarms - hottEAMMessage.warning_beeps = 0x0; - hottEAMMessage.alarm_invers1 = 0x0; + hottEAMMessage->warning_beeps = 0x0; + hottEAMMessage->alarm_invers1 = 0x0; - hottEAMUpdateBattery(); + hottEAMUpdateBattery(hottEAMMessage); } static void hottSerialWrite(uint8_t c) @@ -256,9 +261,9 @@ static inline void hottSendEAMResponse(void) hottSendResponse((uint8_t *)&hottEAMMessage, sizeof(hottEAMMessage)); } -static void hottPrepareMessage(void) { - hottFormatEAMResponse(); - hottFormatGPSResponse(); +static void hottPrepareMessages(void) { + hottPrepareEAMResponse(&hottEAMMessage); + hottPrepareGPSResponse(&hottGPSMessage); } static void processBinaryModeRequest(uint8_t address) { @@ -367,9 +372,9 @@ static void hottSendTelemetryData(void) { hottSerialWrite(*hottMsg++); } -static inline bool shouldPrepareHoTTMessage(uint32_t currentMicros) +static inline bool shouldPrepareHoTTMessages(uint32_t currentMicros) { - return currentMicros - lastMessagePreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ; + return currentMicros - lastMessagesPreparedAt >= HOTT_MESSAGE_PREPARATION_FREQUENCY_5_HZ; } static inline bool shouldCheckForHoTTRequest() @@ -386,9 +391,9 @@ void handleHoTTTelemetry(void) uint32_t now = micros(); - if (shouldPrepareHoTTMessage(now)) { - hottPrepareMessage(); - lastMessagePreparedAt = now; + if (shouldPrepareHoTTMessages(now)) { + hottPrepareMessages(); + lastMessagesPreparedAt = now; } if (shouldCheckForHoTTRequest()) { diff --git a/test/Makefile b/test/Makefile index 64b8bd619f..b8083575f5 100644 --- a/test/Makefile +++ b/test/Makefile @@ -30,7 +30,7 @@ CXXFLAGS += -g -Wall -Wextra -pthread -ggdb -O0 # All tests produced by this Makefile. Remember to add new tests you # created to the list. -TESTS = battery_unittest flight_imu_unittest gps_conversion_unittest +TESTS = battery_unittest flight_imu_unittest gps_conversion_unittest telemetry_hott_unittest # All Google Test headers. Usually you shouldn't change this # definition. @@ -107,3 +107,14 @@ gps_conversion_unittest.o : $(TEST_DIR)/gps_conversion_unittest.cc \ gps_conversion_unittest : gps_conversion.o gps_conversion_unittest.o gtest_main.a $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@ + + +telemetry_hott.o : $(USER_DIR)/telemetry_hott.c $(USER_DIR)/telemetry_hott.h $(GTEST_HEADERS) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(USER_DIR)/telemetry_hott.c -I$(TEST_DIR) + +telemetry_hott_unittest.o : $(TEST_DIR)/telemetry_hott_unittest.cc \ + $(USER_DIR)/telemetry_hott.h $(GTEST_HEADERS) + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -c $(TEST_DIR)/telemetry_hott_unittest.cc -I$(USER_DIR) + +telemetry_hott_unittest : telemetry_hott.o telemetry_hott_unittest.o gps_conversion.o gtest_main.a + $(CXX) $(CPPFLAGS) $(CXXFLAGS) -lpthread $^ -o $@ diff --git a/test/gps_conversion_unittest.cc b/test/gps_conversion_unittest.cc index 4c3d3d53fd..dceb881b8d 100644 --- a/test/gps_conversion_unittest.cc +++ b/test/gps_conversion_unittest.cc @@ -27,11 +27,12 @@ TEST(GpsConversionTest, GPSCoordToDegrees_NMEA_Values) {"000.0", 0}, {"00000.0000", 0}, {"0.0001", 16}, // smallest value - {"25599.9999", 2566666650}, // largest value - {"25599.99999", 2566666650}, // too many fractional digits - {"25699.9999", 16666650}, // overflowed without detection - {"5321.6802", 533613366}, - {"00630.3372", 65056200}, + {"25599.9999", 2566666650UL}, // largest value + {"25599.99999", 2566666650UL}, // too many fractional digits + {"25699.9999", 16666650UL}, // overflowed without detection + {"5128.3727", 514728783UL}, + {"5321.6802", 533613366UL}, + {"00630.3372", 65056200UL}, }; // expect @@ -48,3 +49,4 @@ TEST(GpsConversionTest, GPSCoordToDegrees_NMEA_Values) EXPECT_EQ(result, expectation->degrees); } } + diff --git a/test/telemetry_hott_unittest.cc b/test/telemetry_hott_unittest.cc new file mode 100644 index 0000000000..4622e0d43b --- /dev/null +++ b/test/telemetry_hott_unittest.cc @@ -0,0 +1,141 @@ +#include +#include +#include + +#include + +#include "platform.h" + +#include "common/axis.h" + +#include "drivers/system_common.h" + +#include "drivers/serial_common.h" +#include "serial_common.h" + +#include "runtime_config.h" + +#include "sensors_common.h" + +#include "flight_common.h" +#include "gps_common.h" +#include "battery.h" + +#include "telemetry_common.h" +#include "telemetry_hott.h" + +#include "gps_conversion.h" + + +#include "unittest_macros.h" +#include "gtest/gtest.h" + +void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t longitude); + +// See http://en.wikipedia.org/wiki/Geographic_coordinate_conversion + +HOTT_GPS_MSG_t hottGPSMessage; + +HOTT_GPS_MSG_t *getGPSMessageForTest(void) +{ + memset(&hottGPSMessage, 0, sizeof(hottGPSMessage)); + return &hottGPSMessage; +} + +TEST(TelemetryHottTest, UpdateGPSCoordinates) +{ + // given + HOTT_GPS_MSG_t *hottGPSMessage = getGPSMessageForTest(); + + // Mayrhofen, Austria + uint32_t latitude = GPS_coord_to_degrees("4710.5186"); + uint32_t longitude = GPS_coord_to_degrees("1151.4252"); + + + // when + addGPSCoordinates(hottGPSMessage, longitude, latitude); + + // then + EXPECT_EQ(hottGPSMessage->pos_EW, 0); + EXPECT_EQ(hottGPSMessage->pos_EW_dm_H << 8 | hottGPSMessage->pos_EW_dm_L, 4710); + EXPECT_EQ(hottGPSMessage->pos_EW_sec_H << 8 | hottGPSMessage->pos_EW_sec_L, 5186); + + EXPECT_EQ(hottGPSMessage->pos_NS, 0); + EXPECT_EQ(hottGPSMessage->pos_NS_dm_H << 8 | hottGPSMessage->pos_NS_dm_L, 1151); + EXPECT_EQ(hottGPSMessage->pos_NS_sec_H << 8 | hottGPSMessage->pos_NS_sec_L, 4251); +} + + +// STUBS + +int16_t debug[4]; + +uint8_t GPS_numSat; +flags_t f; +int32_t GPS_coord[2]; +uint16_t GPS_speed; // speed in 0.1m/s +uint16_t GPS_distanceToHome; // distance to home point in meters +uint16_t GPS_altitude; // altitude in 0.1m +uint8_t vbat; +int16_t GPS_directionToHome; // direction to home or hol point in degrees + +uint32_t micros(void) { return 0; } + +uint8_t serialTotalBytesWaiting(serialPort_t *instance) { + UNUSED(instance); + return 0; +} + +uint8_t serialRead(serialPort_t *instance) { + UNUSED(instance); + return 0; +} + +void serialWrite(serialPort_t *instance, uint8_t ch) { + UNUSED(instance); + UNUSED(ch); +} + +void serialSetMode(serialPort_t *instance, portMode_t mode) { + UNUSED(instance); + UNUSED(mode); +} + +void serialSetBaudRate(serialPort_t *instance, uint32_t baudRate) { + UNUSED(instance); + UNUSED(baudRate); +} + +void beginSerialPortFunction(serialPort_t *port, serialPortFunction_e function) { + UNUSED(port); + UNUSED(function); +} + +void endSerialPortFunction(serialPort_t *port, serialPortFunction_e function) { + UNUSED(port); + UNUSED(function); +} + +serialPort_t *openSerialPort(serialPortFunction_e functionMask, serialReceiveCallbackPtr callback, uint32_t baudRate, portMode_t mode, serialInversion_e inversion) { + UNUSED(functionMask); + UNUSED(baudRate); + UNUSED(callback); + UNUSED(mode); + UNUSED(inversion); +} + +serialPort_t *findOpenSerialPort(uint16_t functionMask) { + UNUSED(functionMask); + return NULL; +} + +bool sensors(uint32_t mask) { + UNUSED(mask); + return false; +} + + + + + +