diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 0d92caebe..3168729d8 100644 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -87,6 +87,7 @@ OSD_Entry menuOsdActiveElemsEntries[] = {"GPS LON", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_GPS_LON], 0}, {"HOME DIR", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HOME_DIR], 0}, {"HOME DIST", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_HOME_DIST], 0}, + {"TOTAL DIST", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_TOTAL_DIST], 0}, #endif // GPS {"COMPASS BAR", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_COMPASS_BAR], 0}, {"ALTITUDE", OME_VISIBLE, NULL, &osdConfig_item_pos[OSD_ALTITUDE], 0}, diff --git a/src/main/interface/settings.c b/src/main/interface/settings.c index 8da8d947c..624c7ca0f 100644 --- a/src/main/interface/settings.c +++ b/src/main/interface/settings.c @@ -1042,6 +1042,7 @@ const clivalue_t valueTable[] = { { "osd_gps_sats_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_GPS_SATS]) }, { "osd_home_dir_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_HOME_DIR]) }, { "osd_home_dist_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_HOME_DIST]) }, + { "osd_total_dist_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_TOTAL_DIST]) }, { "osd_compass_bar_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_COMPASS_BAR]) }, { "osd_altitude_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ALTITUDE]) }, { "osd_pid_roll_pos", VAR_UINT16 | MASTER_VALUE, .config.minmax = { 0, OSD_POSCFG_MAX }, PG_OSD_CONFIG, offsetof(osdConfig_t, item_pos[OSD_ROLL_PIDS]) }, @@ -1092,6 +1093,7 @@ const clivalue_t valueTable[] = { { "osd_stat_max_esc_temp", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_TEMP, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, { "osd_stat_max_esc_rpm", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MAX_ESC_RPM, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, { "osd_stat_min_link_quality", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_MIN_LINK_QUALITY,PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, + { "osd_stat_total_dist", VAR_UINT32 | MASTER_VALUE | MODE_BITSET, .config.bitpos = OSD_STAT_TOTAL_DISTANCE, PG_OSD_CONFIG, offsetof(osdConfig_t, enabled_stats)}, #endif diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 0f9c99b01..d5c389ad0 100644 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -77,6 +77,7 @@ static char *gpsPacketLogChar = gpsPacketLog; int32_t GPS_home[2]; uint16_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home or hol point in degrees +uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read int16_t actual_speed[2] = { 0, 0 }; int16_t nav_takeoff_bearing; @@ -1267,6 +1268,7 @@ void GPS_reset_home_position(void) 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 + GPS_distanceFlownInCm = 0; // Set ground altitude ENABLE_STATE(GPS_FIX_HOME); } @@ -1303,9 +1305,9 @@ void GPS_calculateDistanceAndDirectionToHome(void) } //////////////////////////////////////////////////////////////////////////////////// -// Calculate our current speed vector from gps position data +// Calculate our current speed vector and the distance flown from gps position data // -static void GPS_calc_velocity(void) +static void GPS_calculateVelocityAndDistanceFlown(void) { static int16_t speed_old[2] = { 0, 0 }; static int32_t last_coord[2] = { 0, 0 }; @@ -1321,6 +1323,11 @@ static void GPS_calc_velocity(void) speed_old[GPS_X] = actual_speed[GPS_X]; speed_old[GPS_Y] = actual_speed[GPS_Y]; + + uint32_t dist; + int32_t dir; + GPS_distance_cm_bearing(&gpsSol.llh.lat, &gpsSol.llh.lon, &last_coord[LAT], &last_coord[LON], &dist, &dir); + GPS_distanceFlownInCm += dist; } init = 1; @@ -1379,7 +1386,7 @@ void onGpsNewData(void) GPS_calculateDistanceAndDirectionToHome(); // calculate the current velocity based on gps coordinates continously to get a valid speed at the moment when we start navigating - GPS_calc_velocity(); + GPS_calculateVelocityAndDistanceFlown(); #ifdef USE_GPS_RESCUE rescueNewGpsData(); diff --git a/src/main/io/gps.h b/src/main/io/gps.h index f5e257be6..58b6d4978 100644 --- a/src/main/io/gps.h +++ b/src/main/io/gps.h @@ -125,6 +125,7 @@ extern char gpsPacketLog[GPS_PACKET_LOG_ENTRY_COUNT]; extern int32_t GPS_home[2]; extern uint16_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home or hol point in degrees +extern uint32_t GPS_distanceFlownInCm; // distance flown since armed in centimeters extern int16_t GPS_angle[ANGLE_INDEX_COUNT]; // it's the angles that must be applied for GPS correction extern float dTnav; // Delta Time in milliseconds for navigation computations, updated with every good GPS read extern float GPS_scaleLonDown; // this is used to offset the shrinking longitude as we go towards the poles diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 636e41729..5c51b6fd7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -605,6 +605,19 @@ static bool osdDrawSingleElement(uint8_t item) } break; + case OSD_TOTAL_DIST: + if (STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + const int32_t distance = osdGetMetersToSelectedUnit(GPS_distanceFlownInCm / 100); + tfp_sprintf(buff, "%d%c", distance, osdGetMetersToSelectedUnitSymbol()); + } else { + // We use this symbol when we don't have a FIX + buff[0] = SYM_COLON; + // overwrite any previous distance with blanks + memset(buff + 1, SYM_BLANK, 6); + buff[7] = '\0'; + } + break; + #endif // GPS case OSD_COMPASS_BAR: @@ -1159,6 +1172,7 @@ static void osdDrawElements(void) osdDrawSingleElement(OSD_GPS_LON); osdDrawSingleElement(OSD_HOME_DIST); osdDrawSingleElement(OSD_HOME_DIR); + osdDrawSingleElement(OSD_TOTAL_DIST); } #endif // GPS @@ -1556,15 +1570,17 @@ static void osdShowStats(uint16_t endBatteryVoltage) osdDisplayStatisticLabel(top++, osdTimerSourceNames[OSD_TIMER_SRC(osdConfig()->timers[OSD_TIMER_2])], buff); } - if (osdStatGetState(OSD_STAT_MAX_SPEED) && STATE(GPS_FIX)) { - itoa(stats.max_speed, buff, 10); +#ifdef USE_GPS + if (osdStatGetState(OSD_STAT_MAX_SPEED) && featureIsEnabled(FEATURE_GPS)) { + itoa(stats.max_speed, buff, 10); osdDisplayStatisticLabel(top++, "MAX SPEED", buff); } - if (osdStatGetState(OSD_STAT_MAX_DISTANCE)) { + if (osdStatGetState(OSD_STAT_MAX_DISTANCE) && featureIsEnabled(FEATURE_GPS)) { tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(stats.max_distance), osdGetMetersToSelectedUnitSymbol()); osdDisplayStatisticLabel(top++, "MAX DISTANCE", buff); } +#endif if (osdStatGetState(OSD_STAT_MIN_BATTERY)) { tfp_sprintf(buff, "%d.%1d%c", stats.min_voltage / 10, stats.min_voltage % 10, SYM_VOLT); @@ -1642,6 +1658,15 @@ static void osdShowStats(uint16_t endBatteryVoltage) osdDisplayStatisticLabel(top++, "MIN LINK", buff); } #endif + +#ifdef USE_GPS + if (osdStatGetState(OSD_STAT_TOTAL_DISTANCE) && featureIsEnabled(FEATURE_GPS)) { + const uint32_t distanceFlown = GPS_distanceFlownInCm / 100; + tfp_sprintf(buff, "%d%c", osdGetMetersToSelectedUnit(distanceFlown), osdGetMetersToSelectedUnitSymbol()); + osdDisplayStatisticLabel(top++, "TOTAL DISTANCE", buff); + } +#endif + } static void osdShowArmed(void) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index cd8038303..07ef9f280 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -100,6 +100,7 @@ typedef enum { OSD_LOG_STATUS, OSD_FLIP_ARROW, OSD_LINK_QUALITY, + OSD_TOTAL_DIST, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; @@ -131,6 +132,7 @@ typedef enum { OSD_STAT_MAX_ESC_TEMP, OSD_STAT_MAX_ESC_RPM, OSD_STAT_MIN_LINK_QUALITY, + OSD_STAT_TOTAL_DISTANCE, OSD_STAT_COUNT // MUST BE LAST } osd_stats_e; diff --git a/src/test/Makefile b/src/test/Makefile index 62ec370a3..caa91b60e 100644 --- a/src/test/Makefile +++ b/src/test/Makefile @@ -167,6 +167,7 @@ osd_unittest_SRC := \ osd_unittest_DEFINES := \ USE_OSD \ + USE_GPS= \ USE_RTC_TIME \ USE_ADC_INTERNAL diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index f7e56280b..f0b436ee6 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -28,6 +28,8 @@ extern "C" { #include "blackbox/blackbox.h" #include "blackbox/blackbox_io.h" + #include "config/feature.h" + #include "pg/pg.h" #include "pg/pg_ids.h" #include "pg/rx.h" @@ -69,6 +71,7 @@ extern "C" { uint8_t GPS_numSat; uint16_t GPS_distanceToHome; int16_t GPS_directionToHome; + uint32_t GPS_distanceFlownInCm; int32_t GPS_coord[2]; gpsSolutionData_t gpsSol; float motor[8]; @@ -95,13 +98,15 @@ extern "C" { uint16_t simulationCoreTemperature; } +uint32_t simulationFeatureFlags = FEATURE_GPS; + /* #define DEBUG_OSD */ #include "unittest_macros.h" #include "unittest_displayport.h" #include "gtest/gtest.h" -void setDefualtSimulationState() +void setDefaultSimulationState() { rssi = 1024; @@ -190,7 +195,7 @@ TEST(OsdTest, TestInit) // and // default state values are set - setDefualtSimulationState(); + setDefaultSimulationState(); // and // this battery configuration (used for battery voltage elements) @@ -308,11 +313,12 @@ TEST(OsdTest, TestStatsImperial) osdStatSetState(OSD_STAT_TIMER_2, true); osdStatSetState(OSD_STAT_RTC_DATE_TIME, true); osdStatSetState(OSD_STAT_MAX_DISTANCE, true); + osdStatSetState(OSD_STAT_TOTAL_DISTANCE, true); osdStatSetState(OSD_STAT_BLACKBOX_NUMBER, false); osdStatSetState(OSD_STAT_MAX_G_FORCE, false); osdStatSetState(OSD_STAT_MAX_ESC_TEMP, false); osdStatSetState(OSD_STAT_MAX_ESC_RPM, false); - + // and // using imperial unit system osdConfigMutable()->units = OSD_UNIT_IMPERIAL; @@ -350,6 +356,7 @@ TEST(OsdTest, TestStatsImperial) rssi = 1024; gpsSol.groundSpeed = 500; GPS_distanceToHome = 20; + GPS_distanceFlownInCm = 2000; simulationBatteryVoltage = 158; simulationAltitude = 100; simulationTime += 1e6; @@ -358,6 +365,7 @@ TEST(OsdTest, TestStatsImperial) rssi = 512; gpsSol.groundSpeed = 800; GPS_distanceToHome = 50; + GPS_distanceFlownInCm = 10000; simulationBatteryVoltage = 147; simulationAltitude = 150; simulationTime += 1e6; @@ -366,6 +374,7 @@ TEST(OsdTest, TestStatsImperial) rssi = 256; gpsSol.groundSpeed = 200; GPS_distanceToHome = 100; + GPS_distanceFlownInCm = 20000; simulationBatteryVoltage = 152; simulationAltitude = 200; simulationTime += 1e6; @@ -387,6 +396,7 @@ TEST(OsdTest, TestStatsImperial) displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT); displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%"); displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 6.5%c", SYM_FT); + displayPortTestBufferSubstring(2, row++, "TOTAL DISTANCE : 656%c", SYM_FT); } /* @@ -401,7 +411,7 @@ TEST(OsdTest, TestStatsMetric) // and // default state values are set - setDefualtSimulationState(); + setDefaultSimulationState(); // when // the craft is armed @@ -412,6 +422,7 @@ TEST(OsdTest, TestStatsMetric) rssi = 256; gpsSol.groundSpeed = 800; GPS_distanceToHome = 100; + GPS_distanceFlownInCm = 10000; simulationBatteryVoltage = 147; simulationAltitude = 200; simulationTime += 1e6; @@ -438,6 +449,7 @@ TEST(OsdTest, TestStatsMetric) displayPortTestBufferSubstring(2, row++, "END BATTERY : 15.2%c", SYM_VOLT); displayPortTestBufferSubstring(2, row++, "MIN RSSI : 25%%"); displayPortTestBufferSubstring(2, row++, "MAX ALTITUDE : 2.0%c", SYM_M); + displayPortTestBufferSubstring(2, row++, "TOTAL DISTANCE : 100%c", SYM_M); } /* @@ -447,7 +459,7 @@ TEST(OsdTest, TestAlarms) { // given // default state is set - setDefualtSimulationState(); + setDefaultSimulationState(); // and // the following OSD elements are visible @@ -959,6 +971,8 @@ TEST(OsdTest, TestConvertTemperatureUnits) // STUBS extern "C" { + bool featureIsEnabled(uint32_t f) { return simulationFeatureFlags & f; } + void beeperConfirmationBeeps(uint8_t) {} bool isModeActivationConditionPresent(boxId_e) {