MSP command for sonar altitude

src/main/flight/altitudehold.c

@@ -53,7 +53,6 @@ uint8_t velocityControl = 0;
 int32_t errorVelocityI = 0;
 int32_t altHoldThrottleAdjustment = 0;
 int32_t AltHold;
-int32_t EstAlt;                // in cm
 int32_t vario = 0;                      // variometer in cm/s
 
 
@@ -78,7 +77,7 @@ void configureAltitudeHold(
 #if defined(BARO) || defined(SONAR)
 
 static int16_t initialThrottleHold;
-
+static int32_t EstAlt;                // in cm
 
 // 40hz update rate (20hz LPF on acc)
 #define BARO_UPDATE_FREQUENCY_40HZ (1000 * 25)
@@ -230,6 +229,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     float vel_acc;
     int32_t vel_tmp;
     float accZ_tmp;
+    int32_t sonarAlt = -1;
     static float accZ_old = 0.0f;
     static float vel = 0.0f;
     static float accAlt = 0.0f;
@@ -242,8 +242,6 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     int16_t tiltAngle;
 #endif
 
-
-
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -329,5 +327,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
 
     accZ_old = accZ_tmp;
 }
+
+int32_t altitudeHoldGetEstimatedAltitude(void)
+{
+    return EstAlt;
+}
+
 #endif
 

src/main/flight/altitudehold.h

@@ -15,14 +15,16 @@
  * along with Cleanflight.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-//extern int32_t errorVelocityI;
+#include "flight/flight.h"
-//extern int32_t altHoldThrottleAdjustment;
-//extern uint8_t velocityControl;
-//extern int32_t setVelocity;
 
+#include "io/escservo.h"
+#include "io/rc_controls.h"
+
+#include "sensors/barometer.h"
 
 void configureAltitudeHold(pidProfile_t *initialPidProfile, barometerConfig_t *intialBarometerConfig, rcControlsConfig_t *initialRcControlsConfig, escAndServoConfig_t *initialEscAndServoConfig);
 void applyAltHold(airplaneConfig_t *airplaneConfig);
 void updateAltHoldState(void);
 void updateSonarAltHoldState(void);
 
+int32_t altitudeHoldGetEstimatedAltitude(void);

src/main/flight/flight.h

@@ -111,7 +111,6 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3];
 extern int16_t heading, magHold;
 
 extern int32_t AltHold;
-extern int32_t EstAlt;
 extern int32_t vario;
 
 void setPIDController(int type);

src/main/io/rc_controls.h

@@ -17,6 +17,8 @@
 
 #pragma once
 
