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; }