Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_Frsky_Telem: added frame 0x500C for true/apparent WIND info

This commit is contained in:
yaapu 2021-06-18 17:59:41 +02:00 committed by Andrew Tridgell
parent d91e2008d3
commit 617b023c6f
3 changed files with 75 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
libraries/AP_Frsky_Telem/AP_Frsky_SPort.cpp
View File

@ -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) {

73
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp
View File

@ -11,6 +11,9 @@
#include <AP_Terrain/AP_Terrain.h>
#include <AC_Fence/AC_Fence.h>
#include <GCS_MAVLink/GCS.h>
#if APM_BUILD_TYPE(APM_BUILD_Rover)
#include <AP_WindVane/AP_WindVane.h>
#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<<AP_THROTTLE_OFFSET;
ap_status |= prep_number(gcs().get_hud_throttle()*0.63, 2, 0)<<AP_THROTTLE_OFFSET;
// IMU temperature
ap_status |= imu_temp << AP_IMU_TEMP_OFFSET;
return ap_status;
@ -693,6 +717,41 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_terrain(void)
return value;
}
/*
* prepare wind data
* for FrSky SPort Passthrough (OpenTX) protocol (X-receivers)
* wind direction = 0 means North
*/
uint32_t AP_Frsky_SPort_Passthrough::calc_wind(void)
{
#if !APM_BUILD_TYPE(APM_BUILD_Rover)
Vector3f v;
{
AP_AHRS &ahrs = AP::ahrs();
WITH_SEMAPHORE(ahrs.get_semaphore());
v = ahrs.wind_estimate();
}
// wind angle in 3 degree increments 0,360 (unsigned)
uint32_t value = prep_number(roundf(wrap_360(degrees(atan2f(-v.y, -v.x))) * (1.0f/3.0f)), 2, 0);
// wind speed in dm/s
value |= prep_number(roundf(v.length() * 10), 2, 1) << WIND_SPEED_OFFSET;
#else
const AP_WindVane* windvane = AP_WindVane::get_singleton();
uint32_t value = 0;
if (windvane != nullptr && windvane->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

2
libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h
View File

@ -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