+#include "rx/rx.h"
+
 typedef enum {
     BOXARM = 0,
     BOXANGLE,

src/main/io/serial.c

@@ -44,7 +44,6 @@
 
 void updateSerialRxFunctionConstraint(functionConstraint_t *functionConstraintToUpdate);
 
-void mspInit(serialConfig_t *serialConfig);
 void cliInit(serialConfig_t *serialConfig);
 
 // this exists so the user can reference scenarios by a number in the CLI instead of an unuser-friendly bitmask.

src/main/io/serial_msp.c

@@ -45,6 +45,8 @@
 #include "flight/mixer.h"
 #include "flight/failsafe.h"
 #include "flight/navigation.h"
+#include "flight/altitudehold.h"
+
 #include "rx/rx.h"
 #include "rx/msp.h"
 #include "io/escservo.h"
@@ -54,9 +56,11 @@
 #include "io/serial.h"
 #include "io/ledstrip.h"
 #include "telemetry/telemetry.h"
+
 #include "sensors/boardalignment.h"
 #include "sensors/sensors.h"
 #include "sensors/battery.h"
+#include "sensors/sonar.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
 #include "sensors/compass.h"
@@ -249,6 +253,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
 #define MSP_SERVO_CONF           120    //out message         Servo settings
 #define MSP_NAV_STATUS           121    //out message         Returns navigation status
 #define MSP_NAV_CONFIG           122    //out message         Returns navigation parameters
+#define MSP_SONAR_ALTITUDE       123    //out message         Returns sonar altitude [cm]
 
 #define MSP_SET_RAW_RC           200    //in message          8 rc chan
 #define MSP_SET_RAW_GPS          201    //in message          fix, numsat, lat, lon, alt, speed
@@ -368,7 +373,7 @@ static mspPort_t mspPorts[MAX_MSP_PORT_COUNT];
 
 static mspPort_t *currentPort;
 
-void serialize32(uint32_t a)
+static void serialize32(uint32_t a)
 {
     static uint8_t t;
     t = a;
@@ -385,7 +390,7 @@ void serialize32(uint32_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize16(int16_t a)
+static void serialize16(int16_t a)
 {
     static uint8_t t;
     t = a;
@@ -396,32 +401,32 @@ void serialize16(int16_t a)
     currentPort->checksum ^= t;
 }
 
-void serialize8(uint8_t a)
+static void serialize8(uint8_t a)
 {
     serialWrite(mspSerialPort, a);
     currentPort->checksum ^= a;
 }
 
-uint8_t read8(void)
+static uint8_t read8(void)
 {
     return currentPort->inBuf[currentPort->indRX++] & 0xff;
 }
 
-uint16_t read16(void)
+static uint16_t read16(void)
 {
     uint16_t t = read8();
     t += (uint16_t)read8() << 8;
     return t;
 }
 
-uint32_t read32(void)
+static uint32_t read32(void)
 {
     uint32_t t = read16();
     t += (uint32_t)read16() << 16;
     return t;
 }
 
-void headSerialResponse(uint8_t err, uint8_t s)
+static void headSerialResponse(uint8_t err, uint8_t s)
 {
     serialize8('$');
     serialize8('M');
@@ -431,36 +436,36 @@ void headSerialResponse(uint8_t err, uint8_t s)
     serialize8(currentPort->cmdMSP);
 }
 
-void headSerialReply(uint8_t s)
+static void headSerialReply(uint8_t s)
 {
     headSerialResponse(0, s);
 }
 
-void headSerialError(uint8_t s)
+static void headSerialError(uint8_t s)
 {
     headSerialResponse(1, s);
 }
 
-void tailSerialReply(void)
+static void tailSerialReply(void)
 {
     serialize8(currentPort->checksum);
 }
 
-void s_struct(uint8_t *cb, uint8_t siz)
+static void s_struct(uint8_t *cb, uint8_t siz)
 {
     headSerialReply(siz);
     while (siz--)
         serialize8(*cb++);
 }
 
-void serializeNames(const char *s)
+static void serializeNames(const char *s)
 {
     const char *c;
     for (c = s; *c; c++)
         serialize8(*c);
 }
 
-const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
+static const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -473,7 +478,7 @@ const box_t *findBoxByActiveBoxId(uint8_t activeBoxId)
     return NULL;
 }
 
-const box_t *findBoxByPermenantId(uint8_t permenantId)
+static const box_t *findBoxByPermenantId(uint8_t permenantId)
 {
     uint8_t boxIndex;
     const box_t *candidate;
@@ -486,7 +491,7 @@ const box_t *findBoxByPermenantId(uint8_t permenantId)
     return NULL;
 }
 
-void serializeBoxNamesReply(void)
+static void serializeBoxNamesReply(void)
 {
     int i, activeBoxId, j, flag = 1, count = 0, len;
     const box_t *box;
@@ -652,7 +657,6 @@ static bool processOutCommand(uint8_t cmdMSP)
 {
     uint32_t i, tmp, junk;
 
-
 #ifdef GPS
     uint8_t wp_no;
     int32_t lat = 0, lon = 0;
@@ -821,9 +825,21 @@ static bool processOutCommand(uint8_t cmdMSP)
         break;
     case MSP_ALTITUDE:
         headSerialReply(6);
-        serialize32(EstAlt);
+#if defined(BARO) || defined(SONAR)
+        serialize32(altitudeHoldGetEstimatedAltitude());
+#else
+        serialize32(0);
+#endif
         serialize16(vario);
         break;
+    case MSP_SONAR_ALTITUDE:
+        headSerialReply(4);
+#if defined(SONAR)
+        serialize32(sonarGetLatestAltitude());
+#else
+        serialize32(0);
+#endif
+        break;
     case MSP_ANALOG:
         headSerialReply(7);
         serialize8((uint8_t)constrain(vbat, 0, 255));

src/main/io/serial_msp.h

@@ -17,9 +17,14 @@
 
 #pragma once
 
+#include "io/serial.h"
+#include "drivers/serial.h"
+
 // Each MSP port requires state and a receive buffer, revisit this default if someone needs more than 2 MSP ports.
 #define MAX_MSP_PORT_COUNT 2
 
+void mspInit(serialConfig_t *serialConfig);
+
 void mspProcess(void);
 void sendMspTelemetry(void);
 void mspSetTelemetryPort(serialPort_t *mspTelemetryPort);

src/main/main.c

@@ -332,7 +332,7 @@ void init(void)
 
 #ifdef SONAR
     if (feature(FEATURE_SONAR)) {
-        Sonar_init();
+        sonarInit();
     }
 #endif
 

src/main/mw.c

@@ -469,7 +469,7 @@ void executePeriodicTasks(void)
 #ifdef SONAR
     case UPDATE_SONAR_TASK:
         if (sensors(SENSOR_SONAR)) {
-            Sonar_update();
+            sonarUpdate();
         }
         break;
 #endif

src/main/sensors/sonar.c

@@ -33,11 +33,13 @@
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
-int32_t sonarAlt = -1;                  // in cm , -1 indicate sonar is not in range - inclination adjusted by imu
+// in cm , -1 indicate sonar is not in range - inclination adjusted by imu
 
 #ifdef SONAR
 
-void Sonar_init(void)
+static int32_t calculatedAltitude;
+
+void sonarInit(void)
 {
 #if defined(NAZE) || defined(EUSTM32F103RC) || defined(PORT103R)
     static const sonarHardware_t const sonarPWM56 = {
@@ -74,25 +76,33 @@ void Sonar_init(void)
 #endif
 
     sensorsSet(SENSOR_SONAR);
-    sonarAlt = 0;
+    calculatedAltitude = -1;
 }
 
-void Sonar_update(void)
+void sonarUpdate(void)
 {
     hcsr04_start_reading();
 }
 
+int32_t sonarRead(void)
+{
+    return hcsr04_get_distance();
+}
+
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle)
 {
     // calculate sonar altitude only if the sonar is facing downwards(<25deg)
     if (tiltAngle > 250)
-        return -1;
+        calculatedAltitude = -1;
+    else
+        calculatedAltitude = sonarAlt * (900.0f - tiltAngle) / 900.0f;
 
-    return sonarAlt * (900.0f - tiltAngle) / 900.0f;
+    return calculatedAltitude;
 }
 
-int32_t sonarRead(void) {
+int32_t sonarGetLatestAltitude(void)
-    return hcsr04_get_distance();
+{
+    return calculatedAltitude;
 }
 
 #endif

src/main/sensors/sonar.h

@@ -17,10 +17,9 @@
 
 #pragma once
 
-extern int32_t sonarAlt;
+void sonarInit(void);
-
+void sonarUpdate(void);
-void Sonar_init(void);
-void Sonar_update(void);
 
 int32_t sonarRead(void);
 int32_t sonarCalculateAltitude(int32_t sonarAlt, int16_t tiltAngle);
+int32_t sonarGetLatestAltitude(void);