Added variometer variable

Add MESSAGES for OSD

src/main/fc/fc_msp.c

@@ -99,6 +99,7 @@
 #include "msp/msp_serial.h"
 
 #include "navigation/navigation.h"
+#include "navigation/navigation_private.h"
 #include "navigation/navigation_pos_estimator_private.h"
 
 #include "rx/rx.h"
@@ -3244,33 +3245,48 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
         sbufWriteU8(dst, ARMING_FLAG(ARMED)); // arming status
         sbufWriteU8(dst, 0); // arming status
         sbufWriteU32(dst, flightModeFlags);
-        // "1" for error messages
+
+        sbufWriteU8(dst, 1); // messages
+        char buff[50] = {" "};
+        osdGetSystemMessage(buff, sizeof(buff), false);
+        const char *c = buff;
+        while (*c) {
+            sbufWriteU8(dst, *c++);
+        }
+        sbufWriteU8(dst, '\0'); // last symbol
+
         sbufWriteU8(dst, 2); // home position
         sbufWriteU16(dst, GPS_distanceToHome);
         sbufWriteU16(dst, GPS_directionToHome);
         sbufWriteU32(dst, baro.BaroAlt);
         sbufWriteU8(dst, 3); // throttle
         sbufWriteU8(dst, getThrottlePercent());
+        sbufWriteU8(dst, 4); // vario
+        sbufWriteU16(dst, getEstimatedActualVelocity(Z) + 32000);
 
         *ret = MSP_RESULT_ACK;
         break;
     case MSP_SIMULATOR:
         tmp_u8 = sbufReadU8(src);
-        if (tmp_u8 == 1) {
+        if (tmp_u8 >= 1) {
             if (! ARMING_FLAG(SIMULATOR_MODE)) {
                 baroStartCalibration(); // just once
             }
             ENABLE_ARMING_FLAG(SIMULATOR_MODE);
+
+            switch (tmp_u8) {
+                case 0x02: // RST HOME
+                    setHomePosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, NAV_HOME_VALID_ALL);
+                    baroStartCalibration();
+                    break;
+                case 0x03:
+                    break;
+            }
         } else {
             DISABLE_ARMING_FLAG(SIMULATOR_MODE);
         }
         if (dataSize >= 14) {
             gpsSol.fixType = sbufReadU8(src);
-            if (gpsSol.fixType) {
-                ENABLE_STATE(GPS_FIX);
-            } else {
-                DISABLE_STATE(GPS_FIX);
-            }
             gpsSol.flags.hasNewData = true;
             gpsSol.flags.validVelNE = 0;
             gpsSol.flags.validVelD = 0;
@@ -3287,8 +3303,7 @@ bool mspFCProcessInOutCommand(uint16_t cmdMSP, sbuf_t *dst, sbuf_t *src, mspResu
             gpsSol.eph = 100;
             gpsSol.epv = 100;
             // Feed data to navigation
-            sensorsSet(SENSOR_GPS);
+            gpsProcessNewSolutionData();
-            onNewGPSData();
 
             attitude.values.roll = sbufReadU16(src)-1800;
             attitude.values.pitch = sbufReadU16(src)-1800;

src/main/io/osd.c

@@ -4219,6 +4219,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     osdFormatDistanceSymbol(buf, posControl.wpDistance, 0);
                     tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", getGeoWaypointNumber(posControl.activeWaypointIndex), posControl.geoWaypointCount, buf);
                     messages[messageCount++] = messageBuf;
+                } else if (NAV_Status.state == MW_NAV_STATE_RTH_ENROUTE) {
+                    // Countdown display for RTH minutes
+                    char buf[6];
+                    osdFormatDistanceSymbol(buf, GPS_distanceToHome * 60 * 60 * 3.6/ gpsSol.groundSpeed , 0);
+                    tfp_sprintf(messageBuf, "EN ROUTE TO HOME (%)", buf);
+                    messages[messageCount++] = messageBuf;
                 } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) {
                     if (navConfig()->general.flags.waypoint_enforce_altitude && !posControl.wpAltitudeReached) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ADJUSTING_WP_ALT);

src/main/navigation/navigation_fixedwing.c

@@ -146,7 +146,8 @@ static void updateAltitudeVelocityAndPitchController_FW(timeDelta_t deltaMicros)
     //  speedWeight = 1.0 : pitch will only control airspeed and won't control altitude
     //  speedWeight = 0.5 : pitch will be used to control both airspeed and altitude
     //  speedWeight = 0.0 : pitch will only control altitude
-    const float speedWeight = 0.0f; // no speed sensing for now
+    const float speedWeight = 0.7f; // no speed sensing for now
+//    const float speedWeight = 1.0f - constrainf(fabs(demSPE - estSPE) / GRAVITY_MSS, 0.0f, 1.0f); // no speed sensing for now
 
     const float demSEB = demSPE * (1.0f - speedWeight) - demSKE * speedWeight;
     const float estSEB = estSPE * (1.0f - speedWeight) - estSKE * speedWeight;

src/main/navigation/navigation_pos_estimator.c

@@ -448,6 +448,9 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
 
         /* If calibration is incomplete - report zero acceleration */
         if (gravityCalibrationComplete()) {
+            if (ARMING_FLAG(SIMULATOR_MODE)) {
+                posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
+            }
             posEstimator.imu.accelNEU.z -= posEstimator.imu.calibratedGravityCMSS;
         }
         else {
@@ -556,6 +559,15 @@ static void estimationPredict(estimationContext_t * ctx)
 
 static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
 {
+    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 2, imuMeasuredAccelBF.z);        // Baro altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
+    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
+    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
+    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
+    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
+    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
+
     if (ctx->newFlags & EST_BARO_VALID) {
         timeUs_t currentTimeUs = micros();
 
@@ -626,15 +638,6 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx)
         return true;
     }
 
-    DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z);       // Position estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt);        // Baro altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z);       // GPS altitude
-    DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel());       // Vibration level
-    DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z);       // Vertical speed estimate
-    DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z);  // Vertical acceleration on earth frame
-    DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z);       // GPS vertical speed
-    DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount());            // Clip count
-
     return false;
 }
 

src/main/sensors/acceleration.c

@@ -569,6 +569,7 @@ void accUpdate(void)
     // Filter acceleration
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
         acc.accADCf[axis] = accSoftLpfFilterApplyFn(accSoftLpfFilter[axis], acc.accADCf[axis]);
+        DEBUG_SET(DEBUG_ACC, axis, acc.accADCf[axis] * 1000);
     }
 
     if (accelerometerConfig()->acc_notch_hz) {