mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-24 00:35:39 +03:00
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.
This commit is contained in:
Dominic Clifton
parent
49781b5080
commit
10279c0178
6 changed files with 225 additions and 66 deletions
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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()) {
|
||||
|
|
|
|||
|
|
@ -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 $@
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
|
|||
141
test/telemetry_hott_unittest.cc
Normal file
141
test/telemetry_hott_unittest.cc
Normal file
|
|
@ -0,0 +1,141 @@
|
|||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#include <limits.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
Loading…
Add table
Add a link
Reference in a new issue