CR157 + CR156 + Pitot Virtual fix

Author: breadoven

docs/Settings.md

@@ -5644,7 +5644,7 @@ Defines rotation rate on PITCH axis that UAV will try to archive on max. stick d
 
 ### pitot_hardware
 
-Selection of pitot hardware.
+Selection of pitot hardware. VIRTUAL only works if a GPS is enabled.
 
 | Default | Min | Max |
 | --- | --- | --- |

src/main/blackbox/blackbox.c

@@ -504,7 +504,7 @@ typedef struct blackboxMainState_s {
     int16_t gyroPeaksYaw[DYN_NOTCH_PEAK_COUNT];
 
     int16_t accADC[XYZ_AXIS_COUNT];
-    int16_t accVib;
+    uint16_t accVib;  // CR157
     int16_t attitude[XYZ_AXIS_COUNT];
     int32_t debug[DEBUG32_VALUE_COUNT];
     int16_t motor[MAX_SUPPORTED_MOTORS];
@@ -1642,7 +1642,8 @@ static void loadMainState(timeUs_t currentTimeUs)
         blackboxCurrent->axisPID_D[i] = axisPID_D[i];
         blackboxCurrent->axisPID_F[i] = axisPID_F[i];
         blackboxCurrent->gyroADC[i] = lrintf(gyro.gyroADCf[i]);
-        blackboxCurrent->accADC[i] = lrintf(acc.accADCf[i] * acc.dev.acc_1G);
+        // blackboxCurrent->accADC[i] = constrain(lrintf(acc.accADCf[i] * acc.dev.acc_1G), -32678, 32767);
+        blackboxCurrent->accADC[i] = 100 * lrintf(acc.accADCf[i]);  // CR157
         blackboxCurrent->gyroRaw[i] = lrintf(gyro.gyroRaw[i]);
 
 #ifdef USE_DYNAMIC_FILTERS
@@ -1668,7 +1669,8 @@ static void loadMainState(timeUs_t currentTimeUs)
             blackboxCurrent->mcVelAxisOutput[i] = lrintf(nav_pids->vel[i].output_constrained);
         }
     }
-    blackboxCurrent->accVib = lrintf(accGetVibrationLevel() * acc.dev.acc_1G);
+    // blackboxCurrent->accVib = constrain(lrintf(accGetVibrationLevel() * acc.dev.acc_1G), 0, 65535);
+    blackboxCurrent->accVib = 100 * lrintf(accGetVibrationLevel());   // CR157
 
     if (STATE(FIXED_WING_LEGACY)) {
 

src/main/fc/cli.c

@@ -4104,13 +4104,14 @@ static void cliStatus(char *cmdline)
 #endif // for if at32
 #endif // for SITL
 
-    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, GPS=%s",
+    cliPrintLinef("Sensor status: GYRO=%s, ACC=%s, MAG=%s, BARO=%s, RANGEFINDER=%s, OPFLOW=%s, PITOT=%s, GPS=%s",
         hardwareSensorStatusNames[getHwGyroStatus()],
         hardwareSensorStatusNames[getHwAccelerometerStatus()],
         hardwareSensorStatusNames[getHwCompassStatus()],
         hardwareSensorStatusNames[getHwBarometerStatus()],
         hardwareSensorStatusNames[getHwRangefinderStatus()],
         hardwareSensorStatusNames[getHwOpticalFlowStatus()],
+        hardwareSensorStatusNames[getHwPitotmeterStatus()],
         hardwareSensorStatusNames[getHwGPSStatus()]
     );
 

src/main/fc/settings.yaml

@@ -667,7 +667,7 @@ groups:
     condition: USE_PITOT
     members:
       - name: pitot_hardware
-        description: "Selection of pitot hardware."
+        description: "Selection of pitot hardware. VIRTUAL only works if a GPS is enabled."
         default_value: "NONE"
         table: pitot_hardware
       - name: pitot_lpf_milli_hz

src/main/io/osd.c

@@ -6163,7 +6163,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
         char messageBuf[MAX(SETTING_MAX_NAME_LENGTH, OSD_MESSAGE_LENGTH + 1)];
 
         /* WARNING: ensure number of messages returned does not exceed messages array size
-         * Messages array set 1 larger than maximum expected message count of 6 */
+         * Messages array set 1 larger than maximum expected message count of 7 */
         const char *messages[8];
         unsigned messageCount = 0;
 
@@ -6347,12 +6347,21 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter
                     } else if (FLIGHT_MODE(HEADFREE_MODE)) {
                         messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_HEADFREE);
                     }
-                    if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && !navigationRequiresAngleMode()) {
-                        /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes
-                         * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field.
-                         * In this case indicate ALTHOLD is active via a system message */
-
-                        messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
+                    if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) {  // CR156
+                        if (posControl.flags.isTerrainFollowEnabled) {
+                            if (posControl.flags.estAglStatus == EST_TRUSTED) {
+                                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_OK);
+                            } else {
+                                messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_SURFACE_BAD);
+                            }
+                        } else if (!navigationRequiresAngleMode()) {
+                            /* If ALTHOLD is separately enabled for multirotor together with ANGL/HORIZON/ACRO modes
+                             * then ANGL/HORIZON/ACRO are indicated by the OSD_FLYMODE field.
+                             * In this case indicate ALTHOLD is active via a system message */
+
+                            messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD);
+                        }
+                        // CR156
                     }
                 }
             }

src/main/io/osd.h

@@ -123,6 +123,8 @@
 #define OSD_MSG_ANGLEHOLD_PITCH     "(ANGLEHOLD PITCH)"
 #define OSD_MSG_ANGLEHOLD_LEVEL     "(ANGLEHOLD LEVEL)"
 #define OSD_MSG_MOVE_STICKS         "MOVE STICKS TO ABORT"
+#define OSD_MSG_SURFACE_OK          "(SURFACE)"  // CR156
+#define OSD_MSG_SURFACE_BAD         "(!SURFACE INOP!)"  // CR156
 #define OSD_MSG_TOILET_BOWL         "!MAG BAD>FIX ATTEMPTED!"   // CR141
 // #define OSD_MSG_POSEST_DEGRADED     "!POSITION ACCURACY POOR!"   // CR142
 

src/main/navigation/navigation_multicopter.c

@@ -902,9 +902,15 @@ bool isMulticopterLandingDetected(void)
      * Detection active during Failsafe only if throttle below mid hover throttle
      * and WP mission not active (except landing states).
      * Also active in non autonomous flight modes but only when thottle low */
-    bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG))
-                          || (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover())
-                          || (!navigationIsFlyingAutonomousMode() && throttleStickIsLow());
+    // CR156
+    bool throttleLowCheckAllowed = !navigationIsFlyingAutonomousMode();
+    if (posControl.flags.isTerrainFollowEnabled) {
+        throttleLowCheckAllowed = throttleLowCheckAllowed && posControl.flags.estAglStatus == EST_TRUSTED && posControl.actualState.agl.pos.z < 10.0f;
+    }
+    // CR156
+    bool startCondition = (navGetCurrentStateFlags() & (NAV_CTL_LAND | NAV_CTL_EMERG)) ||
+                          (FLIGHT_MODE(FAILSAFE_MODE) && !FLIGHT_MODE(NAV_WP_MODE) && !isMulticopterThrottleAboveMidHover()) ||
+                          (throttleLowCheckAllowed && throttleStickIsLow());  // CR156
     // CR137
     if (FLIGHT_MODE(NAV_RTH_MODE)) {
         startCondition = startCondition && posControl.actualState.abs.pos.z < 3000;

src/main/sensors/acceleration.c

@@ -586,8 +586,8 @@ void accUpdate(void)
         // calc difference from this sample and 5hz filtered value, square and filter at 2hz
         const float accDiff = acc.accADCf[axis] - accFloorFilt;
         acc.accVibeSq[axis] = pt1FilterApply(&accVibeFilter[axis], accDiff * accDiff);
-        acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);
     }
