diff --git a/src/main/rx/crsf_protocol.h b/src/main/rx/crsf_protocol.h index deb3b870e3..1dcb1cb348 100644 --- a/src/main/rx/crsf_protocol.h +++ b/src/main/rx/crsf_protocol.h @@ -45,6 +45,7 @@ typedef enum { CRSF_FRAMETYPE_GPS = 0x02, CRSF_FRAMETYPE_VARIO_SENSOR = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BARO_ALTITUDE = 0x09, CRSF_FRAMETYPE_HEARTBEAT = 0x0B, CRSF_FRAMETYPE_LINK_STATISTICS = 0x14, CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16, @@ -98,6 +99,7 @@ enum { CRSF_FRAME_GPS_PAYLOAD_SIZE = 15, CRSF_FRAME_VARIO_SENSOR_PAYLOAD_SIZE = 2, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8, + CRSF_FRAME_BARO_ALTITUDE_PAYLOAD_SIZE = 3, CRSF_FRAME_HEARTBEAT_PAYLOAD_SIZE = 2, CRSF_FRAME_LINK_STATISTICS_PAYLOAD_SIZE = 10, CRSF_FRAME_LINK_STATISTICS_TX_PAYLOAD_SIZE = 6, diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 16b32eaea6..0e9d024ce2 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -21,6 +21,8 @@ #include #include #include +#include +#include #include "platform.h" @@ -64,6 +66,7 @@ #include "sensors/battery.h" #include "sensors/sensors.h" +#include "sensors/barometer.h" #include "telemetry/telemetry.h" #include "telemetry/msp_shared.h" @@ -298,6 +301,60 @@ void crsfFrameBatterySensor(sbuf_t *dst) sbufWriteU8(dst, batteryRemainingPercentage); } +#if defined(USE_BARO) && defined(USE_VARIO) +// pack altitude in decimeters into a 16-bit value. +// Due to strange OpenTX behavior of count any 0xFFFF value as incorrect, the maximum sending value is limited to 0xFFFE (32766 meters) +// in order to have both precision and range in 16-bit +// value of altitude is packed with different precision depending on highest-bit value. +// on receiving side: +// if MSB==0, altitude is sent in decimeters as uint16 with -1000m base. So, range is -1000..2276m. +// if MSB==1, altitude is sent in meters with 0 base. So, range is 0..32766m (MSB must be zeroed). +// altitude lower than -1000m is sent as zero (should be displayed as "<-1000m" or something). +// altitude higher than 32767m is sent as 0xfffe (should be displayed as ">32766m" or something). +// range from 0 to 2276m might be sent with dm- or m-precision. But this function always use dm-precision. +static inline uint16_t calcAltitudePacked(int32_t altitude_dm) +{ + static const int ALT_DM_OFFSET = 10000; + int valDm = altitude_dm + ALT_DM_OFFSET; + + if (valDm < 0) return 0; // too low, return minimum + if (valDm < 0x8000) return valDm; // 15 bits to return dm value with offset + + return MIN((altitude_dm + 5) / 10, 0x7fffe) | 0x8000; // positive 15bit value in meters, with OpenTX limit +} + +static inline int8_t calcVerticalSpeedPacked(int16_t verticalSpeed) // Vertical speed in m/s (meters per second) +{ + // linearity coefficient. + // Bigger values lead to more linear output i.e., less precise smaller values and more precise big values. + // Decreasing the coefficient increases nonlinearity, i.e., more precise small values and less precise big values. + static const float Kl = 100.0f; + + // Range coefficient is calculated as result_max / log(verticalSpeedMax * LinearityCoefficient + 1); + // but it must be set manually (not calculated) for equality of packing and unpacking + static const float Kr = .026f; + + int8_t sign = verticalSpeed < 0 ? -1 : 1; + const int result32 = lrintf(log_approx(verticalSpeed * sign / Kl + 1) / Kr) * sign; + int8_t result8 = constrain(result32, SCHAR_MIN, SCHAR_MAX); + return result8; + + // for unpacking the following function might be used: + // int unpacked = lrintf((expf(result8 * sign * Kr) - 1) * Kl) * sign; + // lrint might not be used depending on integer or floating output. +} + +// pack barometric altitude +static void crsfFrameAltitude(sbuf_t *dst) +{ + // use sbufWrite since CRC does not include frame length + sbufWriteU8(dst, CRSF_FRAME_BARO_ALTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_BARO_ALTITUDE); + sbufWriteU16BigEndian(dst, calcAltitudePacked((baro.altitude + 5) / 10)); + sbufWriteU8(dst, calcVerticalSpeedPacked(getEstimatedVario())); +} +#endif + /* 0x0B Heartbeat Payload: @@ -370,11 +427,11 @@ static int16_t decidegrees2Radians10000(int16_t angle_decidegree) // fill dst buffer with crsf-attitude telemetry frame void crsfFrameAttitude(sbuf_t *dst) { - sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); - sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE); - sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.pitch)); - sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.roll)); - sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.yaw)); + sbufWriteU8(dst, CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); + sbufWriteU8(dst, CRSF_FRAMETYPE_ATTITUDE); + sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.pitch)); + sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.roll)); + sbufWriteU16BigEndian(dst, decidegrees2Radians10000(attitude.values.yaw)); } /* @@ -638,6 +695,7 @@ static void crsfFrameDisplayPortClear(sbuf_t *dst) typedef enum { CRSF_FRAME_START_INDEX = 0, CRSF_FRAME_ATTITUDE_INDEX = CRSF_FRAME_START_INDEX, + CRSF_FRAME_BARO_ALTITUDE_INDEX, CRSF_FRAME_BATTERY_SENSOR_INDEX, CRSF_FRAME_FLIGHT_MODE_INDEX, CRSF_FRAME_GPS_INDEX, @@ -694,6 +752,14 @@ static void processCrsf(void) crsfFrameAttitude(dst); crsfFinalize(dst); } +#if defined(USE_BARO) && defined(USE_VARIO) + // send barometric altitude + if (currentSchedule & BIT(CRSF_FRAME_BARO_ALTITUDE_INDEX)) { + crsfInitializeFrame(dst); + crsfFrameAltitude(dst); + crsfFinalize(dst); + } +#endif if (currentSchedule & BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX)) { crsfInitializeFrame(dst); crsfFrameBatterySensor(dst); @@ -773,6 +839,11 @@ void initCrsfTelemetry(void) if (sensors(SENSOR_ACC) && telemetryIsSensorEnabled(SENSOR_PITCH | SENSOR_ROLL | SENSOR_HEADING)) { crsfSchedule[index++] = BIT(CRSF_FRAME_ATTITUDE_INDEX); } +#if defined(USE_BARO) && defined(USE_VARIO) + if (telemetryIsSensorEnabled(SENSOR_ALTITUDE)) { + crsfSchedule[index++] = BIT(CRSF_FRAME_BARO_ALTITUDE_INDEX); + } +#endif if ((isBatteryVoltageConfigured() && telemetryIsSensorEnabled(SENSOR_VOLTAGE)) || (isAmperageConfigured() && telemetryIsSensorEnabled(SENSOR_CURRENT | SENSOR_FUEL))) { crsfSchedule[index++] = BIT(CRSF_FRAME_BATTERY_SENSOR_INDEX); diff --git a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc index 1409b61897..c60f3e5332 100644 --- a/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc +++ b/src/test/unit/rx_spi_expresslrs_telemetry_unittest.cc @@ -51,6 +51,7 @@ extern "C" { #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" + #include "sensors/barometer.h" #include "config/config.h" @@ -399,6 +400,7 @@ extern "C" { uint8_t armingFlags; uint8_t stateFlags; uint16_t flightModeFlags; + baro_t baro; uint32_t microsISR(void) {return 0; } diff --git a/src/test/unit/telemetry_crsf_msp_unittest.cc b/src/test/unit/telemetry_crsf_msp_unittest.cc index d56241d370..78f2bfa1a8 100644 --- a/src/test/unit/telemetry_crsf_msp_unittest.cc +++ b/src/test/unit/telemetry_crsf_msp_unittest.cc @@ -65,6 +65,7 @@ extern "C" { #include "telemetry/msp_shared.h" #include "telemetry/smartport.h" #include "sensors/acceleration.h" + #include "sensors/barometer.h" rssiSource_e rssiSource; bool handleMspFrame(uint8_t *frameStart, uint8_t frameLength, uint8_t *skipsBeforeResponse); @@ -96,7 +97,7 @@ extern "C" { extern struct mspPacket_s responsePacket; uint32_t dummyTimeUs; - + baro_t baro; } #include "unittest_macros.h" diff --git a/src/test/unit/telemetry_crsf_unittest.cc b/src/test/unit/telemetry_crsf_unittest.cc index b0ff149ca2..4f45af046f 100644 --- a/src/test/unit/telemetry_crsf_unittest.cc +++ b/src/test/unit/telemetry_crsf_unittest.cc @@ -58,6 +58,7 @@ extern "C" { #include "sensors/battery.h" #include "sensors/sensors.h" #include "sensors/acceleration.h" + #include "sensors/barometer.h" #include "msp/msp_serial.h" @@ -67,6 +68,7 @@ extern "C" { rssiSource_e rssiSource; bool airMode; + baro_t baro; uint16_t testBatteryVoltage = 0; int32_t testAmperage = 0;