AP_Frsky_Telem: added airspeed flag to frame 0x5005 enabled by a new parameter

libraries/AP_Frsky_Telem/AP_Frsky_Backend.h

@@ -139,6 +139,10 @@ protected:
     static const uint8_t SENSOR_ID_RPM             = 0xE4; // Sensor ID  4
     static const uint8_t SENSOR_ID_SP2UR           = 0xC6; // Sensor ID  6
 
+    enum frsky_options_e : uint8_t {
+        OPTION_AIRSPEED_AND_GROUNDSPEED = 1U<<0,
+    };
+
 private:
 
     void loop(void);

libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp

@@ -44,6 +44,13 @@ const AP_Param::GroupInfo AP_Frsky_Parameters::var_info[] = {
     // @Values: -1:Disable,7:7,8:8,9:9,10:10,11:11,12:12,13:13,14:14,15:15,16:16,17:17,18:18,19:19,20:20,21:21,22:22,23:23,24:24,25:25,26:26,27:27
     // @User: Advanced
     AP_GROUPINFO("DNLINK_ID",  4, AP_Frsky_Parameters, _dnlink_id, 27),
+
+    // @Param: OPTIONS
+    // @DisplayName: FRSky Telemetry Options
+    // @Description: A bitmask to set some FRSky Telemetry specific options
+    // @Bitmask: 0:EnableAirspeedAndGroundspeed
+    // @User: Standard
+    AP_GROUPINFO("OPTIONS", 5, AP_Frsky_Parameters, _options, 0),
     AP_GROUPEND
 };
 

libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h

@@ -37,6 +37,7 @@ private:
     AP_Int8 _dnlink_id;
     AP_Int8 _dnlink1_id;
     AP_Int8 _dnlink2_id;
+    AP_Int8 _options;
 };
 
 #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL

libraries/AP_Frsky_Telem/AP_Frsky_SPort.h

@@ -20,7 +20,7 @@ public:
     bool sport_telemetry_push(const uint8_t sensor, const uint8_t frame, const uint16_t appid, const int32_t data);
     // utility method to pack numbers in a compact size
     uint16_t prep_number(int32_t const number, const uint8_t digits, const uint8_t power);
-    
+
     static AP_Frsky_SPort *get_singleton(void) {
         return singleton;
     }
@@ -31,6 +31,7 @@ protected:
 
     struct PACKED {
         bool send_latitude; // sizeof(bool) = 4 ?
+        bool send_airspeed; // toggles 0x5005 between airspeed and groundspeed
         uint32_t gps_lng_sample;
         uint8_t new_byte;
     } _passthrough;

libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp

@@ -59,6 +59,7 @@ for FrSky SPort Passthrough
 #define VELANDYAW_XYVEL_OFFSET      9
 #define VELANDYAW_YAW_LIMIT         0x7FF
 #define VELANDYAW_YAW_OFFSET        17
+#define VELANDYAW_ARSPD_OFFSET      28
 // for attitude (roll, pitch) and range data
 #define ATTIANDRNG_ROLL_LIMIT       0x7FF
 #define ATTIANDRNG_PITCH_LIMIT      0x3FF
@@ -599,17 +600,31 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void)
     float vspd = get_vspeed_ms();
     // vertical velocity in dm/s
     uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1);
+    float airspeed_m;       // m/s
+    float hspeed_m;         // m/s
+    bool airspeed_estimate_true;
     AP_AHRS &_ahrs = AP::ahrs();
-    WITH_SEMAPHORE(_ahrs.get_semaphore());
-    // horizontal velocity in dm/s (use airspeed if available and enabled - even if not used - otherwise use groundspeed)
-    const AP_Airspeed *aspeed = AP::airspeed();
-    if (aspeed && aspeed->enabled()) {
-        velandyaw |= prep_number(roundf(aspeed->get_airspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
-    } else { // otherwise send groundspeed estimate from ahrs
-        velandyaw |= prep_number(roundf(_ahrs.groundspeed() * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
+    {
+        WITH_SEMAPHORE(_ahrs.get_semaphore());
+        hspeed_m = _ahrs.groundspeed(); // default is to use groundspeed
+        airspeed_estimate_true = AP::ahrs().airspeed_estimate_true(airspeed_m);
     }
+    bool option_airspeed_enabled = (_frsky_parameters->_options & frsky_options_e::OPTION_AIRSPEED_AND_GROUNDSPEED) != 0;
+    // airspeed estimate + airspeed option disabled (default) => send airspeed (we give priority to airspeed over groundspeed)
+    // airspeed estimate + airspeed option enabled => alternate airspeed/groundspeed, i.e send airspeed only when _passthrough.send_airspeed==true
+    if (airspeed_estimate_true && (!option_airspeed_enabled || _passthrough.send_airspeed)) {
+        hspeed_m = airspeed_m;
+    }
+    // horizontal velocity in dm/s
+    velandyaw |= prep_number(roundf(hspeed_m * 10), 2, 1)<<VELANDYAW_XYVEL_OFFSET;
     // yaw from [0;36000] centidegrees to .2 degree increments [0;1800] (just in case, limit to 2047 (0x7FF) since the value is stored on 11 bits)
     velandyaw |= ((uint16_t)roundf(_ahrs.yaw_sensor * 0.05f) & VELANDYAW_YAW_LIMIT)<<VELANDYAW_YAW_OFFSET;
+    // flag the airspeed bit if required
+    if (airspeed_estimate_true && option_airspeed_enabled && _passthrough.send_airspeed) {
+        velandyaw |= 1U<<VELANDYAW_ARSPD_OFFSET;
+    }
+    // toggle air/ground speed selector
+    _passthrough.send_airspeed = !_passthrough.send_airspeed;
     return velandyaw;
 }