Altitude Estimator

src/madflight/alt/alt.h

@@ -53,7 +53,7 @@ class AltEst_None : public AltEst {
   void updateBaroAlt(float alt, uint32_t ts) {} 
   float getH() {return 0;}
   float getV() {return 0;}
-  void print() {}
+  void toString(char *s) {s[0] = 0;}
 };
 
 AltEst_None alt_instance;

src/madflight/alt/alt_baro/alt_baro.h

@@ -26,7 +26,7 @@ SOFTWARE.
 
 #pragma once
 
-#include "../../interface.h" //baro.getSampleRate()
+#include "../alt_interface.h" //AltEst
 #include "../../common/common.h" //lowpass_to_beta()
 
 class AltEst_Baro : public AltEst {
@@ -61,9 +61,10 @@ class AltEst_Baro : public AltEst {
   float getH() {return h;} //altitude estimate in [m]
   float getV() {return v;} //vertical up speed (climb rate) estimate in [m/s]
 
-  void print() {
-    Serial.printf("alt.h:%.2f\t", h);
-    Serial.printf("alt.v:%+.2f\t", v);
+  void toString(char *s) {
+    int n = 0;
+    n += sprintf(s+n, "alt.h:%.2f\t", h);
+    n += sprintf(s+n, "alt.v:%+.2f\t", v);
   }
 
   float h = 0;    // Filtered approximate International Standard Atmosphere (ISA) Altitude in [m]

src/madflight/alt/alt_comp/alt_comp.h

@@ -25,16 +25,20 @@ SOFTWARE.
 ===========================================================================================*/
 #pragma once
 
+#include "../alt_interface.h" //AltEst
 #include <math.h>
 #include <stdlib.h>
+#include <string.h>
+
+
 
 class AltEst_Comp: public AltEst {
 public:
   void setup(float alt) {\
-    setup2(baro.alt, 0.4, 0.03, 0.1, 12); //baroAltitude[m], sigmaBaro[m], sigmaAccel[m/s2], accelThreshold[m/s2], ZUPT_SIZE[n]);
+    setup2(alt, 0.4, 0.03, 0.1, 12); //baroAltitude[m], sigmaBaro[m], sigmaAccel[m/s2], accelThreshold[m/s2], ZUPT_SIZE[n]);
     ts = 0;
 
-    Serial.printf("ALT:  ALT_USE_COMP gain=%f,%f\n", gain0, gain1);
+    //Serial.printf("ALT:  ALT_USE_COMP gain=%f,%f\n", gain0, gain1);
   }
 
   //a: accel up in [m/s^2], ts: timestamp in [us]
@@ -59,10 +63,11 @@ class AltEst_Comp: public AltEst {
   float getH() {return h;} //altitude estimate in [m]
   float getV() {return v;} //vertical up speed (climb rate) estimate in [m/s]
 
-  void print() {
-    Serial.printf("alt.h:%.2f\t", h);
-    Serial.printf("alt.v:%+.2f\t", v);
-    Serial.printf("alt.a:%+.2f\t", a);
+  void toString(char *s) {
+    int n = 0;
+    n += sprintf(s+n, "alt.h:%.2f\t", h);
+    n += sprintf(s+n, "alt.v:%+.2f\t", v);
+    n += sprintf(s+n, "alt.a:%+.2f\t", a);
   }
 
   void setup2(float baroAltitude, float sigmaAccel, float sigmaBaro, float accelThreshold, int ZUPT_SIZE)
@@ -88,7 +93,7 @@ class AltEst_Comp: public AltEst {
     pastAltitude = baroAltitude;
 
     // zero-velocity update if more than ZUPT_SIZE small acc were received
-    if(abs(a) >= accelThreshold) {
+    if(fabs(a) >= accelThreshold) {
       ZUPTIdx = 0;
     }else if(ZUPTIdx < ZUPT_SIZE) {
       ZUPTIdx++;

src/madflight/alt/alt_interface.h

@@ -0,0 +1,41 @@
+/*==========================================================================================
+alt_interface.h - This file defines the interfaces for the altitude estimator
+
+MIT License
+
+Copyright (c) 2024 https://madflight.com
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.
+===========================================================================================*/
+
+#pragma once
+
+//=================================================================================================
+// ALT - Altitude Estimator
+//=================================================================================================
+
+class AltEst {
+  public:
+    virtual void setup(float alt) = 0; //setup with default parameters and initial altitude in [m]
+    virtual void updateAccelUp(float a, uint32_t ts) = 0; //a: accel up in [m/s^2], ts: timestamp in [us]
+    virtual void updateBaroAlt(float alt, uint32_t ts) = 0; //alt: barometric altitude in [m], ts: timestamp in [us]
+    virtual float getH() = 0; //altitude estimate in [m]
+    virtual float getV() = 0; //vertical up speed (climb rate) estimate in [m/s]
+    virtual void toString(char *s); //print state info to s (max 100 chars)
+};

src/madflight/alt/alt_kalman2/alt_kalman2.h

@@ -60,9 +60,10 @@ class AltEst_Kalman2 : public AltEst {
   float getH() {return filter.h;} //altitude estimate in [m]
   float getV() {return filter.v;} //vertical up speed (climb rate) estimate in [m/s]
 
-  void print() {
-    Serial.printf("alt.h:%.2f\t", filter.h);
-    Serial.printf("alt.v:%+.2f\t", filter.v);
+  void toString(char *s) {
+    int n = 0;
+    n += sprintf(s+n, "alt.h:%.2f\t", filter.h);
+    n += sprintf(s+n, "alt.v:%+.2f\t", filter.v);
   }
 
 protected:

src/madflight/alt/alt_kalman3/alt_kalman3.h

@@ -62,10 +62,11 @@ class AltEst_Kalman3 : public AltEst {
   float getH() {return filter.h;} //altitude estimate in [m]
   float getV() {return filter.v;} //vertical up speed (climb rate) estimate in [m/s]
 
-  void print() {
-    Serial.printf("alt.h:%.2f\t", filter.h);
-    Serial.printf("alt.v:%+.2f\t", filter.v);
-    Serial.printf("alt.abias:%+.2f\t", filter.bias);
+  void toString(char *s) {
+    int n = 0;
+    n += sprintf(s+n, "alt.h:%.2f\t", filter.h);
+    n += sprintf(s+n, "alt.v:%+.2f\t", filter.v);
+    n += sprintf(s+n, "alt.abias:%+.2f\t", filter.bias);
   }
 
 protected:

src/madflight/bb/bb_sdcard/bb_sdcard.h

@@ -799,14 +799,12 @@ class BlackBox_SD : public BlackBox {
       bl.flt("Alt", baro.alt, 1, "m");  //float Alt: calculated altitude [m]
                                         //float AltAMSL: altitude AMSL
       bl.flt("Press", baro.press, 1, "Pa");      //float Press: measured atmospheric pressure [Pa]
-                                        //int16_t Temp: measured atmospheric temperature
-      bl.flt("CRt", baro.vz, 1, "m/s"); //float CRt: derived climb rate from primary barometer
+      bl.i16("Temp", baro.temp, 1, "degC"); //int16_t Temp: measured atmospheric temperature [C]
+                                        //float CRt: derived climb rate from primary barometer
                                         //uint32_t SMS: time last sample was taken
                                         //float Offset: raw adjustment of barometer altitude, zeroed on calibration, possibly set by GCS
                                         //float GndTemp: temperature on ground, specified by parameter or measured while on ground
                                         //uint8_t Health: true if barometer is considered healthy
-      //non-standard
-      bl.flt("AltRaw", baro.altRaw, 1, "m");
     } 
 
     void log_bat() override {
@@ -868,9 +866,9 @@ class BlackBox_SD : public BlackBox {
     void log_ahrs() override {
       BinLog bl("AHRS"); 
       bl.TimeUS();
-      bl.i16("ax",ahrs.ax*100, 1e-2, "G"); //G
-      bl.i16("ay",ahrs.ay*100, 1e-2, "G"); //G
-      bl.i16("az",ahrs.az*100, 1e-2, "G"); //G
+      bl.i16("ax",ahrs.ax*1000, 1e-3, "G"); //G
+      bl.i16("ay",ahrs.ay*1000, 1e-3, "G"); //G
+      bl.i16("az",ahrs.az*1000, 1e-3, "G"); //G
       bl.i16("gx",ahrs.gx*10, 1e-1, "deg/s"); //dps
       bl.i16("gy",ahrs.gy*10, 1e-1, "deg/s"); //dps
       bl.i16("gz",ahrs.gz*10, 1e-1, "deg/s"); //dps
@@ -901,9 +899,9 @@ class BlackBox_SD : public BlackBox {
       BinLog bl("IMU");
       bl.keepFree = QUEUE_LENGTH/4; //keep 25% of queue free for other messages
       bl.TimeUS(imu.ts);
-      bl.i16("ax",(imu.ax - cfg.IMU_CAL_AX)*100, 1e-2, "G"); //G
-      bl.i16("ay",(imu.ay - cfg.IMU_CAL_AY)*100, 1e-2, "G"); //G
-      bl.i16("az",(imu.az - cfg.IMU_CAL_AZ)*100, 1e-2, "G"); //G
+      bl.i16("ax",(imu.ax - cfg.IMU_CAL_AX)*1000, 1e-3, "G"); //G
+      bl.i16("ay",(imu.ay - cfg.IMU_CAL_AY)*1000, 1e-3, "G"); //G
+      bl.i16("az",(imu.az - cfg.IMU_CAL_AZ)*1000, 1e-3, "G"); //G
       bl.i16("gx",(imu.gx - cfg.IMU_CAL_GX)*10, 1e-1, "deg/s"); //dps
       bl.i16("gy",(imu.gy - cfg.IMU_CAL_GY)*10, 1e-1, "deg/s"); //dps
       bl.i16("gz",(imu.gz - cfg.IMU_CAL_GZ)*10, 1e-1, "deg/s"); //dps

src/madflight/cli/cli.h

@@ -3,7 +3,7 @@ cli.h - madflight Command Line Interface
 
 MIT License
 
-Copyright (c) 2024 https://madflight.com
+Copyright (c) 2024-2025 https://madflight.com
 
 Permission is hereby granted, free of charge, to any person obtaining a copy
 of this software and associated documentation files (the "Software"), to deal
@@ -30,6 +30,19 @@ SOFTWARE.
 #include "stat.h"
 #include "FreeRTOS_ps.h"
 
+//cli command extension, return true if command was processed
+extern bool cli_execute(String cmd, String arg1, String arg2) __attribute__((weak));
+
+/* example cli command extension
+bool cli_execute(String cmd, String arg1, String arg2) {
+  if(cmd == "mycommand") {
+    Serial.println("ASDFASDFASDF");
+    return true;
+  }
+  return false;
+}
+*/
+
 void cli_print_overview() {
   Serial.printf("rcin.pwm%d:%d\t", 1, rcin.pwm[0]);
   Serial.printf("rcin.roll:%+.2f\t", rcin.roll);
@@ -141,7 +154,9 @@ void cli_print_gps() {
 }
 
 static void cli_print_alt() {
-  alt.print();
+  char s[100];
+  alt.toString(s);
+  Serial.print(s);
   Serial.printf("baro.alt:%.2f\t", baro.alt);
   Serial.printf("ahrs.aup:%.2f\t", ahrs.getAccelUp());
 }
@@ -285,6 +300,11 @@ class CLI {
       }
     }
 
+    //call user defined commands, skip futher processing if true was returned
+    if(cli_execute) {
+      if(cli_execute(cmd, arg1, arg2)) return;
+    }
+
     if (cmd=="help" || cmd=="?") {
       help();
     }else if (cmd == "board") {

src/madflight/interface.h

@@ -298,14 +298,6 @@ PID PIDyaw;
 // ALT - Altitude Estimator
 //=================================================================================================
 
-class AltEst {
-  public:
-    virtual void setup(float alt) = 0; //setup with default parameters and initial altitude in [m]
-    virtual void updateAccelUp(float a, uint32_t ts) = 0; //a: accel up in [m/s^2], ts: timestamp in [us]
-    virtual void updateBaroAlt(float alt, uint32_t ts) = 0; //alt: barometric altitude in [m], ts: timestamp in [us]
-    virtual float getH() = 0; //altitude estimate in [m]
-    virtual float getV() = 0; //vertical up speed (climb rate) estimate in [m/s]
-    virtual void print(); //print state info
-};
+#include "alt/alt_interface.h"
 
 extern AltEst &alt;