From 564e0c94b835cc3d7622673ad521324db3514a5d Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 25 Jun 2017 06:55:17 +0100 Subject: [PATCH] Move some GPS variables into struct. Some alignment with iNav --- src/main/blackbox/blackbox.c | 23 ++++++------ src/main/fc/fc_core.c | 2 +- src/main/fc/fc_msp.c | 34 ++++++++--------- src/main/flight/imu.c | 4 +- src/main/flight/navigation.c | 47 +++++++++++++----------- src/main/io/beeper.c | 4 +- src/main/io/dashboard.c | 8 ++-- src/main/io/gps.c | 40 ++++++++------------ src/main/io/gps.h | 24 ++++++++---- src/main/io/ledstrip.c | 4 +- src/main/io/osd.c | 10 ++--- src/main/target/COLIBRI_RACE/i2c_bst.c | 14 +++---- src/main/telemetry/crsf.c | 16 ++++---- src/main/telemetry/frsky.c | 17 +++++---- src/main/telemetry/hott.c | 12 +++--- src/main/telemetry/ltm.c | 14 +++---- src/main/telemetry/mavlink.c | 30 +++++++-------- src/main/telemetry/smartport.c | 14 +++---- src/test/unit/flight_imu_unittest.cc | 24 ++++++------ src/test/unit/ledstrip_unittest.cc | 19 +++++----- src/test/unit/telemetry_crsf_unittest.cc | 24 ++++-------- src/test/unit/telemetry_hott_unittest.cc | 5 +-- 22 files changed, 192 insertions(+), 197 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index acb8b3629..c42eca5a8 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -992,16 +992,16 @@ static void writeGPSFrame(timeUs_t currentTimeUs) blackboxWriteUnsignedVB(currentTimeUs - blackboxHistory[1]->time); } - blackboxWriteUnsignedVB(GPS_numSat); - blackboxWriteSignedVB(GPS_coord[0] - gpsHistory.GPS_home[0]); - blackboxWriteSignedVB(GPS_coord[1] - gpsHistory.GPS_home[1]); - blackboxWriteUnsignedVB(GPS_altitude); - blackboxWriteUnsignedVB(GPS_speed); - blackboxWriteUnsignedVB(GPS_ground_course); + blackboxWriteUnsignedVB(gpsSol.numSat); + blackboxWriteSignedVB(gpsSol.llh.lat - gpsHistory.GPS_home[LAT]); + blackboxWriteSignedVB(gpsSol.llh.lon - gpsHistory.GPS_home[LON]); + blackboxWriteUnsignedVB(gpsSol.llh.alt); + blackboxWriteUnsignedVB(gpsSol.groundSpeed); + blackboxWriteUnsignedVB(gpsSol.groundCourse); - gpsHistory.GPS_numSat = GPS_numSat; - gpsHistory.GPS_coord[0] = GPS_coord[0]; - gpsHistory.GPS_coord[1] = GPS_coord[1]; + gpsHistory.GPS_numSat = gpsSol.numSat; + gpsHistory.GPS_coord[LAT] = gpsSol.llh.lat; + gpsHistory.GPS_coord[LON] = gpsSol.llh.lon; } #endif @@ -1474,8 +1474,9 @@ static void blackboxLogIteration(timeUs_t currentTimeUs) if (blackboxShouldLogGpsHomeFrame()) { writeGPSHomeFrame(); writeGPSFrame(currentTimeUs); - } else if (GPS_numSat != gpsHistory.GPS_numSat || GPS_coord[0] != gpsHistory.GPS_coord[0] - || GPS_coord[1] != gpsHistory.GPS_coord[1]) { + } else if (gpsSol.numSat != gpsHistory.GPS_numSat + || gpsSol.llh.lat != gpsHistory.GPS_coord[LAT] + || gpsSol.llh.lon != gpsHistory.GPS_coord[LON]) { //We could check for velocity changes as well but I doubt it changes independent of position writeGPSFrame(currentTimeUs); } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 90a5c22e5..50e126a39 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -231,7 +231,7 @@ void mwArm(void) //beep to indicate arming #ifdef GPS - if (feature(FEATURE_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5) + if (feature(FEATURE_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5) beeper(BEEPER_ARMING_GPS_FIX); else beeper(BEEPER_ARMING); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index ce8a74520..53774dc22 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1113,12 +1113,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_RAW_GPS: sbufWriteU8(dst, STATE(GPS_FIX)); - sbufWriteU8(dst, GPS_numSat); - sbufWriteU32(dst, GPS_coord[LAT]); - sbufWriteU32(dst, GPS_coord[LON]); - sbufWriteU16(dst, GPS_altitude); - sbufWriteU16(dst, GPS_speed); - sbufWriteU16(dst, GPS_ground_course); + sbufWriteU8(dst, gpsSol.numSat); + sbufWriteU32(dst, gpsSol.llh.lat); + sbufWriteU32(dst, gpsSol.llh.lon); + sbufWriteU16(dst, gpsSol.llh.alt); + sbufWriteU16(dst, gpsSol.groundSpeed); + sbufWriteU16(dst, gpsSol.groundCourse); break; case MSP_COMP_GPS: @@ -1129,12 +1129,12 @@ static bool mspFcProcessOutCommand(uint8_t cmdMSP, sbuf_t *dst, mspPostProcessFn case MSP_GPSSVINFO: sbufWriteU8(dst, GPS_numCh); - for (int i = 0; i < GPS_numCh; i++) { - sbufWriteU8(dst, GPS_svinfo_chn[i]); - sbufWriteU8(dst, GPS_svinfo_svid[i]); - sbufWriteU8(dst, GPS_svinfo_quality[i]); - sbufWriteU8(dst, GPS_svinfo_cno[i]); - } + for (int i = 0; i < GPS_numCh; i++) { + sbufWriteU8(dst, GPS_svinfo_chn[i]); + sbufWriteU8(dst, GPS_svinfo_svid[i]); + sbufWriteU8(dst, GPS_svinfo_quality[i]); + sbufWriteU8(dst, GPS_svinfo_cno[i]); + } break; #endif @@ -1838,11 +1838,11 @@ static mspResult_e mspFcProcessInCommand(uint8_t cmdMSP, sbuf_t *src) } else { DISABLE_STATE(GPS_FIX); } - GPS_numSat = sbufReadU8(src); - GPS_coord[LAT] = sbufReadU32(src); - GPS_coord[LON] = sbufReadU32(src); - GPS_altitude = sbufReadU16(src); - GPS_speed = sbufReadU16(src); + gpsSol.numSat = sbufReadU8(src); + gpsSol.llh.lat = sbufReadU32(src); + gpsSol.llh.lon = sbufReadU32(src); + gpsSol.llh.alt = sbufReadU16(src); + gpsSol.groundSpeed = sbufReadU16(src); GPS_update |= 2; // New data signalisation to GPS functions // FIXME Magic Numbers break; diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 4056e9ce5..efa4b853c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -419,9 +419,9 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) useMag = true; } #if defined(GPS) - else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && GPS_numSat >= 5 && GPS_speed >= 300) { + else if (STATE(FIXED_WING) && sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 5 && gpsSol.groundSpeed >= 300) { // In case of a fixed-wing aircraft we can use GPS course over ground to correct heading - rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - GPS_ground_course); + rawYawError = DECIDEGREES_TO_RADIANS(attitude.values.yaw - gpsSol.groundCourse); useYaw = true; } #endif diff --git a/src/main/flight/navigation.c b/src/main/flight/navigation.c index 1ee364b87..c34b66819 100644 --- a/src/main/flight/navigation.c +++ b/src/main/flight/navigation.c @@ -254,7 +254,7 @@ void GPS_calculateDistanceAndDirectionToHome(void) if (STATE(GPS_FIX_HOME)) { // If we don't have home set, do not display anything uint32_t dist; int32_t dir; - GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_home[LAT], &GPS_home[LON], &dist, &dir); + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_home[LAT], &GPS_home[LON], &dist, &dir); GPS_distanceToHome = dist / 100; GPS_directionToHome = dir / 100; } else { @@ -270,7 +270,7 @@ void onGpsNewData(void) uint16_t speed; - if (!(STATE(GPS_FIX) && GPS_numSat >= 5)) { + if (!(STATE(GPS_FIX) && gpsSol.numSat >= 5)) { return; } @@ -284,7 +284,7 @@ void onGpsNewData(void) #if defined(GPS_FILTERING) GPS_filter_index = (GPS_filter_index + 1) % GPS_FILTER_VECTOR_LENGTH; for (axis = 0; axis < 2; axis++) { - GPS_read[axis] = GPS_coord[axis]; // latest unfiltered data is in GPS_latitude and GPS_longitude + GPS_read[axis] = axis == LAT ? gpsSol.llh.lat : gpsSol.llh.lon; // latest unfiltered data is in GPS_latitude and GPS_longitude GPS_degree[axis] = GPS_read[axis] / 10000000; // get the degree to assure the sum fits to the int32_t // How close we are to a degree line ? its the first three digits from the fractions of degree @@ -296,8 +296,13 @@ void onGpsNewData(void) GPS_filter_sum[axis] += GPS_filter[axis][GPS_filter_index]; GPS_filtered[axis] = GPS_filter_sum[axis] / GPS_FILTER_VECTOR_LENGTH + (GPS_degree[axis] * 10000000); if (nav_mode == NAV_MODE_POSHOLD) { // we use gps averaging only in poshold mode... - if (fraction3[axis] > 1 && fraction3[axis] < 999) - GPS_coord[axis] = GPS_filtered[axis]; + if (fraction3[axis] > 1 && fraction3[axis] < 999) { + if (axis == LAT) { + gpsSol.llh.lat = GPS_filtered[LAT]; + } else { + gpsSol.llh.lon = GPS_filtered[LON]; + } + } } } #endif @@ -320,8 +325,8 @@ void onGpsNewData(void) // we are navigating // gps nav calculations, these are common for nav and poshold - GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); - GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); + GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon); switch (nav_mode) { case NAV_MODE_POSHOLD: @@ -359,10 +364,10 @@ void onGpsNewData(void) void GPS_reset_home_position(void) { - if (STATE(GPS_FIX) && GPS_numSat >= 5) { - GPS_home[LAT] = GPS_coord[LAT]; - GPS_home[LON] = GPS_coord[LON]; - GPS_calc_longitude_scaling(GPS_coord[LAT]); // need an initial value for distance and bearing calc + if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { + GPS_home[LAT] = gpsSol.llh.lat; + GPS_home[LON] = gpsSol.llh.lon; + GPS_calc_longitude_scaling(gpsSol.llh.lat); // need an initial value for distance and bearing calc nav_takeoff_bearing = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // save takeoff heading // Set ground altitude ENABLE_STATE(GPS_FIX_HOME); @@ -429,10 +434,10 @@ void GPS_set_next_wp(int32_t *lat, int32_t *lon) GPS_WP[LON] = *lon; GPS_calc_longitude_scaling(*lat); - GPS_distance_cm_bearing(&GPS_coord[LAT], &GPS_coord[LON], &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &GPS_WP[LAT], &GPS_WP[LON], &wp_distance, &target_bearing); nav_bearing = target_bearing; - GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &GPS_coord[LAT], &GPS_coord[LON]); + GPS_calc_location_error(&GPS_WP[LAT], &GPS_WP[LON], &gpsSol.llh.lat, &gpsSol.llh.lon); original_target_bearing = target_bearing; waypoint_speed_gov = navigationConfig()->nav_speed_min; } @@ -484,13 +489,11 @@ static void GPS_calc_velocity(void) static int16_t speed_old[2] = { 0, 0 }; static int32_t last_coord[2] = { 0, 0 }; static uint8_t init = 0; - // y_GPS_speed positive = Up - // x_GPS_speed positive = Right if (init) { float tmp = 1.0f / dTnav; - actual_speed[GPS_X] = (float)(GPS_coord[LON] - last_coord[LON]) * GPS_scaleLonDown * tmp; - actual_speed[GPS_Y] = (float)(GPS_coord[LAT] - last_coord[LAT]) * tmp; + actual_speed[GPS_X] = (float)(gpsSol.llh.lon - last_coord[LON]) * GPS_scaleLonDown * tmp; + actual_speed[GPS_Y] = (float)(gpsSol.llh.lat - last_coord[LAT]) * tmp; actual_speed[GPS_X] = (actual_speed[GPS_X] + speed_old[GPS_X]) / 2; actual_speed[GPS_Y] = (actual_speed[GPS_Y] + speed_old[GPS_Y]) / 2; @@ -500,8 +503,8 @@ static void GPS_calc_velocity(void) } init = 1; - last_coord[LON] = GPS_coord[LON]; - last_coord[LAT] = GPS_coord[LAT]; + last_coord[LON] = gpsSol.llh.lon; + last_coord[LAT] = gpsSol.llh.lat; } //////////////////////////////////////////////////////////////////////////////////// @@ -666,7 +669,7 @@ void updateGpsWaypointsAndMode(void) bool resetNavNow = false; static bool gpsReadyBeepDone = false; - if (STATE(GPS_FIX) && GPS_numSat >= 5) { + if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { // // process HOME mode @@ -701,8 +704,8 @@ void updateGpsWaypointsAndMode(void) // Transition to HOLD mode ENABLE_FLIGHT_MODE(GPS_HOLD_MODE); - GPS_hold[LAT] = GPS_coord[LAT]; - GPS_hold[LON] = GPS_coord[LON]; + GPS_hold[LAT] = gpsSol.llh.lat; + GPS_hold[LON] = gpsSol.llh.lon; GPS_set_next_wp(&GPS_hold[LAT], &GPS_hold[LON]); nav_mode = NAV_MODE_POSHOLD; resetNavNow = true; diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index 3b35e4be8..7b3a9d6e7 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -295,12 +295,12 @@ void beeperConfirmationBeeps(uint8_t beepCount) void beeperGpsStatus(void) { // if GPS fix then beep out number of satellites - if (STATE(GPS_FIX) && GPS_numSat >= 5) { + if (STATE(GPS_FIX) && gpsSol.numSat >= 5) { uint8_t i = 0; do { beep_multiBeeps[i++] = 5; beep_multiBeeps[i++] = 10; - } while (i < MAX_MULTI_BEEPS && GPS_numSat > i / 2); + } while (i < MAX_MULTI_BEEPS && gpsSol.numSat > i / 2); beep_multiBeeps[i-1] = 50; // extend last pause beep_multiBeeps[i] = BEEPER_COMMAND_STOP; diff --git a/src/main/io/dashboard.c b/src/main/io/dashboard.c index 129b62976..f54511fb5 100644 --- a/src/main/io/dashboard.c +++ b/src/main/io/dashboard.c @@ -365,22 +365,22 @@ void showGpsPage() { char fixChar = STATE(GPS_FIX) ? 'Y' : 'N'; - tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", GPS_numSat, fixChar); + tfp_sprintf(lineBuffer, "Sats: %d Fix: %c", gpsSol.numSat, fixChar); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "La/Lo: %d/%d", GPS_coord[LAT] / GPS_DEGREES_DIVIDER, GPS_coord[LON] / GPS_DEGREES_DIVIDER); + tfp_sprintf(lineBuffer, "La/Lo: %d/%d", gpsSol.llh.lat / GPS_DEGREES_DIVIDER, gpsSol.llh.lon / GPS_DEGREES_DIVIDER); padLineBuffer(); i2c_OLED_set_line(rowIndex++); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "Spd: %d", GPS_speed); + tfp_sprintf(lineBuffer, "Spd: %d", gpsSol.groundSpeed); padHalfLineBuffer(); i2c_OLED_set_line(rowIndex); i2c_OLED_send_string(lineBuffer); - tfp_sprintf(lineBuffer, "GC: %d", GPS_ground_course); + tfp_sprintf(lineBuffer, "GC: %d", gpsSol.groundCourse); padHalfLineBuffer(); i2c_OLED_set_xy(HALF_SCREEN_CHARACTER_COLUMN_COUNT, rowIndex++); i2c_OLED_send_string(lineBuffer); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 260341c66..46b086be6 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -71,18 +71,11 @@ static char *gpsPacketLogChar = gpsPacketLog; // ********************** // GPS // ********************** -int32_t GPS_coord[2]; // LAT/LON - -uint8_t GPS_numSat; -uint16_t GPS_hdop = 9999; // Compute GPS quality signal +gpsSolutionData_t gpsSol; uint32_t GPS_packetCount = 0; uint32_t GPS_svInfoReceivedCount = 0; // SV = Space Vehicle, counter increments each time SV info is received. uint8_t GPS_update = 0; // it's a binary toggle to distinct a GPS position update -uint16_t GPS_altitude; // altitude in 0.1m -uint16_t GPS_speed; // speed in 0.1m/s -uint16_t GPS_ground_course = 0; // degrees * 10 - uint8_t GPS_numCh; // Number of channels uint8_t GPS_svinfo_chn[GPS_SV_MAXSATS]; // Channel number uint8_t GPS_svinfo_svid[GPS_SV_MAXSATS]; // Satellite ID @@ -424,7 +417,7 @@ void gpsInitHardware(void) static void updateGpsIndicator(timeUs_t currentTimeUs) { static uint32_t GPSLEDTime; - if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (GPS_numSat >= 5)) { + if ((int32_t)(currentTimeUs - GPSLEDTime) >= 0 && (gpsSol.numSat >= 5)) { GPSLEDTime = currentTimeUs + 150000; LED1_TOGGLE; } @@ -456,8 +449,7 @@ void gpsUpdate(timeUs_t currentTimeUs) gpsData.baudrateIndex %= GPS_INIT_ENTRIES; } gpsData.lastMessage = millis(); - // TODO - move some / all of these into gpsData - GPS_numSat = 0; + gpsSol.numSat = 0; DISABLE_STATE(GPS_FIX); gpsSetState(GPS_INITIALIZING); break; @@ -746,16 +738,16 @@ static bool gpsNewFrameNMEA(char c) *gpsPacketLogChar = LOG_NMEA_GGA; frameOK = 1; if (STATE(GPS_FIX)) { - GPS_coord[LAT] = gps_Msg.latitude; - GPS_coord[LON] = gps_Msg.longitude; - GPS_numSat = gps_Msg.numSat; - GPS_altitude = gps_Msg.altitude; + gpsSol.llh.lat = gps_Msg.latitude; + gpsSol.llh.lon = gps_Msg.longitude; + gpsSol.numSat = gps_Msg.numSat; + gpsSol.llh.alt = gps_Msg.altitude; } break; case FRAME_RMC: *gpsPacketLogChar = LOG_NMEA_RMC; - GPS_speed = gps_Msg.speed; - GPS_ground_course = gps_Msg.ground_course; + gpsSol.groundSpeed = gps_Msg.speed; + gpsSol.groundCourse = gps_Msg.ground_course; break; } // end switch } else { @@ -952,9 +944,9 @@ static bool UBLOX_parse_gps(void) case MSG_POSLLH: *gpsPacketLogChar = LOG_UBLOX_POSLLH; //i2c_dataset.time = _buffer.posllh.time; - GPS_coord[LON] = _buffer.posllh.longitude; - GPS_coord[LAT] = _buffer.posllh.latitude; - GPS_altitude = _buffer.posllh.altitude_msl / 10 / 100; //alt in m + gpsSol.llh.lon = _buffer.posllh.longitude; + gpsSol.llh.lat = _buffer.posllh.latitude; + gpsSol.llh.alt = _buffer.posllh.altitude_msl / 10 / 100; //alt in m if (next_fix) { ENABLE_STATE(GPS_FIX); } else { @@ -973,14 +965,14 @@ static bool UBLOX_parse_gps(void) next_fix = (_buffer.solution.fix_status & NAV_STATUS_FIX_VALID) && (_buffer.solution.fix_type == FIX_3D); if (!next_fix) DISABLE_STATE(GPS_FIX); - GPS_numSat = _buffer.solution.satellites; - GPS_hdop = _buffer.solution.position_DOP; + gpsSol.numSat = _buffer.solution.satellites; + gpsSol.hdop = _buffer.solution.position_DOP; break; case MSG_VELNED: *gpsPacketLogChar = LOG_UBLOX_VELNED; // speed_3d = _buffer.velned.speed_3d; // cm/s - GPS_speed = _buffer.velned.speed_2d; // cm/s - GPS_ground_course = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 + gpsSol.groundSpeed = _buffer.velned.speed_2d; // cm/s + gpsSol.groundCourse = (uint16_t) (_buffer.velned.heading_2d / 10000); // Heading 2D deg * 100000 rescaled to deg * 10 _new_speed = true; break; case MSG_SVINFO: diff --git a/src/main/io/gps.h b/src/main/io/gps.h index 81ee218e4..d33f04438 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -31,8 +31,6 @@ typedef enum { GPS_UBLOX } gpsProvider_e; -#define GPS_PROVIDER_MAX GPS_UBLOX - typedef enum { SBAS_AUTO = 0, SBAS_EGNOS, @@ -77,6 +75,21 @@ typedef struct gpsCoordinateDDDMMmmmm_s { int16_t mmmm; } gpsCoordinateDDDMMmmmm_t; +/* LLH Location in NEU axis system */ +typedef struct gpsLocation_s { + int32_t lat; // latitude * 1e+7 + int32_t lon; // longitude * 1e+7 + uint16_t alt; // altitude in 0.1m +} gpsLocation_t; + +typedef struct gpsSolutionData_s { + uint8_t numSat; + gpsLocation_t llh; + uint16_t GPS_altitude; // altitude in 0.1m + uint16_t groundSpeed; // speed in 0.1m/s + uint16_t groundCourse; // degrees * 10 + uint16_t hdop; // generic HDOP value (*100) +} gpsSolutionData_t; typedef enum { GPS_MESSAGE_STATE_IDLE = 0, @@ -102,16 +115,11 @@ typedef struct gpsData_s { extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; extern gpsData_t gpsData; -extern int32_t GPS_coord[2]; // LAT/LON +extern gpsSolutionData_t gpsSol; -extern uint8_t GPS_numSat; -extern uint16_t GPS_hdop; // GPS signal quality extern uint8_t GPS_update; // it's a binary toogle to distinct a GPS position update extern uint32_t GPS_packetCount; extern uint32_t GPS_svInfoReceivedCount; -extern uint16_t GPS_altitude; // altitude in 0.1m -extern uint16_t GPS_speed; // speed in 0.1m/s -extern uint16_t GPS_ground_course; // degrees * 10 extern uint8_t GPS_numCh; // Number of channels extern uint8_t GPS_svinfo_chn[16]; // Channel number extern uint8_t GPS_svinfo_svid[16]; // Satellite ID diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 0be72d3a9..6baa47362 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -731,7 +731,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer) static uint8_t gpsFlashCounter = 0; if (gpsPauseCounter > 0) { gpsPauseCounter--; - } else if (gpsFlashCounter >= GPS_numSat) { + } else if (gpsFlashCounter >= gpsSol.numSat) { gpsFlashCounter = 0; gpsPauseCounter = blinkPauseLength; } else { @@ -743,7 +743,7 @@ static void applyLedGpsLayer(bool updateNow, timeUs_t *timer) const hsvColor_t *gpsColor; - if (GPS_numSat == 0 || !sensors(SENSOR_GPS)) { + if (gpsSol.numSat == 0 || !sensors(SENSOR_GPS)) { gpsColor = getSC(LED_SCOLOR_GPSNOSATS); } else { bool colorOn = gpsPauseCounter == 0; // each interval starts with pause diff --git a/src/main/io/osd.c b/src/main/io/osd.c index dcbd7257f..2a9262bad 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -277,12 +277,12 @@ static void osdDrawSingleElement(uint8_t item) #ifdef GPS case OSD_GPS_SATS: buff[0] = 0x1f; - tfp_sprintf(buff + 1, "%d", GPS_numSat); + tfp_sprintf(buff + 1, "%d", gpsSol.numSat); break; case OSD_GPS_SPEED: // FIXME ideally we want to use SYM_KMH symbol but it's not in the font any more, so we use K. - tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(GPS_speed)); + tfp_sprintf(buff, "%3dK", CM_S_TO_KM_H(gpsSol.groundSpeed)); break; case OSD_GPS_LAT: @@ -291,10 +291,10 @@ static void osdDrawSingleElement(uint8_t item) int32_t val; if (item == OSD_GPS_LAT) { buff[0] = SYM_ARROW_EAST; - val = GPS_coord[LAT]; + val = gpsSol.llh.lat; } else { buff[0] = SYM_ARROW_SOUTH; - val = GPS_coord[LON]; + val = gpsSol.llh.lon; } char wholeDegreeString[5]; @@ -880,7 +880,7 @@ static void osdUpdateStats(void) { int16_t value = 0; #ifdef GPS - value = CM_S_TO_KM_H(GPS_speed); + value = CM_S_TO_KM_H(gpsSol.groundSpeed); #endif if (stats.max_speed < value) stats.max_speed = value; diff --git a/src/main/target/COLIBRI_RACE/i2c_bst.c b/src/main/target/COLIBRI_RACE/i2c_bst.c index 9da520de1..ca870492e 100644 --- a/src/main/target/COLIBRI_RACE/i2c_bst.c +++ b/src/main/target/COLIBRI_RACE/i2c_bst.c @@ -778,15 +778,15 @@ static uint8_t numOfSat = 0; #ifdef GPS bool writeGpsPositionPrameToBST(void) { - if((lat != GPS_coord[LAT]) || (lon != GPS_coord[LON]) || (alt != GPS_altitude) || (numOfSat != GPS_numSat)) { - lat = GPS_coord[LAT]; - lon = GPS_coord[LON]; - alt = GPS_altitude; - numOfSat = GPS_numSat; - uint16_t speed = (GPS_speed * 9 / 25); + if((lat != gpsSol.llh.lat) || (lon != gpsSol.llh.lon) || (alt != gpsSol.llh.alt) || (numOfSat != gpsSol.numSat)) { + lat = gpsSol.llh.lat; + lon = gpsSol.llh.lon; + alt = gpsSol.llh.alt; + numOfSat = gpsSol.numSat; + uint16_t speed = (gpsSol.groundSpeed * 9 / 25); uint16_t gpsHeading = 0; uint16_t altitude = 0; - gpsHeading = GPS_ground_course * 10; + gpsHeading = gpsSol.groundCourse * 10; altitude = alt * 10 + 1000; bstMasterStartBuffer(PUBLIC_ADDRESS); diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 5f32767a6..1b0a0ab1e 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -111,7 +111,7 @@ int32_t Latitude ( degree / 10`000`000 ) int32_t Longitude (degree / 10`000`000 ) uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) -uint16 Altitude ( meter ­ 1000m offset ) +uint16 Altitude ( meter ­1000m offset ) uint8_t Satellites in use ( counter ) */ void crsfFrameGps(sbuf_t *dst) @@ -119,14 +119,14 @@ void crsfFrameGps(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_GPS_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); sbufWriteU8(dst, CRSF_FRAMETYPE_GPS); - sbufWriteU32BigEndian(dst, GPS_coord[LAT]); // CRSF and betaflight use same units for degrees - sbufWriteU32BigEndian(dst, GPS_coord[LON]); - sbufWriteU16BigEndian(dst, (GPS_speed * 36 + 5) / 10); // GPS_speed is in 0.1m/s - sbufWriteU16BigEndian(dst, GPS_ground_course * 10); // GPS_ground_course is degrees * 10 + sbufWriteU32BigEndian(dst, gpsSol.llh.lat); // CRSF and betaflight use same units for degrees + sbufWriteU32BigEndian(dst, gpsSol.llh.lon); + sbufWriteU16BigEndian(dst, (gpsSol.groundSpeed * 36 + 5) / 10); // gpsSol.groundSpeed is in 0.1m/s + sbufWriteU16BigEndian(dst, gpsSol.groundCourse * 10); // gpsSol.groundCourse is degrees * 10 //Send real GPS altitude only if it's reliable (there's a GPS fix) - const uint16_t altitude = (STATE(GPS_FIX) ? GPS_altitude : 0) + 1000; + const uint16_t altitude = (STATE(GPS_FIX) ? gpsSol.llh.alt : 0) + 1000; sbufWriteU16BigEndian(dst, altitude); - sbufWriteU8(dst, GPS_numSat); + sbufWriteU8(dst, gpsSol.numSat); } /* @@ -204,7 +204,7 @@ void crsfFrameAttitude(sbuf_t *dst) /* 0x21 Flight mode text based Payload: -char[] Flight mode ( Null­terminated string ) +char[] Flight mode ( Null terminated string ) */ void crsfFrameFlightMode(sbuf_t *dst) { diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 60a280c43..39451a247 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -183,7 +183,7 @@ static void sendBaro(void) #ifdef GPS static void sendGpsAltitude(void) { - uint16_t altitude = GPS_altitude; + uint16_t altitude = gpsSol.llh.alt; //Send real GPS altitude only if it's reliable (there's a GPS fix) if (!STATE(GPS_FIX)) { altitude = 0; @@ -230,9 +230,9 @@ static void sendTemperature1(void) #ifdef GPS static void sendSatalliteSignalQualityAsTemperature2(void) { - uint16_t satellite = GPS_numSat; - if (GPS_hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s - satellite = constrain(GPS_hdop, 0, GPS_MAX_HDOP_VAL); + uint16_t satellite = gpsSol.numSat; + if (gpsSol.hdop > GPS_BAD_QUALITY && ( (cycleNum % 16 ) < 8)) {//Every 1s + satellite = constrain(gpsSol.hdop, 0, GPS_MAX_HDOP_VAL); } sendDataHead(ID_TEMPRATURE2); @@ -254,9 +254,9 @@ static void sendSpeed(void) //Speed should be sent in knots (GPS speed is in cm/s) sendDataHead(ID_GPS_SPEED_BP); //convert to knots: 1cm/s = 0.0194384449 knots - serialize16(GPS_speed * 1944 / 100000); + serialize16(gpsSol.groundSpeed * 1944 / 100000); sendDataHead(ID_GPS_SPEED_AP); - serialize16((GPS_speed * 1944 / 100) % 100); + serialize16((gpsSol.groundSpeed * 1944 / 100) % 100); } #endif @@ -337,11 +337,14 @@ static void sendFakeLatLong(void) static void sendGPSLatLong(void) { static uint8_t gpsFixOccured = 0; + int32_t coord[2] = {0,0}; if (STATE(GPS_FIX) || gpsFixOccured == 1) { // If we have ever had a fix, send the last known lat/long gpsFixOccured = 1; - sendLatLong(GPS_coord); + coord[LAT] = gpsSol.llh.lat; + coord[LON] = gpsSol.llh.lon; + sendLatLong(coord); } else { // otherwise send fake lat/long in order to display compass value sendFakeLatLong(); diff --git a/src/main/telemetry/hott.c b/src/main/telemetry/hott.c index c2c3c0907..4f5d893bf 100644 --- a/src/main/telemetry/hott.c +++ b/src/main/telemetry/hott.c @@ -185,35 +185,35 @@ void addGPSCoordinates(HOTT_GPS_MSG_t *hottGPSMessage, int32_t latitude, int32_t void hottPrepareGPSResponse(HOTT_GPS_MSG_t *hottGPSMessage) { - hottGPSMessage->gps_satelites = GPS_numSat; + hottGPSMessage->gps_satelites = gpsSol.numSat; if (!STATE(GPS_FIX)) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_NONE; return; } - if (GPS_numSat >= 5) { + if (gpsSol.numSat >= 5) { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_3D; } else { hottGPSMessage->gps_fix_char = GPS_FIX_CHAR_2D; } - addGPSCoordinates(hottGPSMessage, GPS_coord[LAT], GPS_coord[LON]); + addGPSCoordinates(hottGPSMessage, gpsSol.llh.lat, gpsSol.llh.lon); // GPS Speed is returned in cm/s (from io/gps.c) and must be sent in km/h (Hott requirement) - const uint16_t speed = (GPS_speed * 36) / 1000; + const uint16_t speed = (gpsSol.groundSpeed * 36) / 1000; 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; - uint16_t altitude = GPS_altitude; + uint16_t altitude = gpsSol.llh.alt; if (!STATE(GPS_FIX)) { altitude = getEstimatedAltitude() / 100; } - const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // GPS_altitude in m ; offset = 500 -> O m + const uint16_t hottGpsAltitude = (altitude) + HOTT_GPS_ALTITUDE_OFFSET; // gpsSol.llh.alt in m ; offset = 500 -> O m hottGPSMessage->altitude_L = hottGpsAltitude & 0x00FF; hottGPSMessage->altitude_H = hottGpsAltitude >> 8; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index c5997f630..c4cfed818 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -133,23 +133,23 @@ static void ltm_gframe(void) if (!STATE(GPS_FIX)) gps_fix_type = 1; - else if (GPS_numSat < 5) + else if (gpsSol.numSat < 5) gps_fix_type = 2; else gps_fix_type = 3; ltm_initialise_packet('G'); - ltm_serialise_32(GPS_coord[LAT]); - ltm_serialise_32(GPS_coord[LON]); - ltm_serialise_8((uint8_t)(GPS_speed / 100)); + ltm_serialise_32(gpsSol.llh.lat); + ltm_serialise_32(gpsSol.llh.lon); + ltm_serialise_8((uint8_t)(gpsSol.groundSpeed / 100)); #if defined(BARO) || defined(SONAR) - ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : GPS_altitude * 100; + ltm_alt = (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() : gpsSol.llh.alt * 100; #else - ltm_alt = GPS_altitude * 100; + ltm_alt = gpsSol.llh.alt * 100; #endif ltm_serialise_32(ltm_alt); - ltm_serialise_8((GPS_numSat << 2) | gps_fix_type); + ltm_serialise_8((gpsSol.numSat << 2) | gps_fix_type); ltm_finalise(); #endif } diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 1cedab5bb..6b0ae90e5 100755 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -286,7 +286,7 @@ void mavlinkSendPosition(void) gpsFixType = 1; } else { - if (GPS_numSat < 5) { + if (gpsSol.numSat < 5) { gpsFixType = 2; } else { @@ -300,21 +300,21 @@ void mavlinkSendPosition(void) // fix_type 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix. gpsFixType, // lat Latitude in 1E7 degrees - GPS_coord[LAT], + gpsSol.llh.lat, // lon Longitude in 1E7 degrees - GPS_coord[LON], + gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - GPS_altitude * 1000, + gpsSol.llh.alt * 1000, // eph GPS HDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // epv GPS VDOP horizontal dilution of position in cm (m*100). If unknown, set to: 65535 65535, // vel GPS ground speed (m/s * 100). If unknown, set to: 65535 - GPS_speed, + gpsSol.groundSpeed, // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 - GPS_ground_course * 10, + gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 - GPS_numSat); + gpsSol.numSat); msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavMsg); mavlinkSerialWrite(mavBuffer, msgLength); @@ -323,16 +323,16 @@ void mavlinkSendPosition(void) // time_usec Timestamp (microseconds since UNIX epoch or microseconds since system boot) micros(), // lat Latitude in 1E7 degrees - GPS_coord[LAT], + gpsSol.llh.lat, // lon Longitude in 1E7 degrees - GPS_coord[LON], + gpsSol.llh.lon, // alt Altitude in 1E3 meters (millimeters) above MSL - GPS_altitude * 1000, + gpsSol.llh.alt * 1000, // relative_alt Altitude above ground in meters, expressed as * 1000 (millimeters) #if defined(BARO) || defined(SONAR) - (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : GPS_altitude * 1000, + (sensors(SENSOR_SONAR) || sensors(SENSOR_BARO)) ? getEstimatedAltitude() * 10 : gpsSol.llh.alt * 1000, #else - GPS_altitude * 1000, + gpsSol.llh.alt * 1000, #endif // Ground X Speed (Latitude), expressed as m/s * 100 0, @@ -391,7 +391,7 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(GPS) // use ground speed if source available if (sensors(SENSOR_GPS)) { - mavGroundSpeed = GPS_speed / 100.0f; + mavGroundSpeed = gpsSol.groundSpeed / 100.0f; } #endif @@ -404,13 +404,13 @@ void mavlinkSendHUDAndHeartbeat(void) #if defined(GPS) else if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS - mavAltitude = GPS_altitude; + mavAltitude = gpsSol.llh.alt; } #endif #elif defined(GPS) if (sensors(SENSOR_GPS)) { // No sonar or baro, just display altitude above MLS - mavAltitude = GPS_altitude; + mavAltitude = gpsSol.llh.alt; } #endif diff --git a/src/main/telemetry/smartport.c b/src/main/telemetry/smartport.c index 8a372c116..08f85752c 100644 --- a/src/main/telemetry/smartport.c +++ b/src/main/telemetry/smartport.c @@ -602,7 +602,7 @@ void handleSmartPortTelemetry(void) if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { //convert to knots: 1cm/s = 0.0194384449 knots //Speed should be sent in knots/1000 (GPS speed is in cm/s) - uint32_t tmpui = GPS_speed * 1944 / 100; + uint32_t tmpui = gpsSol.groundSpeed * 1944 / 100; smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; } @@ -649,14 +649,14 @@ void handleSmartPortTelemetry(void) // the MSB of the sent uint32_t helps FrSky keep track // the even/odd bit of our counter helps us keep track if (smartPortIdCnt & 1) { - tmpui = abs(GPS_coord[LON]); // now we have unsigned value and one bit to spare + tmpui = abs(gpsSol.llh.lon); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25 | 0x80000000; // 6/100 = 1.5/25, division by power of 2 is fast - if (GPS_coord[LON] < 0) tmpui |= 0x40000000; + if (gpsSol.llh.lon < 0) tmpui |= 0x40000000; } else { - tmpui = abs(GPS_coord[LAT]); // now we have unsigned value and one bit to spare + tmpui = abs(gpsSol.llh.lat); // now we have unsigned value and one bit to spare tmpui = (tmpui + tmpui / 2) / 25; // 6/100 = 1.5/25, division by power of 2 is fast - if (GPS_coord[LAT] < 0) tmpui |= 0x40000000; + if (gpsSol.llh.lat < 0) tmpui |= 0x40000000; } smartPortSendPackage(id, tmpui); smartPortHasRequest = 0; @@ -735,7 +735,7 @@ void handleSmartPortTelemetry(void) if (sensors(SENSOR_GPS)) { #ifdef GPS // provide GPS lock status - smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + GPS_numSat); + smartPortSendPackage(id, (STATE(GPS_FIX) ? 1000 : 0) + (STATE(GPS_FIX_HOME) ? 2000 : 0) + gpsSol.numSat); smartPortHasRequest = 0; #endif } else if (feature(FEATURE_GPS)) { @@ -776,7 +776,7 @@ void handleSmartPortTelemetry(void) #ifdef GPS case FSSP_DATAID_GPS_ALT : if (sensors(SENSOR_GPS) && STATE(GPS_FIX)) { - smartPortSendPackage(id, GPS_altitude * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) + smartPortSendPackage(id, gpsSol.llh.alt * 100); // given in 0.1m , requested in 10 = 1m (should be in mm, probably a bug in opentx, tested on 2.0.1.7) smartPortHasRequest = 0; } break; diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index b9fc11a32..290fb7265 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -35,22 +35,24 @@ extern "C" { #include "drivers/compass/compass.h" #include "drivers/sensor.h" - #include "sensors/sensors.h" - #include "sensors/gyro.h" - #include "sensors/compass.h" - #include "sensors/acceleration.h" - #include "sensors/barometer.h" - - #include "fc/runtime_config.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" - - #include "rx/rx.h" + #include "fc/runtime_config.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/imu.h" + #include "io/gps.h" + + #include "rx/rx.h" + + #include "sensors/acceleration.h" + #include "sensors/barometer.h" + #include "sensors/compass.h" + #include "sensors/gyro.h" + #include "sensors/sensors.h" + void imuComputeRotationMatrix(void); void imuUpdateEulerAngles(void); @@ -206,9 +208,7 @@ gyro_t gyro; acc_t acc; mag_t mag; -uint8_t GPS_numSat; -uint16_t GPS_speed; -uint16_t GPS_ground_course; +gpsSolutionData_t gpsSol; uint8_t debugMode; int16_t debug[DEBUG16_VALUE_COUNT]; diff --git a/src/test/unit/ledstrip_unittest.cc b/src/test/unit/ledstrip_unittest.cc index b2b712bcb..058cf2131 100644 --- a/src/test/unit/ledstrip_unittest.cc +++ b/src/test/unit/ledstrip_unittest.cc @@ -24,27 +24,28 @@ extern "C" { #include "build/build_config.h" - #include "common/color.h" #include "common/axis.h" + #include "common/color.h" #include "config/parameter_group_ids.h" - #include "sensors/battery.h" - #include "drivers/io.h" - #include "rx/rx.h" + #include "drivers/light_ws2811strip.h" + #include "drivers/timer.h" + #include "drivers/timer_def.h" #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" - #include "drivers/light_ws2811strip.h" - #include "drivers/timer.h" - #include "drivers/timer_def.h" - + #include "io/gps.h" #include "io/ledstrip.h" + #include "rx/rx.h" + + #include "sensors/battery.h" + #include "target.h" } @@ -298,7 +299,7 @@ float rcCommand[4]; int16_t rcData[MAX_SUPPORTED_RC_CHANNEL_COUNT]; uint32_t rcModeActivationMask; uint16_t rssi = 0; -uint8_t GPS_numSat = 0; +gpsSolutionData_t gpsSol; batteryState_e getBatteryState(void) { return BATTERY_OK; diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index 0f4afaf44..661cb4569 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -82,12 +82,7 @@ uint16_t Groundspeed ( km/h / 10 ) uint16_t GPS heading ( degree / 100 ) uint16 Altitude ( meter ­ 1000m offset ) uint8_t Satellites in use ( counter ) -uint8_t GPS_numSat; -int32_t GPS_coord[2]; uint16_t GPS_distanceToHome; // distance to home point in meters -uint16_t GPS_altitude; // altitude in m -uint16_t GPS_speed; // speed in 0.1m/s -uint16_t GPS_ground_course = 0; // degrees * 10 */ #define FRAME_HEADER_FOOTER_LEN 4 @@ -114,13 +109,13 @@ TEST(TelemetryCrsfTest, TestGPS) EXPECT_EQ(0, satelliteCount); EXPECT_EQ(crfsCrc(frame, frameLen), frame[18]); - GPS_coord[LAT] = 56 * GPS_DEGREES_DIVIDER; - GPS_coord[LON] = 163 * GPS_DEGREES_DIVIDER; + gpsSol.llh.lat = 56 * GPS_DEGREES_DIVIDER; + gpsSol.llh.lon = 163 * GPS_DEGREES_DIVIDER; ENABLE_STATE(GPS_FIX); - GPS_altitude = 2345; // altitude in m - GPS_speed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 - GPS_numSat = 9; - GPS_ground_course = 1479; // degrees * 10 + gpsSol.llh.alt = 2345; // altitude in m + gpsSol.groundSpeed = 163; // speed in 0.1m/s, 16.3 m/s = 58.68 km/h, so CRSF (km/h *10) value is 587 + gpsSol.numSat = 9; + gpsSol.groundCourse = 1479; // degrees * 10 frameLen = getCrsfFrame(frame, CRSF_FRAME_GPS); lattitude = frame[3] << 24 | frame[4] << 16 | frame[5] << 8 | frame[6]; EXPECT_EQ(560000000, lattitude); @@ -279,12 +274,8 @@ uint8_t useHottAlarmSoundPeriod (void) { return 0; } attitudeEulerAngles_t attitude = { { 0, 0, 0 } }; // absolute angle inclination in multiple of 0.1 degree 180 deg = 1800 -uint8_t GPS_numSat; -int32_t GPS_coord[2]; uint16_t GPS_distanceToHome; // distance to home point in meters -uint16_t GPS_altitude; // altitude in m -uint16_t GPS_speed; // speed in 0.1m/s -uint16_t GPS_ground_course = 0; // degrees * 10 +gpsSolutionData_t gpsSol; void beeperConfirmationBeeps(uint8_t beepCount) {UNUSED(beepCount);} @@ -327,4 +318,3 @@ uint8_t calculateBatteryPercentageRemaining(void) { } } - diff --git a/src/test/unit/telemetry_hott_unittest.cc b/src/test/unit/telemetry_hott_unittest.cc index d4d427969..f8843b2e5 100644 --- a/src/test/unit/telemetry_hott_unittest.cc +++ b/src/test/unit/telemetry_hott_unittest.cc @@ -171,11 +171,8 @@ uint16_t batteryWarningVoltage; uint8_t useHottAlarmSoundPeriod (void) { return 0; } -uint8_t GPS_numSat; -int32_t GPS_coord[2]; -uint16_t GPS_speed; // speed in 0.1m/s +gpsSolutionData_t gpsSol; uint16_t GPS_distanceToHome; // distance to home point in meters -uint16_t GPS_altitude; // altitude in 0.1m int16_t GPS_directionToHome; // direction to home or hol point in degrees