From 617b023c6f00004945f815ef13cdb008ad75c8d7 Mon Sep 17 00:00:00 2001 From: yaapu Date: Fri, 18 Jun 2021 17:59:41 +0200 Subject: [PATCH] AP_Frsky_Telem: added frame 0x500C for true/apparent WIND info --- libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp | 8 +- .../AP_Frsky_SPort_Passthrough.cpp | 73 +++++++++++++++++-- .../AP_Frsky_SPort_Passthrough.h | 2 + 3 files changed, 75 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp index 084db99645..3ac28d5813 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp @@ -380,7 +380,13 @@ uint16_t AP_Frsky_SPort::prep_number(int32_t number, uint8_t digits, uint8_t pow uint16_t res = 0; uint32_t abs_number = abs(number); - if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power + if ((digits == 2) && (power == 0)) { // number encoded on 7 bits, client side needs to know if expected range is 0,127 or -63,63 + uint8_t max_value = number < 0 ? (0x1<<6)-1 : (0x1<<7)-1; + res = constrain_int16(abs_number,0,max_value); + if (number < 0) { // if number is negative, add sign bit in front + res |= 1U<<6; + } + } else if ((digits == 2) && (power == 1)) { // number encoded on 8 bits: 7 bits for digits + 1 for 10^power if (abs_number < 100) { res = abs_number<<1; } else if (abs_number < 1270) { diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index bc7b85c03c..db93e74204 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -11,6 +11,9 @@ #include #include #include +#if APM_BUILD_TYPE(APM_BUILD_Rover) +#include +#endif #if HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL #include "AP_Frsky_MAVlite.h" @@ -67,6 +70,12 @@ for FrSky SPort Passthrough #define ATTIANDRNG_RNGFND_OFFSET 21 // for terrain data #define TERRAIN_UNHEALTHY_OFFSET 13 +// for wind data +#define WIND_ANGLE_LIMIT 0x7F +#define WIND_SPEED_OFFSET 7 +#define WIND_APPARENT_ANGLE_OFFSET 15 +#define WIND_APPARENT_SPEED_OFFSET 23 + extern const AP_HAL::HAL& hal; AP_Frsky_SPort_Passthrough *AP_Frsky_SPort_Passthrough::singleton; @@ -121,6 +130,7 @@ void AP_Frsky_SPort_Passthrough::setup_wfq_scheduler(void) set_scheduler_entry(PARAM, 1700, 1000); // 0x5007 parameters set_scheduler_entry(RPM, 300, 330); // 0x500A rpm sensors 1 and 2 set_scheduler_entry(TERRAIN, 700, 500); // 0x500B terrain data + set_scheduler_entry(WIND, 700, 500); // 0x500C wind data set_scheduler_entry(UDATA, 5000, 200); // user data // initialize default sport sensor ID @@ -221,6 +231,23 @@ bool AP_Frsky_SPort_Passthrough::is_packet_ready(uint8_t idx, bool queue_empty) #endif } break; + case WIND: +#if !APM_BUILD_TYPE(APM_BUILD_Rover) + { + float a; + WITH_SEMAPHORE(AP::ahrs().get_semaphore()); + if (AP::ahrs().airspeed_estimate_true(a)) { + // if we have an airspeed estimate then we have a valid wind estimate + packet_ready = true; + } + } +#else + { + const AP_WindVane* windvane = AP_WindVane::get_singleton(); + packet_ready = windvane != nullptr && windvane->enabled(); + } +#endif + break; case UDATA: // when using fport user data is sent by scheduler // when using sport user data is sent responding to custom polling @@ -293,6 +320,9 @@ void AP_Frsky_SPort_Passthrough::process_packet(uint8_t idx) case TERRAIN: // 0x500B terrain data send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0B, calc_terrain()); break; + case WIND: // 0x500C terrain data + send_sport_frame(SPORT_DATA_FRAME, DIY_FIRST_ID+0x0C, calc_wind()); + break; case UDATA: // user data { WITH_SEMAPHORE(_sport_push_buffer.sem); @@ -540,13 +570,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_ap_status(void) ap_status |= (uint8_t)(fence->get_breaches()>0) << AP_FENCE_BREACH_OFFSET; } // signed throttle [-100,100] scaled down to [-63,63] on 7 bits, MSB for sign + 6 bits for 0-63 - int16_t throttle = gcs().get_hud_throttle(); - uint8_t scaled_throttle = constrain_int16(abs(throttle*0.63f),0,63); - // add sign - if (throttle < 0) { - scaled_throttle |= 0x1<<6; - } - ap_status |= scaled_throttle<enabled()) { + // true wind angle in 3 degree increments 0,360 (unsigned) + value = prep_number(roundf(wrap_360(degrees(windvane->get_true_wind_direction_rad())) * (1.0f/3.0f)), 2, 0); + // true wind speed in dm/s + value |= prep_number(roundf(windvane->get_true_wind_speed() * 10), 2, 1) << WIND_SPEED_OFFSET; + // apparent wind angle in 3 degree increments -180,180 (signed) + value |= prep_number(roundf(degrees(windvane->get_apparent_wind_direction_rad()) * (1.0f/3.0f)), 2, 0); + // apparent wind speed in dm/s + value |= prep_number(roundf(windvane->get_apparent_wind_speed() * 10), 2, 1) << WIND_APPARENT_SPEED_OFFSET; + } +#endif + return value; +} + /* fetch Sport data for an external transport, such as FPort or crossfire Note: we need to create a packet array with unique packet types diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h index 9c94978649..7501a7fb6a 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h @@ -73,6 +73,7 @@ public: MAV = 13, // mavlite #endif //HAL_WITH_FRSKY_TELEM_BIDIRECTIONAL TERRAIN = 14, // 0x500B terrain data + WIND = 15, // 0x500C wind data WFQ_LAST_ITEM // must be last }; @@ -108,6 +109,7 @@ private: uint32_t calc_attiandrng(void); uint32_t calc_rpm(void); uint32_t calc_terrain(void); + uint32_t calc_wind(void); // use_external_data is set when this library will // be providing data to another transport, such as FPort