AP_RCTelemetry: add support for baro/vario CRSF frames

libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp

@@ -81,6 +81,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
     // CRSF telemetry rate is 150Hz (4ms) max, so these rates must fit
     add_scheduler_entry(50, 100);   // heartbeat        10Hz
     add_scheduler_entry(5, 20);     // parameters       50Hz (generally not active unless requested by the TX)
+    add_scheduler_entry(50, 200);   // baro_vario        5Hz
     add_scheduler_entry(50, 120);   // Attitude and compass 8Hz
     add_scheduler_entry(200, 1000); // VTX parameters    1Hz
     add_scheduler_entry(1300, 500); // battery           2Hz
@@ -152,6 +153,8 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
         // standard telemetry for high data rates
         set_scheduler_entry(BATTERY, 1000, 1000);       // 1Hz
         set_scheduler_entry(ATTITUDE, 1000, 1000);      // 1Hz
+        set_scheduler_entry(BARO_VARIO, 1000, 1000);    // 1Hz
+        set_scheduler_entry(VARIO, 1000, 1000);         // 1Hz
         // custom telemetry for high data rates
         set_scheduler_entry(GPS, 550, 500);            // 2.0Hz
         set_scheduler_entry(PASSTHROUGH, 100, 100);    // 8Hz
@@ -160,6 +163,8 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
         // standard telemetry for low data rates
         set_scheduler_entry(BATTERY, 1000, 2000);       // 0.5Hz
         set_scheduler_entry(ATTITUDE, 1000, 3000);      // 0.33Hz
+        set_scheduler_entry(BARO_VARIO, 1000, 3000);    // 0.33Hz
+        set_scheduler_entry(VARIO, 1000, 3000);         // 0.33Hz
         if (is_elrs()) {
             // ELRS custom telemetry for low data rates
             set_scheduler_entry(GPS, 550, 1000);            // 1.0Hz
@@ -320,6 +325,8 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
 void AP_CRSF_Telem::disable_tx_entries()
 {
     disable_scheduler_entry(ATTITUDE);
+    disable_scheduler_entry(BARO_VARIO);
+    disable_scheduler_entry(VARIO);
     disable_scheduler_entry(BATTERY);
     disable_scheduler_entry(GPS);
     disable_scheduler_entry(FLIGHT_MODE);
@@ -334,6 +341,8 @@ void AP_CRSF_Telem::disable_tx_entries()
 void AP_CRSF_Telem::enable_tx_entries()
 {
     enable_scheduler_entry(ATTITUDE);
+    enable_scheduler_entry(BARO_VARIO);
+    enable_scheduler_entry(VARIO);
     enable_scheduler_entry(BATTERY);
     enable_scheduler_entry(GPS);
     enable_scheduler_entry(FLIGHT_MODE);
@@ -428,6 +437,12 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
         case PARAMETERS: // update parameter settings
             process_pending_requests();
             break;
+        case BARO_VARIO:
+            calc_baro_vario();
+            break;
+        case VARIO:
+            calc_vario();
+            break;
         case ATTITUDE:
             calc_attitude();
             break;
@@ -914,6 +929,60 @@ void AP_CRSF_Telem::calc_battery()
     _telem_pending = true;
 }
 
+uint16_t AP_CRSF_Telem::get_altitude_packed()
+{
+    int32_t altitude_dm = get_nav_alt_m(Location::AltFrame::ABOVE_HOME) * 10;
+
+    enum : int32_t {
+        ALT_MIN_DM = 10000,                     // minimum altitude in dm
+        ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM, // altitude of precision changing in dm
+        ALT_MAX_DM = 0x7ffe * 10 - 5,           // maximum altitude in dm
+    };
+
+    if (altitude_dm < -ALT_MIN_DM) { // less than minimum altitude
+        return 0; // minimum
+    }
+    if (altitude_dm > ALT_MAX_DM) { // more than maximum
+        return 0xFFFEU; // maximum
+    }
+    if(altitude_dm < ALT_THRESHOLD_DM) { //dm-resolution range
+        return uint16_t(altitude_dm + ALT_MIN_DM);
+    }
+    return uint16_t((altitude_dm + 5) / 10) | uint16_t(0x8000); // meter-resolution range
+}
+
+int8_t AP_CRSF_Telem::get_vertical_speed_packed()
+{
+    float vspeed = get_vspeed_ms();
+    float vertical_speed_cm_s = vspeed * 100.0f;
+    const int16_t Kl = 100; // linearity constant;
+    const float Kr = .026f; // range constant;
+    int8_t vspeed_packed = int8_t(logf(fabsf(vertical_speed_cm_s)/Kl + 1)/Kr);
+    return vspeed_packed * (is_negative(vertical_speed_cm_s) ? -1 : 1);
+}
+
+// prepare vario data
+void AP_CRSF_Telem::calc_baro_vario()
+{
+    _telem.bcast.baro_vario.altitude_packed = get_altitude_packed();
+    _telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed();
+
+    _telem_size = sizeof(BaroVarioFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BARO_VARIO;
+
+    _telem_pending = true;
+}
+
+// prepare vario data
+void AP_CRSF_Telem::calc_vario()
+{
+    _telem.bcast.vario.v_speed = int16_t(get_vspeed_ms() * 100.0f);
+    _telem_size = sizeof(VarioFrame);
+    _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VARIO;
+
+    _telem_pending = true;
+}
+
 // prepare gps data
 void AP_CRSF_Telem::calc_gps()
 {

libraries/AP_RCTelemetry/AP_CRSF_Telem.h

@@ -64,6 +64,15 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
         uint8_t remaining; // ( percent )
     };
 
+    struct PACKED BaroVarioFrame {
+        uint16_t altitude_packed; // Altitude above start (calibration) point.
+        int8_t vertical_speed_packed; // vertical speed.
+    };
+
+    struct PACKED VarioFrame {
+        int16_t v_speed; // vertical speed cm/s
+    };
+
     struct PACKED VTXFrame {
 #if __BYTE_ORDER != __LITTLE_ENDIAN
 #error "Only supported on little-endian architectures"
@@ -201,6 +210,8 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
     union PACKED BroadcastFrame {
         GPSFrame gps;
         HeartbeatFrame heartbeat;
+        BaroVarioFrame baro_vario;
+        VarioFrame vario;
         BatteryFrame battery;
         VTXFrame vtx;
         AttitudeFrame attitude;
@@ -244,6 +255,8 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
     enum SensorType {
         HEARTBEAT,
         PARAMETERS,
+        BARO_VARIO,
+        VARIO,
         ATTITUDE,
         VTX_PARAMETERS,
         BATTERY,
@@ -267,6 +280,10 @@ class AP_CRSF_Telem : public AP_RCTelemetry {
     void calc_parameter_ping();
     void calc_heartbeat();
     void calc_battery();
+    uint16_t get_altitude_packed();
+    int8_t get_vertical_speed_packed();
+    void calc_baro_vario();
+    void calc_vario();
     void calc_gps();
     void calc_attitude();
     void calc_flight_mode();

libraries/AP_RCTelemetry/AP_RCTelemetry.cpp

@@ -25,6 +25,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <AP_Vehicle/AP_Vehicle_Type.h>
+#include <AP_Baro/AP_Baro.h>
 
 #ifdef TELEM_DEBUG
 # define debug(fmt, args...)	hal.console->printf("Telem: " fmt "\n", ##args)
@@ -293,3 +294,46 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const
 
     return ~health & enabled & present;
 }
+
+/*
+ * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
+ */
+float AP_RCTelemetry::get_vspeed_ms(void)
+{
+    {
+        // release semaphore as soon as possible
+        AP_AHRS &_ahrs = AP::ahrs();
+        Vector3f v;
+        WITH_SEMAPHORE(_ahrs.get_semaphore());
+        if (_ahrs.get_velocity_NED(v)) {
+            return -v.z;
+        }
+    }
+    auto &_baro = AP::baro();
+    WITH_SEMAPHORE(_baro.get_semaphore());
+    return _baro.get_climb_rate();
+}
+
+/*
+ * prepare altitude between vehicle and home location data
+ */
+float AP_RCTelemetry::get_nav_alt_m(Location::AltFrame frame)
+{
+    Location loc;
+    float current_height = 0;
+
+    AP_AHRS &_ahrs = AP::ahrs();
+    WITH_SEMAPHORE(_ahrs.get_semaphore());
+
+    if (frame == Location::AltFrame::ABOVE_HOME) {
+        _ahrs.get_relative_position_D_home(current_height);
+        return -current_height;
+    }
+
+    if (_ahrs.get_location(loc)) {
+        if (!loc.get_alt_m(frame, current_height)) {
+            // ignore this error
+        }
+    }
+    return current_height;
+}

libraries/AP_RCTelemetry/AP_RCTelemetry.h

@@ -18,6 +18,7 @@
 #include <AP_HAL/utility/RingBuffer.h>
 #include <AP_Math/AP_Math.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
+#include <AP_Common/Location.h>
 
 #define TELEM_PAYLOAD_STATUS_CAPACITY          5 // size of the message buffer queue (max number of messages waiting to be sent)
 
@@ -77,6 +78,9 @@ class AP_RCTelemetry {
         return _scheduler.max_packet_rate;
     }
 
+    static float get_vspeed_ms(void);
+    static float get_nav_alt_m(Location::AltFrame frame = Location::AltFrame::ABSOLUTE);
+
 protected:
     uint8_t run_wfq_scheduler(const bool use_shaper = true);
     // process a specific entry