Ads barometric altitude and vertical speed to telemetry

src/alt/alt_kalman3/alt_kalman3.h

@@ -35,9 +35,15 @@ class AltEst_Kalman3 : public AltEst {
 public:
   void setup(float alt) {
     //default parameters (see usage.txt)
-    float altCov = 0.2;  //measured stdev BME280 = 0.4 [m] -> cov = 0.4*0.4 = 0.16
-    float accCov = 0.01; //measured stdev MPU6500 @ 16G = 0.003 [G] = 0.03 [m/s2] -> cov = 0.03*0.03 = 0.0009
-    float biasCov = 0.1; //estimated 3% * 9.81 = 0.3 [m/s2]  -> cov = 0.3*0.3 = 0.09
+    //float altCov = 0.2;  //measured stdev BME280 = 0.4 [m] -> cov = 0.4*0.4 = 0.16
+    //float accCov = 0.01; //measured stdev MPU6500 @ 16G = 0.003 [G] = 0.03 [m/s2] -> cov = 0.03*0.03 = 0.0009
+    //float biasCov = 0.1; //estimated 3% * 9.81 = 0.3 [m/s2]  -> cov = 0.3*0.3 = 0.09
+
+    // The following values work substantially better for altitude estimation for a BMP280/MPU6500 combo
+    // resulting in vertical speeds noise within 0.3 m/s, compared to ~2m/s.
+    float altCov = 40;   
+    float accCov = 0.05;  
+    float biasCov = 0.0005; 
 
     Serial.printf("ALT: KALMAN3  altCov=%f accCov=%f biasCov=%f\n", altCov, accCov, biasCov);
 

src/rcl/crsf/crsf.h

@@ -64,6 +64,8 @@ C8 0C 14 33 00 64 0D 00 04 01 00 00 00 96 --> 0x0c:len 12 bytes (frame len is 14
 #include "../../gps/gps.h"
 #include "../../out/out.h"
 #include "../../veh/veh.h"
+#include "../../bar/bar.h"
+#include "../../alt/alt.h"
 
 #define CRSF_BAUD 420000
 #define CRSF_FRAME_SIZE_MAX 64 //max number of bytes of a frame
@@ -143,6 +145,7 @@ class CRSF {
         String fm_str = String(out.armed() ? "*" : "") + (gps.sat>0 ?  String(gps.sat) :  String("")) + veh.flightmode_name();
         telem_flight_mode(fm_str.c_str());  //only first 14 char get transmitted
         telem_attitude(ahr.pitch, ahr.roll, ahr.yaw);  
+        telem_altitude(alt.getH(), alt.getV());
         if(telem_cnt % 10 == 0) telem_battery(bat.v, bat.i, bat.mah, 100);
         if(telem_cnt % 10 == 5) telem_gps(gps.lat, gps.lon, gps.sog/278, gps.cog/1000, (gps.alt<0 ? 0 : gps.alt/1000), gps.sat); // sog/278 is conversion from mm/s to km/h 
       }
@@ -175,6 +178,12 @@ class CRSF {
         int len = CRSF_Telemetry::telemetry_battery(buf, voltage_V, current_A, fuel_mAh, remaining);
         if((int)ser_bus->availableForWrite() >= len) ser_bus->write(buf, len);
     }
+
+    void telem_altitude(float alt, float vs) {
+        uint8_t buf[65];
+        int len = CRSF_Telemetry::telemetry_altitude(buf, alt, vs);
+        if((int)ser_bus->availableForWrite() >= len) ser_bus->write(buf, len);
+    }
 
     //-------------------------------------------------------------------------------
 

src/rcl/crsf/crsf_telemetry.h

@@ -10,6 +10,7 @@ enum class crsf_frame_type_t : uint8_t {
 	rc_channels_packed = 0x16,
 	attitude = 0x1E,
 	flight_mode = 0x21,
+	baro_altitude = 0x09,
 
 	// Extended Header Frames, range: 0x28 to 0x96
 	device_ping = 0x28,
@@ -26,12 +27,35 @@ enum class crsf_payload_size_t : uint8_t {
 	link_statistics = 10,
 	rc_channels = 22, ///< 11 bits per channel * 16 channels = 22 bytes.
 	attitude = 6,
+	baro_altitude = 4,
 };
 
 class CRSF_Telemetry {
 public:
 
 
+/*
+0x09 Barometric altitude
+Payload:
+uint16_t  	Altitude + 10000 ( dm )
+int16_t		Vertical Speed ( cm/s )
+*/
+static int telemetry_altitude(uint8_t *buf, float altitude_m, float vspd_mps)
+{
+	int offset = 0;
+	write_frame_header(buf, offset, crsf_frame_type_t::baro_altitude, (uint8_t)crsf_payload_size_t::baro_altitude);
+        // EdgeTX expects: (alt_m * 10) + 10000
+        uint16_t alt_dm = (uint16_t)((altitude_m * 10.0f) + 10000.0f);
+        write_uint16_t(buf, offset, alt_dm);
+
+        int16_t vspd_cmps = (int16_t)(vspd_mps * 100.0f);
+        write_uint16_t(buf, offset, (uint16_t) vspd_cmps);
+
+	write_frame_crc(buf, offset);
+	return offset;
+}
+
+
 /*
 0x08 Battery sensor - message length = 13
 Payload: