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