diff --git a/src/main/config/config.c b/src/main/config/config.c
index 197edd9d34..b90e49e5e4 100755
--- a/src/main/config/config.c
+++ b/src/main/config/config.c
@@ -99,7 +99,7 @@ void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DCon
 master_t masterConfig;      // master config struct with data independent from profiles
 profile_t *currentProfile;   // profile config struct
 
-static const uint8_t EEPROM_CONF_VERSION = 79;
+static const uint8_t EEPROM_CONF_VERSION = 80;
 
 static void resetAccelerometerTrims(flightDynamicsTrims_t *accelerometerTrims)
 {
diff --git a/src/main/config/runtime_config.h b/src/main/config/runtime_config.h
index 2ff818a96d..4a8f73cca1 100644
--- a/src/main/config/runtime_config.h
+++ b/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;
diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c
index 526100e86b..92a59fcdf8 100644
--- a/src/main/flight/altitudehold.c
+++ b/src/main/flight/altitudehold.c
@@ -62,7 +62,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)
@@ -83,7 +83,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;
@@ -119,16 +119,34 @@ void updateAltHold(void)
 void updateAltHoldState(void)
 {
     // Baro alt hold activate
-    if (rcOptions[BOXBARO]) {
-        if (!FLIGHT_MODE(BARO_MODE)) {
-            ENABLE_FLIGHT_MODE(BARO_MODE);
-            AltHold = EstAlt;
-            initialThrottleHold = rcCommand[THROTTLE];
-            errorVelocityI = 0;
-            BaroPID = 0;
-        }
-    } else {
+    if (!rcOptions[BOXBARO]) {
         DISABLE_FLIGHT_MODE(BARO_MODE);
+        return;
+    }
+
+    if (!FLIGHT_MODE(BARO_MODE)) {
+        ENABLE_FLIGHT_MODE(BARO_MODE);
+        AltHold = EstAlt;
+        initialThrottleHold = rcCommand[THROTTLE];
+        errorVelocityI = 0;
+        BaroPID = 0;
+    }
+}
+
+void updateSonarAltHoldState(void)
+{
+    // Sonar alt hold activate
+    if (!rcOptions[BOXSONAR]) {
+        DISABLE_FLIGHT_MODE(SONAR_MODE);
+        return;
+    }
+
+    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
     }
 }
 
diff --git a/src/main/flight/altitudehold.h b/src/main/flight/altitudehold.h
index ada049f187..252c12f78f 100644
--- a/src/main/flight/altitudehold.h
+++ b/src/main/flight/altitudehold.h
@@ -18,4 +18,5 @@
 
 void updateAltHold(void);
 void updateAltHoldState(void);
+void updateSonarAltHoldState(void);
 
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
index c0a6b31ab0..90eb488e3d 100755
--- a/src/main/flight/imu.c
+++ b/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,9 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     }
 
     BaroAlt = baroCalculateAltitude();
+#else
+	BaroAlt = 0;
+#endif
 
 #ifdef SONAR
     tiltAngle = calculateTiltAngle(&inclination);
@@ -496,9 +501,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
diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h
index 92a725b681..9fb6ee7cc5 100644
--- a/src/main/io/rc_controls.h
+++ b/src/main/io/rc_controls.h
@@ -40,6 +40,7 @@ typedef enum {
     BOXOSD,
     BOXTELEMETRY,
     BOXAUTOTUNE,
+    BOXSONAR,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 
diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c
index 5907b647bf..1b4632b671 100755
--- a/src/main/io/serial_msp.c
+++ b/src/main/io/serial_msp.c
@@ -169,6 +169,7 @@ static const box_t const boxes[] = {
     { BOXOSD, "OSD SW;", 19 },
     { BOXTELEMETRY, "TELEMETRY;", 20 },
     { BOXAUTOTUNE, "AUTOTUNE;", 21 },
+    { BOXSONAR, "SONAR;", 22 },
     { CHECKBOX_ITEM_COUNT, NULL, 0xFF }
 };
 
@@ -456,6 +457,11 @@ void mspInit(serialConfig_t *serialConfig)
     activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE;
 #endif
 
+    if (feature(FEATURE_SONAR)){
+        activeBoxIds[activeBoxIdCount++] = BOXSONAR;
+    }
+
+
     memset(mspPorts, 0x00, sizeof(mspPorts));
 
     openAllMSPSerialPorts(serialConfig);
@@ -513,6 +519,7 @@ static bool processOutCommand(uint8_t cmdMSP)
             rcOptions[BOXOSD] << BOXOSD |
             rcOptions[BOXTELEMETRY] << BOXTELEMETRY |
             rcOptions[BOXAUTOTUNE] << BOXAUTOTUNE |
+            IS_ENABLED(FLIGHT_MODE(SONAR_MODE)) << BOXSONAR |
             IS_ENABLED(ARMING_FLAG(ARMED)) << BOXARM;
         for (i = 0; i < activeBoxIdCount; i++) {
             int flag = (tmp & (1 << activeBoxIds[i]));
diff --git a/src/main/mw.c b/src/main/mw.c
index fc2a0bbede..790bba241b 100755
--- a/src/main/mw.c
+++ b/src/main/mw.c
@@ -377,10 +377,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,
     UPDATE_DISPLAY_TASK
@@ -408,9 +410,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;
@@ -558,7 +571,7 @@ void processRx(void)
 void loop(void)
 {
     static uint32_t loopTime;
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
     static bool haveProcessedAnnexCodeOnce = false;
 #endif
 
@@ -575,6 +588,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();
@@ -592,7 +615,7 @@ void loop(void)
         previousTime = currentTime;
 
         annexCode();
-#ifdef BARO
+#if defined(BARO) || defined(SONAR)
         haveProcessedAnnexCodeOnce = true;
 #endif
 
@@ -606,13 +629,12 @@ void loop(void)
         }
 #endif
 
-#ifdef BARO
-        if (sensors(SENSOR_BARO)) {
-            if (FLIGHT_MODE(BARO_MODE)) {
+#if defined(BARO) || defined(SONAR)
+        if (sensors(SENSOR_BARO) || sensors(SENSOR_SONAR)) {
+            if (FLIGHT_MODE(BARO_MODE) || FLIGHT_MODE(SONAR_MODE)) {
                 updateAltHold();
             }
         }
-
 #endif
 
         if (currentProfile->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) {
diff --git a/src/main/sensors/initialisation.c b/src/main/sensors/initialisation.c
index 965a816563..fb771fe477 100755
--- a/src/main/sensors/initialisation.c
+++ b/src/main/sensors/initialisation.c
@@ -406,8 +406,8 @@ static void detectBaro()
         return;
     }
 #endif
-    sensorsClear(SENSOR_BARO);
 #endif
+    sensorsClear(SENSOR_BARO);
 }
 
 void reconfigureAlignment(sensorAlignmentConfig_t *sensorAlignmentConfig)