Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-15 12:25:20 +03:00
Code Issues Wiki Activity

Feature: show 3d speed on OSD

Browse source
This commit is contained in:
Tony Cabello 2019-08-23 03:00:15 +02:00
parent b215def714
commit 0ddcfc097c
7 changed files with 21 additions and 7 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/cli/settings.c
View file

@ -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

13
src/main/io/gps.c
View file

@ -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;
}
}

2
src/main/io/gps.h
View file

@ -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)

6
src/main/osd/osd.c
View file

@ -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;
}

2
src/main/osd/osd_elements.c
View file

@ -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

1
src/test/unit/link_quality_unittest.cc
View file

@ -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;

1
src/test/unit/osd_unittest.cc
View file

@ -92,6 +92,7 @@ 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;