From 6a9066d103f0d3fa7da6d1fda923449741d81148 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 6 Jun 2026 12:25:29 +0100 Subject: [PATCH] add common 3D speed --- src/main/io/osd.c | 4 ++-- src/main/io/osd_common.c | 19 ++++--------------- src/main/io/osd_common.h | 4 ---- src/main/io/osd_dji_hd.c | 2 +- src/main/navigation/navigation.c | 10 ++++++---- src/main/navigation/navigation_private.h | 1 + src/main/programming/logic_condition.c | 13 ++++++------- 7 files changed, 20 insertions(+), 33 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d2ee2f1246..62e17bd6b9a 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1984,7 +1984,7 @@ static bool osdDrawSingleElement(uint8_t item) break; case OSD_3D_SPEED: - osdFormatVelocityStr(buff, osdGet3DSpeed(), OSD_SPEED_TYPE_3D, false); + osdFormatVelocityStr(buff, posControl.actualState.vel3D, OSD_SPEED_TYPE_3D, false); break; case OSD_3D_MAX_SPEED: @@ -4862,7 +4862,7 @@ static void osdUpdateStats(void) int32_t value; if (feature(FEATURE_GPS)) { - value = osdGet3DSpeed(); + value = posControl.actualState.vel3D; const float airspeed_estimate = getAirspeedEstimate(); if (stats.max_3D_speed < value) stats.max_3D_speed = value; diff --git a/src/main/io/osd_common.c b/src/main/io/osd_common.c index 38563ed5d88..009a6559618 100644 --- a/src/main/io/osd_common.c +++ b/src/main/io/osd_common.c @@ -42,6 +42,8 @@ #include "io/osd_grid.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" + #include "sensors/pitotmeter.h" #if defined(USE_OSD) || defined(USE_DJI_HD_OSD) @@ -59,7 +61,7 @@ int16_t osdGetSpeedFromSelectedSource(void) { speed = gpsSol.groundSpeed; break; case OSD_SPEED_SOURCE_3D: - speed = osdGet3DSpeed(); + speed = posControl.actualState.vel3D; break; case OSD_SPEED_SOURCE_AIR: #ifdef USE_PITOT @@ -164,7 +166,7 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c { uint8_t gx; uint8_t gy; - + #if defined(USE_CANVAS) if (canvas) { osdCanvasDrawArtificialHorizon(display, canvas, p, pitchAngle, rollAngle); @@ -209,17 +211,4 @@ void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas) #endif osdGridDrawSidebars(display); } - -#endif - -#ifdef USE_GPS -/* - * 3D speed in cm/s - */ -int16_t osdGet3DSpeed(void) -{ - float vert_speed = getEstimatedActualVelocity(Z); - float hor_speed = (float)gpsSol.groundSpeed; - return (int16_t)calc_length_pythagorean_2D(hor_speed, vert_speed); -} #endif diff --git a/src/main/io/osd_common.h b/src/main/io/osd_common.h index a032747effc..0831a36e678 100644 --- a/src/main/io/osd_common.h +++ b/src/main/io/osd_common.h @@ -105,7 +105,3 @@ void osdDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *canvas, c // grid slots. void osdDrawHeadingGraph(displayPort_t *display, displayCanvas_t *canvas, const osdDrawPoint_t *p, int heading); void osdDrawSidebars(displayPort_t *display, displayCanvas_t *canvas); - -#ifdef USE_GPS -int16_t osdGet3DSpeed(void); -#endif diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 086c9a6965f..3a69a0c77e4 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -681,7 +681,7 @@ void osdDJIFormatVelocityStr(char* buff) break; case OSD_SPEED_SOURCE_3D: strcpy(sourceBuf, "3D"); - vel = osdGet3DSpeed(); + vel = posControl.actualState.vel3D; break; case OSD_SPEED_SOURCE_AIR: strcpy(sourceBuf, "AIR"); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index d640e2b25c3..5b4f38e686c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2852,6 +2852,8 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo posControl.actualState.agl.pos.z = surfaceDistance; posControl.actualState.agl.vel.z = surfaceVelocity; + posControl.actualState.vel3D = calc_length_pythagorean_2D(posControl.actualState.velXY, posControl.actualState.abs.vel.z); + // Update altitude that would be used when executing RTH if (estimateValid) { updateDesiredRTHAltitude(); @@ -3874,10 +3876,10 @@ void getWaypoint(uint8_t wpNumber, navWaypoint_t * wpData) int isGCSValid(void) { - return (ARMING_FLAG(ARMED) && - (posControl.flags.estPosStatus >= EST_TRUSTED) && - posControl.gpsOrigin.valid && - posControl.flags.isGCSAssistedNavigationEnabled && + return (ARMING_FLAG(ARMED) && + (posControl.flags.estPosStatus >= EST_TRUSTED) && + posControl.gpsOrigin.valid && + posControl.flags.isGCSAssistedNavigationEnabled && (posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)); } diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6f076659caf..da4d27c6710 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -137,6 +137,7 @@ typedef struct { float cosYaw; float surfaceMin; float velXY; + float vel3D; } navigationEstimatedState_t; typedef struct { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 3f32fcee861..8c988d78384 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -747,9 +747,8 @@ static int logicConditionGetFlightOperandValue(int operand) { return getMinGroundSpeed(navConfig()->general.min_ground_speed); break; - //FIXME align with osdGet3DSpeed case LOGIC_CONDITION_OPERAND_FLIGHT_3D_SPEED: // cm/s - return osdGet3DSpeed(); + return posControl.actualState.vel3D; break; case LOGIC_CONDITION_OPERAND_FLIGHT_AIR_SPEED: // cm/s @@ -781,7 +780,7 @@ static int logicConditionGetFlightOperandValue(int operand) { uint16_t windAngle; getEstimatedHorizontalWindSpeed(&windAngle); int32_t windHeading = (int32_t)windAngle + 18000; // Correct heading to display correctly. - + while (windHeading < 0) windHeading += 36000; while (windHeading >= 36000) windHeading -= 36000; @@ -801,10 +800,10 @@ static int logicConditionGetFlightOperandValue(int operand) { uint16_t windAngle; getEstimatedHorizontalWindSpeed(&windAngle); int32_t relativeWindHeading = (int32_t)windAngle + 18000 - DECIDEGREES_TO_CENTIDEGREES(attitude.values.yaw); - + while (relativeWindHeading < 0) relativeWindHeading += 36000; while (relativeWindHeading >= 36000) relativeWindHeading -= 36000; - + relativeWindHeading = -relativeWindHeading; if (relativeWindHeading <= -18000) relativeWindHeading = 18000 + (relativeWindHeading + 18000); @@ -816,7 +815,7 @@ static int logicConditionGetFlightOperandValue(int operand) { #else return 0; #endif - break; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_ALTITUDE: // cm return constrain(getEstimatedActualPosition(Z), INT32_MIN, INT32_MAX); @@ -908,7 +907,7 @@ static int logicConditionGetFlightOperandValue(int operand) { return rxLinkStatistics.uplinkRSSI; #else return 0; -#endif +#endif break; case LOGIC_CONDITION_OPERAND_FLIGHT_LQ_DOWNLINK: