Code allowing the use of sonar without requiring a barometer.

This code has been flight tested on a Naze32 acro, with no barometer onboard.  It also works when the target doesn't have BARO defined.

src/main/config/runtime_config.h

@@ -40,6 +40,7 @@ typedef enum {
     HEADFREE_MODE   = (1 << 6),
     AUTOTUNE_MODE   = (1 << 7),
     PASSTHRU_MODE   = (1 << 8),
+    SONAR_MODE       = (1 << 9),
 } flightModeFlags_e;
 
 extern uint16_t flightModeFlags;

src/main/flight/altitudehold.c

@@ -58,7 +58,7 @@
 #include "config/config_profile.h"
 #include "config/config_master.h"
 
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
 static int16_t initialThrottleHold;
 
 static void multirotorAltHold(void)
@@ -79,7 +79,7 @@ static void multirotorAltHold(void)
             rcCommand[THROTTLE] = constrain(initialThrottleHold + BaroPID, masterConfig.escAndServoConfig.minthrottle, masterConfig.escAndServoConfig.maxthrottle);
         }
     } else {
-        // slow alt changes for apfags
+        // slow alt changes, mostly used for aerial photography
         if (abs(rcCommand[THROTTLE] - initialThrottleHold) > currentProfile->alt_hold_deadband) {
             // set velocity proportional to stick movement +100 throttle gives ~ +50 cm/s
             setVelocity = (rcCommand[THROTTLE] - initialThrottleHold) / 2;
@@ -128,4 +128,20 @@ void updateAltHoldState(void)
     }
 }
 
+void updateSonarAltHoldState(void)
+{
+	// Sonar alt hold activate
+	if (rcOptions[BOXSONAR]) {
+		if (!FLIGHT_MODE(SONAR_MODE)) {
+			ENABLE_FLIGHT_MODE(SONAR_MODE);
+			AltHold = EstAlt;
+			initialThrottleHold = rcCommand[THROTTLE];
+			errorVelocityI = 0;
+			BaroPID = 0;	// TODO: Change naming of BaroPID to "AltPID" as this is used by both sonar and baro
+		}
+	} else {
+		DISABLE_FLIGHT_MODE(SONAR_MODE);
+	}
+}
+
 #endif

src/main/flight/altitudehold.h

@@ -18,4 +18,5 @@
 
 void updateAltHold(void);
 void updateAltHoldState(void);
+void updateSonarAltHoldState(void);
 

src/main/flight/imu.c

@@ -372,7 +372,8 @@ int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value)
     return lrintf(throttle_correction_value * sinf(angle / (900.0f * M_PI / 2.0f)));
 }
 
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
+
 // 40hz update rate (20hz LPF on acc)
 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
 
@@ -453,6 +454,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
         
     previousTime = currentTime;
 
+#ifdef BARO
     if (!isBaroCalibrationComplete()) {
         performBaroCalibrationCycle();
         vel = 0;
@@ -460,6 +462,10 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     }
 
     BaroAlt = baroCalculateAltitude();
+#else
+	BaroAlt = 0;
+#endif
+
 
 #ifdef SONAR
     tiltAngle = calculateTiltAngle(&inclination);
@@ -496,9 +502,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     accSum_reset();
 
+#ifdef BARO
     if (!isBaroCalibrationComplete()) {
         return;
     }
+#endif
 
     if (sonarAlt > 0 && sonarAlt < 200) {
         // the sonar has the best range

src/main/io/rc_controls.h

@@ -22,7 +22,7 @@ enum {
     BOXANGLE,
     BOXHORIZON,
     BOXBARO,
-    // BOXVARIO,
+    BOXSONAR,
     BOXMAG,
     BOXHEADFREE,
     BOXHEADADJ,

src/main/io/serial_msp.c

@@ -149,7 +149,7 @@ static const box_t const boxes[] = {
     { BOXANGLE, "ANGLE;", 1 },
     { BOXHORIZON, "HORIZON;", 2 },
     { BOXBARO, "BARO;", 3 },
-    //{ BOXVARIO, "VARIO;", 4 },
+    { BOXSONAR, "SONAR;", 4 },
     { BOXMAG, "MAG;", 5 },
     { BOXHEADFREE, "HEADFREE;", 6 },
     { BOXHEADADJ, "HEADADJ;", 7 },
@@ -421,6 +421,10 @@ void mspInit(serialConfig_t *serialConfig)
         activeBoxIds[activeBoxIdCount++] = BOXBARO;
     }
 
+	if (feature(FEATURE_SONAR)){
+		activeBoxIds[activeBoxIdCount++] = BOXSONAR;
+	}
+
     if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) {
         activeBoxIds[activeBoxIdCount++] = BOXMAG;
         activeBoxIds[activeBoxIdCount++] = BOXHEADFREE;
@@ -491,6 +495,7 @@ static bool processOutCommand(uint8_t cmdMSP)
         tmp = IS_ENABLED(FLIGHT_MODE(ANGLE_MODE)) << BOXANGLE |
             IS_ENABLED(FLIGHT_MODE(HORIZON_MODE)) << BOXHORIZON |
             IS_ENABLED(FLIGHT_MODE(BARO_MODE)) << BOXBARO |
+            IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
             IS_ENABLED(FLIGHT_MODE(MAG_MODE)) << BOXMAG |
             IS_ENABLED(FLIGHT_MODE(HEADFREE_MODE)) << BOXHEADFREE |
             rcOptions[BOXHEADADJ] << BOXHEADADJ |

src/main/mw.c

@@ -375,10 +375,12 @@ typedef enum {
 #endif
 #ifdef BARO
 	UPDATE_BARO_TASK,
-	CALCULATE_ALTITUDE_TASK,
 #endif
 #ifdef SONAR
 	UPDATE_SONAR_TASK,
+#endif
+#if defined(BARO) || defined(SONAR)
+	CALCULATE_ALTITUDE_TASK,
 #endif
 	UPDATE_GPS_TASK
 } periodicTasks;
@@ -405,9 +407,20 @@ void executePeriodicTasks(void)
             baroUpdate(currentTime);
         }
         break;
+#endif
 
+#if defined(BARO) || defined(SONAR)
     case CALCULATE_ALTITUDE_TASK:
+
+#if defined(BARO) && !defined(SONAR)
         if (sensors(SENSOR_BARO) && isBaroReady()) {
+#endif
+#if defined(BARO) && defined(SONAR)
+        if ((sensors(SENSOR_BARO) && isBaroReady()) || sensors(SENSOR_SONAR)) {
+#endif
+#if !defined(BARO) && defined(SONAR)
+        if (sensors(SENSOR_SONAR)) {
+#endif
             calculateEstimatedAltitude(currentTime);
         }
         break;
@@ -548,7 +561,7 @@ void processRx(void)
 void loop(void)
 {
     static uint32_t loopTime;
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
     static bool haveProcessedAnnexCodeOnce = false;
 #endif
 
@@ -565,6 +578,16 @@ void loop(void)
 		}
 	}
 #endif
+
+#ifdef SONAR
+	// the 'annexCode' initialses rcCommand, updateAltHoldState depends on valid rcCommand data.
+	if (haveProcessedAnnexCodeOnce) {
+		if (sensors(SENSOR_SONAR)) {
+			updateSonarAltHoldState();
+		}
+	}
+#endif
+
     } else {
         // not processing rx this iteration
         executePeriodicTasks();
@@ -582,7 +605,7 @@ void loop(void)
         previousTime = currentTime;
 
         annexCode();
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;
 #endif
 
@@ -596,13 +619,12 @@ void loop(void)
         }
 #endif
 
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
-        if (sensors(SENSOR_BARO)) {
+        if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
-            if (FLIGHT_MODE(BARO_MODE)) {
+            if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
                 updateAltHold();
             }
         }
-
 #endif
 
         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {

src/main/sensors/initialisation.c

@@ -363,8 +363,8 @@ static void detectBaro()
         return;
     }
 #endif
-    sensorsClear(SENSOR_BARO);
 #endif
+    sensorsClear(SENSOR_BARO);
 }
 
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)