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

AP_RCTelemetry: add support for baro/vario CRSF frames

This commit is contained in:
Andy Piper 2024-04-24 16:52:13 +01:00 committed by Andrew Tridgell
parent 1e2621466c
commit 3a4fdb16a8
4 changed files with 134 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

69
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -81,6 +81,7 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void)
// CRSF telemetry rate is 150Hz (4ms) max, so these rates must fit
add_scheduler_entry(50, 100); // heartbeat 10Hz
add_scheduler_entry(5, 20); // parameters 50Hz (generally not active unless requested by the TX)
add_scheduler_entry(50, 200); // baro_vario 5Hz
add_scheduler_entry(50, 120); // Attitude and compass 8Hz
add_scheduler_entry(200, 1000); // VTX parameters 1Hz
add_scheduler_entry(1300, 500); // battery 2Hz
@ -152,6 +153,8 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
// standard telemetry for high data rates
set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
set_scheduler_entry(BARO_VARIO, 1000, 1000); // 1Hz
set_scheduler_entry(VARIO, 1000, 1000); // 1Hz
// custom telemetry for high data rates
set_scheduler_entry(GPS, 550, 500); // 2.0Hz
set_scheduler_entry(PASSTHROUGH, 100, 100); // 8Hz
@ -160,6 +163,8 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
// standard telemetry for low data rates
set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz
set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz
set_scheduler_entry(BARO_VARIO, 1000, 3000); // 0.33Hz
set_scheduler_entry(VARIO, 1000, 3000); // 0.33Hz
if (is_elrs()) {
// ELRS custom telemetry for low data rates
set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
@ -320,6 +325,8 @@ void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
void AP_CRSF_Telem::disable_tx_entries()
{
disable_scheduler_entry(ATTITUDE);
disable_scheduler_entry(BARO_VARIO);
disable_scheduler_entry(VARIO);
disable_scheduler_entry(BATTERY);
disable_scheduler_entry(GPS);
disable_scheduler_entry(FLIGHT_MODE);
@ -334,6 +341,8 @@ void AP_CRSF_Telem::disable_tx_entries()
void AP_CRSF_Telem::enable_tx_entries()
{
enable_scheduler_entry(ATTITUDE);
enable_scheduler_entry(BARO_VARIO);
enable_scheduler_entry(VARIO);
enable_scheduler_entry(BATTERY);
enable_scheduler_entry(GPS);
enable_scheduler_entry(FLIGHT_MODE);
@ -428,6 +437,12 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
case PARAMETERS: // update parameter settings
process_pending_requests();
break;
case BARO_VARIO:
calc_baro_vario();
break;
case VARIO:
calc_vario();
break;
case ATTITUDE:
calc_attitude();
break;
@ -914,6 +929,60 @@ void AP_CRSF_Telem::calc_battery()
_telem_pending = true;
}
uint16_t AP_CRSF_Telem::get_altitude_packed()
{
int32_t altitude_dm = get_nav_alt_m(Location::AltFrame::ABOVE_HOME) * 10;
enum : int32_t {
ALT_MIN_DM = 10000, // minimum altitude in dm
ALT_THRESHOLD_DM = 0x8000 - ALT_MIN_DM, // altitude of precision changing in dm
ALT_MAX_DM = 0x7ffe * 10 - 5, // maximum altitude in dm
};
if (altitude_dm < -ALT_MIN_DM) { // less than minimum altitude
return 0; // minimum
}
if (altitude_dm > ALT_MAX_DM) { // more than maximum
return 0xFFFEU; // maximum
}
if(altitude_dm < ALT_THRESHOLD_DM) { //dm-resolution range
return uint16_t(altitude_dm + ALT_MIN_DM);
}
return uint16_t((altitude_dm + 5) / 10) | uint16_t(0x8000); // meter-resolution range
}
int8_t AP_CRSF_Telem::get_vertical_speed_packed()
{
float vspeed = get_vspeed_ms();
float vertical_speed_cm_s = vspeed * 100.0f;
const int16_t Kl = 100; // linearity constant;
const float Kr = .026f; // range constant;
int8_t vspeed_packed = int8_t(logf(fabsf(vertical_speed_cm_s)/Kl + 1)/Kr);
return vspeed_packed * (is_negative(vertical_speed_cm_s) ? -1 : 1);
}
// prepare vario data
void AP_CRSF_Telem::calc_baro_vario()
{
_telem.bcast.baro_vario.altitude_packed = get_altitude_packed();
_telem.bcast.baro_vario.vertical_speed_packed = get_vertical_speed_packed();
_telem_size = sizeof(BaroVarioFrame);
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BARO_VARIO;
_telem_pending = true;
}
// prepare vario data
void AP_CRSF_Telem::calc_vario()
{
_telem.bcast.vario.v_speed = int16_t(get_vspeed_ms() * 100.0f);
_telem_size = sizeof(VarioFrame);
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VARIO;
_telem_pending = true;
}
// prepare gps data
void AP_CRSF_Telem::calc_gps()
{

17
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -64,6 +64,15 @@ public:
uint8_t remaining; // ( percent )
};
struct PACKED BaroVarioFrame {
uint16_t altitude_packed; // Altitude above start (calibration) point.
int8_t vertical_speed_packed; // vertical speed.
};
struct PACKED VarioFrame {
int16_t v_speed; // vertical speed cm/s
};
struct PACKED VTXFrame {
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
@ -201,6 +210,8 @@ public:
union PACKED BroadcastFrame {
GPSFrame gps;
HeartbeatFrame heartbeat;
BaroVarioFrame baro_vario;
VarioFrame vario;
BatteryFrame battery;
VTXFrame vtx;
AttitudeFrame attitude;
@ -244,6 +255,8 @@ private:
enum SensorType {
HEARTBEAT,
PARAMETERS,
BARO_VARIO,
VARIO,
ATTITUDE,
VTX_PARAMETERS,
BATTERY,
@ -267,6 +280,10 @@ private:
void calc_parameter_ping();
void calc_heartbeat();
void calc_battery();
uint16_t get_altitude_packed();
int8_t get_vertical_speed_packed();
void calc_baro_vario();
void calc_vario();
void calc_gps();
void calc_attitude();
void calc_flight_mode();

44
libraries/AP_RCTelemetry/AP_RCTelemetry.cpp
View File

@ -25,6 +25,7 @@
#include <stdio.h>
#include <math.h>
#include <AP_Vehicle/AP_Vehicle_Type.h>
#include <AP_Baro/AP_Baro.h>
#ifdef TELEM_DEBUG
# define debug(fmt, args...) hal.console->printf("Telem: " fmt "\n", ##args)
@ -293,3 +294,46 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const
return ~health & enabled & present;
}
/*
* get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s
*/
float AP_RCTelemetry::get_vspeed_ms(void)
{
{
// release semaphore as soon as possible
AP_AHRS &_ahrs = AP::ahrs();
Vector3f v;
WITH_SEMAPHORE(_ahrs.get_semaphore());
if (_ahrs.get_velocity_NED(v)) {
return -v.z;
}
}
auto &_baro = AP::baro();
WITH_SEMAPHORE(_baro.get_semaphore());
return _baro.get_climb_rate();
}
/*
* prepare altitude between vehicle and home location data
*/
float AP_RCTelemetry::get_nav_alt_m(Location::AltFrame frame)
{
Location loc;
float current_height = 0;
AP_AHRS &_ahrs = AP::ahrs();
WITH_SEMAPHORE(_ahrs.get_semaphore());
if (frame == Location::AltFrame::ABOVE_HOME) {
_ahrs.get_relative_position_D_home(current_height);
return -current_height;
}
if (_ahrs.get_location(loc)) {
if (!loc.get_alt_m(frame, current_height)) {
// ignore this error
}
}
return current_height;
}

4
libraries/AP_RCTelemetry/AP_RCTelemetry.h
View File

@ -18,6 +18,7 @@
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_Math/AP_Math.h>
#include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_Common/Location.h>
#define TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent)
@ -77,6 +78,9 @@ public:
return _scheduler.max_packet_rate;
}
static float get_vspeed_ms(void);
static float get_nav_alt_m(Location::AltFrame frame = Location::AltFrame::ABSOLUTE);
protected:
uint8_t run_wfq_scheduler(const bool use_shaper = true);
// process a specific entry