diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 0ad397c03f..57314c385b 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -903,6 +903,7 @@ const clivalue_t valueTable[] = { { "gps_auto_baud", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, autoBaud) }, { "gps_ublox_use_galileo", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_ublox_use_galileo) }, { "gps_set_home_point_once", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_set_home_point_once) }, + { "gps_use_3d_speed", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_GPS_CONFIG, offsetof(gpsConfig_t, gps_use_3d_speed) }, #ifdef USE_GPS_RESCUE // PG_GPS_RESCUE diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 5ae82e5bb5..451fdce679 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -82,7 +82,7 @@ int16_t GPS_verticalSpeedInCmS; // vertical speed in cm/s float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read int16_t nav_takeoff_bearing; -#define GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph +#define GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S 15 // 5.4Km/h 3.35mph gpsSolutionData_t gpsSol; uint32_t GPS_packetCount = 0; @@ -231,7 +231,8 @@ PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, .autoConfig = GPS_AUTOCONFIG_ON, .autoBaud = GPS_AUTOBAUD_OFF, .gps_ublox_use_galileo = false, - .gps_set_home_point_once = false + .gps_set_home_point_once = false, + .gps_use_3d_speed = false ); static void shiftPacketLog(void) @@ -1104,7 +1105,7 @@ static bool UBLOX_parse_gps(void) break; case MSG_VELNED: *gpsPacketLogChar = LOG_UBLOX_VELNED; - // speed_3d = _buffer.velned.speed_3d; // cm/s + gpsSol.speed3d = _buffer.velned.speed_3d; // cm/s 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; @@ -1274,11 +1275,15 @@ static void GPS_calculateDistanceFlownVerticalSpeed(bool initialize) GPS_verticalSpeedInCmS = 0; } else { if (STATE(GPS_FIX_HOME) && ARMING_FLAG(ARMED)) { + uint16_t speed = gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed; // Only add up movement when speed is faster than minimum threshold - if (gpsSol.groundSpeed > GPS_DISTANCE_FLOWN_MIN_GROUND_SPEED_THRESHOLD_CM_S) { + if (speed > GPS_DISTANCE_FLOWN_MIN_SPEED_THRESHOLD_CM_S) { uint32_t dist; int32_t dir; GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &lastCoord[LAT], &lastCoord[LON], &dist, &dir); + if (gpsConfig()->gps_use_3d_speed) { + dist = sqrtf(powf(gpsSol.llh.altCm - lastAlt, 2.0f) + powf(dist, 2.0f)); + } GPS_distanceFlownInCm += dist; } } diff --git a/src/main/io/gps.h b/src/main/io/gps.h index a510b962e5..85eb9164a8 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -75,6 +75,7 @@ typedef struct gpsConfig_s { gpsAutoBaud_e autoBaud; uint8_t gps_ublox_use_galileo; uint8_t gps_set_home_point_once; + uint8_t gps_use_3d_speed; } gpsConfig_t; PG_DECLARE(gpsConfig_t, gpsConfig); @@ -93,6 +94,7 @@ typedef struct gpsLocation_s { typedef struct gpsSolutionData_s { gpsLocation_t llh; + uint16_t speed3d; // speed in 0.1m/s uint16_t groundSpeed; // speed in 0.1m/s uint16_t groundCourse; // degrees * 10 uint16_t hdop; // generic HDOP value (*100) diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index a4953067e1..2f6338c30d 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -389,7 +389,11 @@ static void osdUpdateStats(void) int16_t value = 0; #ifdef USE_GPS - value = gpsSol.groundSpeed; + if (gpsConfig()->gps_use_3d_speed) { + value = gpsSol.speed3d; + } else { + value = gpsSol.groundSpeed; + } if (stats.max_speed < value) { stats.max_speed = value; } diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 883c8c32a6..0db11f783e 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -852,7 +852,7 @@ static void osdElementGpsSats(osdElementParms_t *element) static void osdElementGpsSpeed(osdElementParms_t *element) { - tfp_sprintf(element->buff, "%c%3d%c", SYM_SPEED, osdGetSpeedToSelectedUnit(gpsSol.groundSpeed), osdGetSpeedToSelectedUnitSymbol()); + tfp_sprintf(element->buff, "%c%3d%c", SYM_SPEED, osdGetSpeedToSelectedUnit(gpsConfig()->gps_use_3d_speed ? gpsSol.speed3d : gpsSol.groundSpeed), osdGetSpeedToSelectedUnitSymbol()); } #endif // USE_GPS diff --git a/src/test/unit/link_quality_unittest.cc b/src/test/unit/link_quality_unittest.cc index 6164b49c19..73cc3a3079 100644 --- a/src/test/unit/link_quality_unittest.cc +++ b/src/test/unit/link_quality_unittest.cc @@ -83,6 +83,7 @@ extern "C" { PG_REGISTER(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, 0); PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); timeUs_t simulationTime = 0; diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 831093c715..0f36fb906e 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -92,7 +92,8 @@ extern "C" { PG_REGISTER(pilotConfig_t, pilotConfig, PG_PILOT_CONFIG, 0); PG_REGISTER(gpsRescueConfig_t, gpsRescueConfig, PG_GPS_RESCUE, 0); PG_REGISTER(imuConfig_t, imuConfig, PG_IMU_CONFIG, 0); - + PG_REGISTER(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); + timeUs_t simulationTime = 0; batteryState_e simulationBatteryState; uint8_t simulationBatteryCellCount;