From afa6e30b946c36c4c94d29b5152344f9a0b730f1 Mon Sep 17 00:00:00 2001
From: yaapu <alex.apostoli@gmail.com>
Date: Tue, 15 Jun 2021 14:59:43 +0200
Subject: [PATCH] AP_Frsky_Telem: added airspeed flag to frame 0x5005 enabled
 by a new parameter

---
 libraries/AP_Frsky_Telem/AP_Frsky_Backend.h   |  4 +++
 .../AP_Frsky_Telem/AP_Frsky_Parameters.cpp    |  7 +++++
 .../AP_Frsky_Telem/AP_Frsky_Parameters.h      |  1 +
 libraries/AP_Frsky_Telem/AP_Frsky_SPort.h     |  3 +-
 .../AP_Frsky_SPort_Passthrough.cpp            | 29 ++++++++++++++-----
 5 files changed, 36 insertions(+), 8 deletions(-)

diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
index 4dc28efcab..6796b2c841 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.h
+++ b/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);
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp
index d4fa51c7db..f9bf7872e8 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.cpp
+++ b/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
 };
 
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h b/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h
index 6cfc1b1a03..3826f340e6 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_Parameters.h
+++ b/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
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
index f847794e66..33ce14af99 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.h
+++ b/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;
diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
index 20d5bd2528..bc7b85c03c 100644
--- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
+++ b/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;
 }