Merge pull request #11628 from breadoven/abo_common_3Dspeed

Add common 3D speed

Author: breadoven

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;

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

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

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");

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

src/main/navigation/navigation_private.h

@@ -137,6 +137,7 @@ typedef struct {
     float                   cosYaw;
     float                   surfaceMin;
     float                   velXY;
+    float                   vel3D;
 } navigationEstimatedState_t;
 
 typedef struct {

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: