From a74ba11114a0a72858363eebcd702d0f53d75223 Mon Sep 17 00:00:00 2001 From: slavic Date: Thu, 28 May 2026 21:17:59 +0300 Subject: [PATCH 1/4] Fixes MPU6500 not being detected on Ardu Nano ESP32 --- src/imu/MPUXXXX/MPUXXXX.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/imu/MPUXXXX/MPUXXXX.cpp b/src/imu/MPUXXXX/MPUXXXX.cpp index 204f82e2..01c47d69 100644 --- a/src/imu/MPUXXXX/MPUXXXX.cpp +++ b/src/imu/MPUXXXX/MPUXXXX.cpp @@ -68,8 +68,8 @@ int MPUXXXX::begin(MPU_Type type, SensorDevice *dev, int gyro_scale_dps, int acc Serial.printf("IMU: WARNING: MPU60X0 whoami mismatch, got:0x%02X expected:0x68, attempting autodetect\n",wai); _type = AUTO; } - if(_type == MPU6500 && wai != 0x70) { - Serial.printf("IMU: WARNING: MPU6500 whoami mismatch, got:0x%02X expected:0x70, attempting autodetect\n",wai); + if(_type == MPU6500 && (wai != 0x70 && wai != 0x30)) { + Serial.printf("IMU: WARNING: MPU6500 whoami mismatch, got:0x%02X expected:0x70 or 0x30, attempting autodetect\n",wai); _type = AUTO; } if(_type == MPU9150 && wai != 0x68) { From c5c580f1684a1545282a8f2098488723bd6b0da5 Mon Sep 17 00:00:00 2001 From: slavic Date: Mon, 1 Jun 2026 02:42:25 +0300 Subject: [PATCH 2/4] barometric attitude telemetry --- src/alt/alt_kalman3/alt_kalman3.h | 12 +++++++++--- src/rcl/crsf/crsf.h | 9 +++++++++ src/rcl/crsf/crsf_telemetry.h | 24 ++++++++++++++++++++++++ 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/src/alt/alt_kalman3/alt_kalman3.h b/src/alt/alt_kalman3/alt_kalman3.h index b527549a..a1b43c10 100644 --- a/src/alt/alt_kalman3/alt_kalman3.h +++ b/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); diff --git a/src/rcl/crsf/crsf.h b/src/rcl/crsf/crsf.h index ad6384b1..1dd407ea 100644 --- a/src/rcl/crsf/crsf.h +++ b/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); + } //------------------------------------------------------------------------------- diff --git a/src/rcl/crsf/crsf_telemetry.h b/src/rcl/crsf/crsf_telemetry.h index 5f6b92d2..e4c17735 100644 --- a/src/rcl/crsf/crsf_telemetry.h +++ b/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_dmps = (int16_t)(vspd_mps * 100.0f); + write_uint16_t(buf, offset, (uint16_t) vspd_dmps); + + write_frame_crc(buf, offset); + return offset; +} + + /* 0x08 Battery sensor - message length = 13 Payload: From fdd3eb197139374a1326930a06b85bf82262794d Mon Sep 17 00:00:00 2001 From: slavic Date: Mon, 1 Jun 2026 02:43:48 +0300 Subject: [PATCH 3/4] reverts the MPU workaround from this branch --- src/imu/MPUXXXX/MPUXXXX.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/imu/MPUXXXX/MPUXXXX.cpp b/src/imu/MPUXXXX/MPUXXXX.cpp index 01c47d69..204f82e2 100644 --- a/src/imu/MPUXXXX/MPUXXXX.cpp +++ b/src/imu/MPUXXXX/MPUXXXX.cpp @@ -68,8 +68,8 @@ int MPUXXXX::begin(MPU_Type type, SensorDevice *dev, int gyro_scale_dps, int acc Serial.printf("IMU: WARNING: MPU60X0 whoami mismatch, got:0x%02X expected:0x68, attempting autodetect\n",wai); _type = AUTO; } - if(_type == MPU6500 && (wai != 0x70 && wai != 0x30)) { - Serial.printf("IMU: WARNING: MPU6500 whoami mismatch, got:0x%02X expected:0x70 or 0x30, attempting autodetect\n",wai); + if(_type == MPU6500 && wai != 0x70) { + Serial.printf("IMU: WARNING: MPU6500 whoami mismatch, got:0x%02X expected:0x70, attempting autodetect\n",wai); _type = AUTO; } if(_type == MPU9150 && wai != 0x68) { From 7fef642fe143c023fa6c70374313e836bf26b676 Mon Sep 17 00:00:00 2001 From: slavic Date: Mon, 1 Jun 2026 02:49:04 +0300 Subject: [PATCH 4/4] small var name change --- src/rcl/crsf/crsf_telemetry.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/rcl/crsf/crsf_telemetry.h b/src/rcl/crsf/crsf_telemetry.h index e4c17735..2fe3b6b2 100644 --- a/src/rcl/crsf/crsf_telemetry.h +++ b/src/rcl/crsf/crsf_telemetry.h @@ -48,8 +48,8 @@ static int telemetry_altitude(uint8_t *buf, float altitude_m, float vspd_mps) uint16_t alt_dm = (uint16_t)((altitude_m * 10.0f) + 10000.0f); write_uint16_t(buf, offset, alt_dm); - int16_t vspd_dmps = (int16_t)(vspd_mps * 100.0f); - write_uint16_t(buf, offset, (uint16_t) vspd_dmps); + 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;