diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp index 9db8f890c3ad7..e78b576c27c45 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Backend.cpp @@ -5,6 +5,7 @@ #include #include #include +#include extern const AP_HAL::HAL& hal; @@ -44,47 +45,15 @@ void AP_Frsky_Backend::loop(void) } } -/* - * get vertical speed from ahrs, if not available fall back to baro climbrate, units is m/s - * for FrSky D and SPort protocols - */ -float AP_Frsky_Backend::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 * for FrSky D and SPort protocols */ void AP_Frsky_Backend::calc_nav_alt(void) { - _SPort_data.vario_vspd = (int32_t)(get_vspeed_ms()*100); //convert to cm/s - - Location loc; - float current_height = 0; - - AP_AHRS &_ahrs = AP::ahrs(); - WITH_SEMAPHORE(_ahrs.get_semaphore()); - if (_ahrs.get_location(loc)) { - if (!loc.get_alt_m(Location::AltFrame::ABSOLUTE, current_height)) { - // ignore this error - } - } + _SPort_data.vario_vspd = (int32_t)(AP_RCTelemetry::get_vspeed_ms()*100); //convert to cm/s + float current_height = AP_RCTelemetry::get_nav_alt_m(); _SPort_data.alt_nav_meters = float_to_uint16(current_height); _SPort_data.alt_nav_cm = float_to_uint16((current_height - _SPort_data.alt_nav_meters) * 100); } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp index 2f3acb0fb3627..db60fa40483bb 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.cpp @@ -646,7 +646,7 @@ uint32_t AP_Frsky_SPort_Passthrough::calc_home(void) */ uint32_t AP_Frsky_SPort_Passthrough::calc_velandyaw(void) { - float vspd = get_vspeed_ms(); + float vspd = AP_RCTelemetry::get_vspeed_ms(); // vertical velocity in dm/s uint32_t velandyaw = prep_number(roundf(vspd * 10), 2, 1); float airspeed_m; // m/s diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h index aaaf0fb2bf2d8..e4ebba502775e 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_CRSF.h @@ -80,7 +80,9 @@ class AP_RCProtocol_CRSF : public AP_RCProtocol_Backend { enum FrameType { CRSF_FRAMETYPE_GPS = 0x02, + CRSF_FRAMETYPE_VARIO = 0x07, CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08, + CRSF_FRAMETYPE_BARO_VARIO = 0x09, CRSF_FRAMETYPE_HEARTBEAT = 0x0B, CRSF_FRAMETYPE_VTX = 0x0F, CRSF_FRAMETYPE_VTX_TELEM = 0x10, diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index c645add90fcba..4f76626701f56 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -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() { diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index e0d557cb74cc7..f02e531a5759e 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -64,6 +64,15 @@ class AP_CRSF_Telem : public AP_RCTelemetry { 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 @@ class AP_CRSF_Telem : public AP_RCTelemetry { union PACKED BroadcastFrame { GPSFrame gps; HeartbeatFrame heartbeat; + BaroVarioFrame baro_vario; + VarioFrame vario; BatteryFrame battery; VTXFrame vtx; AttitudeFrame attitude; @@ -244,6 +255,8 @@ class AP_CRSF_Telem : public AP_RCTelemetry { enum SensorType { HEARTBEAT, PARAMETERS, + BARO_VARIO, + VARIO, ATTITUDE, VTX_PARAMETERS, BATTERY, @@ -267,6 +280,10 @@ class AP_CRSF_Telem : public AP_RCTelemetry { 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(); diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp index c57c3d35cefe1..3c7d4e3358e73 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp @@ -25,6 +25,7 @@ #include #include #include +#include #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; +} diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h index 40fc27ca838b2..5ebea0ff35770 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.h +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.h @@ -18,6 +18,7 @@ #include #include #include +#include #define TELEM_PAYLOAD_STATUS_CAPACITY 5 // size of the message buffer queue (max number of messages waiting to be sent) @@ -77,6 +78,9 @@ class AP_RCTelemetry { 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