+    acc.accVibe = fast_fsqrtf(acc.accVibeSq[X] + acc.accVibeSq[Y] + acc.accVibeSq[Z]);  // CR157
 
     // Filter acceleration
     for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) {
@@ -599,7 +599,6 @@ void accUpdate(void)
             acc.accADCf[axis] = accNotchFilterApplyFn(accNotchFilter[axis], acc.accADCf[axis]);
         }
     }
-
 }
 
 // Record extremes: min/max for each axis and acceleration vector modulus

src/main/sensors/pitotmeter.c

@@ -134,17 +134,30 @@ bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse)
             FALLTHROUGH;
 
         case PITOT_VIRTUAL:
-#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL) 
-            if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) {
-                pitotHardware = PITOT_VIRTUAL;
-                break;
-            }
-#endif
-            /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
             if (pitotHardwareToUse != PITOT_AUTODETECT) {
+#if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
+                if (virtualPitotDetect(dev)) {
+                    pitotHardware = PITOT_VIRTUAL;
+                    break;
+                }
+#endif
+                requestedSensors[SENSOR_INDEX_PITOT] = PITOT_NONE;
                 break;
             }
+
             FALLTHROUGH;
+        // case PITOT_VIRTUAL:
+// #if defined(USE_WIND_ESTIMATOR) && defined(USE_PITOT_VIRTUAL)
+            // if ((pitotHardwareToUse != PITOT_AUTODETECT) && virtualPitotDetect(dev)) {
+                // pitotHardware = PITOT_VIRTUAL;
+                // break;
+            // }
+// #endif
+            // /* If we are asked for a specific sensor - break out, otherwise - fall through and continue */
+            // if (pitotHardwareToUse != PITOT_AUTODETECT) {
+                // break;
+            // }
+            // FALLTHROUGH;
 
         case PITOT_MSP:
 #ifdef USE_PITOT_MSP
@@ -240,29 +253,29 @@ STATIC_PROTOTHREAD(pitotThread)
         if ( pitot.lastSeenHealthyMs == 0 ) {
             if (pitot.dev.start(&pitot.dev)) {
                 pitot.lastSeenHealthyMs = millis();
-            }        
+            }
         }
 
         if ( (millis() - pitot.lastSeenHealthyMs) >= US2MS(pitot.dev.delay)) {
             if (pitot.dev.get(&pitot.dev))          // read current data
                 pitot.lastSeenHealthyMs = millis();
 
             if (pitot.dev.start(&pitot.dev))        // init for next read
-                pitot.lastSeenHealthyMs = millis();        
+                pitot.lastSeenHealthyMs = millis();
         }
 
 
         pitot.dev.calculate(&pitot.dev, &pitotPressureTmp, &pitotTemperatureTmp);
 
 #ifdef USE_SIMULATOR
         if (SIMULATOR_HAS_OPTION(HITL_AIRSPEED)) {
-            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
+            pitotPressureTmp = sq(simulatorData.airSpeed) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
         }
 #endif
 #if defined(USE_PITOT_FAKE)
-        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
-            pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;     
-        } 
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
+            pitotPressureTmp = sq(fakePitotGetAirspeed()) * SSL_AIR_DENSITY / 20000.0f + SSL_AIR_PRESSURE;
+        }
 #endif
         ptYield();
 
@@ -297,7 +310,7 @@ STATIC_PROTOTHREAD(pitotThread)
         }
 
 #if defined(USE_PITOT_FAKE)
-        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) { 
+        if (pitotmeterConfig()->pitot_hardware == PITOT_FAKE) {
             pitot.airSpeed = fakePitotGetAirspeed();
         }
 #endif