diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp index 22a819cda3..2ebc3508bb 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp @@ -26,6 +26,7 @@ #include #include #include +#include #include #include @@ -73,26 +74,218 @@ void AP_CRSF_Telem::setup_wfq_scheduler(void) // CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit add_scheduler_entry(50, 100); // heartbeat 10Hz - add_scheduler_entry(200, 50); // parameters 20Hz (generally not active unless requested by the TX) + add_scheduler_entry(50, 50); // parameters 20Hz (generally not active unless requested by the TX) add_scheduler_entry(50, 120); // Attitude and compass 8Hz add_scheduler_entry(200, 1000); // VTX parameters 1Hz add_scheduler_entry(1300, 500); // battery 2Hz add_scheduler_entry(550, 280); // GPS 3Hz add_scheduler_entry(550, 500); // flight mode 2Hz + add_scheduler_entry(5000, 100); // passthrough max 10Hz + add_scheduler_entry(5000, 500); // status text max 2Hz +} + +void AP_CRSF_Telem::setup_custom_telemetry() +{ + if (_custom_telem.init_done) { + return; + } + + if (!rc().crsf_custom_telemetry()) { + return; + } + + // we need crossfire firmware version + if (_crsf_version.pending) { + return; + } + + AP_Frsky_SPort_Passthrough* passthrough = AP::frsky_passthrough_telem(); + if (passthrough == nullptr) { + return; + } + + // setup the frsky scheduler for crossfire + passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT); + passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON); + passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT); + passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz + + // setup the crossfire scheduler for custom telemetry + set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz + set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz + set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz + set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz + + _telem_rf_mode = get_rf_mode(); + // setup custom telemetry for current rf_mode + update_custom_telemetry_rates(_telem_rf_mode); + + gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: custom telem init done, fw %d.%02d", _crsf_version.major, _crsf_version.minor); + _custom_telem.init_done = true; +} + +void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode) +{ + // ignore rf mode changes if we are processing parameter packets + if (_custom_telem.params_mode_active) { + return; + } + + if (is_high_speed_telemetry(rf_mode)) { + // custom telemetry for high data rates + set_scheduler_entry(GPS, 550, 500); // 2.0Hz + set_scheduler_entry(PASSTHROUGH, 100, 100); // 10Hz + set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz + } else { + // custom telemetry for low data rates + set_scheduler_entry(GPS, 550, 1000); // 1.0Hz + set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz + set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz + } +} + +void AP_CRSF_Telem::process_rf_mode_changes() +{ + const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode(); + if ( _telem_rf_mode != current_rf_mode) { + gcs().send_text(MAV_SEVERITY_INFO, "CRSF: rf mode change %d->%d, rate is %dHz", (uint8_t)_telem_rf_mode, (uint8_t)current_rf_mode, _scheduler.avg_packet_rate); + update_custom_telemetry_rates(current_rf_mode); + _telem_rf_mode = current_rf_mode; + } +} + +// return custom frame id based on fw version +uint8_t AP_CRSF_Telem::get_custom_telem_frame_id() const +{ + if (!_crsf_version.pending && _crsf_version.major >= 4 && _crsf_version.minor >= 6) { + return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM; + } + return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY; +} + +AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const +{ + AP_RCProtocol_CRSF* crsf = AP::crsf(); + if (crsf == nullptr) { + return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_UNKNOWN; + } + + if (!_crsf_version.pending && _crsf_version.use_rf_mode) { + return crsf->get_link_status().rf_mode; + } + + /* + Note: + - rf mode 2 on UARTS with DMA runs @160Hz + - rf mode 2 on UARTS with no DMA runs @70Hz + */ + if (get_avg_packet_rate() < 40U) { + // no DMA rf mode 1 + return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ; + } + if (get_avg_packet_rate() > 120U) { + // DMA rf mode 2 + return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; + } + if (get_max_packet_rate() < 120U) { + // no DMA rf mode 2 + return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; + } + return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ; +} + +bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const +{ + return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; +} + +void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text) +{ + // no need to queue status text messages when crossfire + // custom telemetry is not enabled + if (!rc().crsf_custom_telemetry()) { + return; + } + AP_RCTelemetry::queue_message(severity, text); +} + +void AP_CRSF_Telem::enter_scheduler_params_mode() +{ + set_scheduler_entry(HEARTBEAT, 50, 100); // heartbeat 10Hz + set_scheduler_entry(ATTITUDE, 50, 120); // Attitude and compass 8Hz + set_scheduler_entry(BATTERY, 1300, 500); // battery 2Hz + set_scheduler_entry(GPS, 550, 280); // GPS 3Hz + set_scheduler_entry(FLIGHT_MODE, 550, 500); // flight mode 2Hz + + disable_scheduler_entry(PASSTHROUGH); + disable_scheduler_entry(STATUS_TEXT); +} + +void AP_CRSF_Telem::exit_scheduler_params_mode() +{ + // setup the crossfire scheduler for custom telemetry + set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz + set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz + set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz + set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz + + enable_scheduler_entry(PASSTHROUGH); + enable_scheduler_entry(STATUS_TEXT); + + update_custom_telemetry_rates(_telem_rf_mode); } void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty) { + uint32_t now_ms = AP_HAL::millis(); + setup_custom_telemetry(); + + /* + whenever we detect a pending request we configure the scheduler + to allow faster parameters processing. + We start a "fast parameter window" that we close after 5sec + */ + bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000; + if (!_custom_telem.params_mode_active && _pending_request.frame_type > 0) { + // fast window start + _custom_telem.params_mode_start_ms = now_ms; + _custom_telem.params_mode_active = true; + enter_scheduler_params_mode(); + } else if (expired && _custom_telem.params_mode_active) { + // fast window stop + _custom_telem.params_mode_active = false; + exit_scheduler_params_mode(); + } } // WFQ scheduler bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty) { + process_rf_mode_changes(); + switch (idx) { case PARAMETERS: - return _request_pending > 0; + // to get crossfire firmware version we send an RX device ping until we get a response + // but only if there are no other requests pending + if (_crsf_version.pending && _pending_request.frame_type == 0) { + if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) { + _crsf_version.pending = false; + _crsf_version.minor = 0; + _crsf_version.major = 0; + gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed"); + } else { + _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER; + _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO; + gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info"); + } + } + return _pending_request.frame_type > 0; case VTX_PARAMETERS: return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending; + case PASSTHROUGH: + return rc().crsf_custom_telemetry(); + case STATUS_TEXT: + return rc().crsf_custom_telemetry() && !queue_empty; default: return _enable_telemetry; } @@ -124,6 +317,20 @@ void AP_CRSF_Telem::process_packet(uint8_t idx) case FLIGHT_MODE: // GPS calc_flight_mode(); break; + case PASSTHROUGH: + if (is_high_speed_telemetry(_telem_rf_mode)) { + // on fast links we have 1:1 ratio between + // passthrough frames and crossfire frames + get_single_packet_passthrough_telem_data(); + } else { + // on slower links we pack many passthrough + // frames in a single crossfire one (up to 9) + get_multi_packet_passthrough_telem_data(); + } + break; + case STATUS_TEXT: + calc_status_text(); + break; default: break; } @@ -159,6 +366,10 @@ bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, voi process_param_write_frame((ParameterSettingsWriteFrame*)data); break; + case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO: + process_device_info_frame((ParameterDeviceInfoFrame*)data); + break; + default: break; } @@ -245,7 +456,45 @@ void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping) } _param_request.origin = ping->origin; - _request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; + _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; +} + +// request for device info +void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info) +{ + debug("process_device_info_frame: %d -> %d", info->origin, info->destination); + if (info->destination != 0 && info->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) { + return; // request was not for us + } + + // we are only interested in RC device info for firmware version detection + if (info->origin != 0 && info->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) { + return; + } + /* + Payload size is 58: + char[] Device name ( Null-terminated string, max len is 42 ) + uint32_t Serial number + uint32_t Hardware ID + uint32_t Firmware ID (0x00:0x00:0xAA:0xBB AA=major, BB=minor) + uint8_t Parameters count + uint8_t Parameter version number + */ + // get the terminator of the device name string + const uint8_t offset = strnlen((char*)info->payload,42U); + /* + fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes + fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes + */ + _crsf_version.major = info->payload[offset+11]; + _crsf_version.minor = info->payload[offset+12]; + + // should we use rf_mode reported by link statistics? + if (_crsf_version.major >= 3 && _crsf_version.minor >= 72) { + _crsf_version.use_rf_mode = true; + } + + _crsf_version.pending = false; } void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame) @@ -257,7 +506,7 @@ void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_fr } _param_request = *read_frame; - _request_pending = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ; + _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ; } // process any changed settings and schedule for transmission @@ -268,13 +517,16 @@ void AP_CRSF_Telem::update() void AP_CRSF_Telem::update_params() { // handle general parameter requests - switch (_request_pending) { + switch (_pending_request.frame_type) { case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING: calc_device_info(); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ: calc_parameter(); break; + case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO: + calc_device_ping(); + break; default: break; } @@ -495,11 +747,24 @@ void AP_CRSF_Telem::calc_device_info() { _telem_size = n + 2; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO; - _request_pending = 0; + _pending_request.frame_type = 0; _telem_pending = true; #endif } +// send a device ping +void AP_CRSF_Telem::calc_device_ping() { + _telem.ext.ping.destination = _pending_request.destination; + _telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; + _telem_size = 2; + _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; + + _pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST; + _pending_request.frame_type = 0; + + _telem_pending = true; +} + // return parameter information void AP_CRSF_Telem::calc_parameter() { #if OSD_PARAM_ENABLED @@ -601,7 +866,7 @@ void AP_CRSF_Telem::calc_parameter() { _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY; - _request_pending = 0; + _pending_request.frame_type = 0; _telem_pending = true; #endif } @@ -750,7 +1015,7 @@ void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chun _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written(); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY; - _request_pending = 0; + _pending_request.frame_type = 0; _telem_pending = true; } #endif @@ -826,6 +1091,86 @@ void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write #endif } +// get status text data +void AP_CRSF_Telem::calc_status_text() +{ + if (!_statustext.available) { + WITH_SEMAPHORE(_statustext.sem); + // check link speed + if (!is_high_speed_telemetry(_telem_rf_mode)) { + // keep only warning/error/critical/alert/emergency status text messages + bool got_message = false; + while (_statustext.queue.pop(_statustext.next)) { + if (_statustext.next.severity <= MAV_SEVERITY_WARNING) { + got_message = true; + break; + } + } + if (!got_message) { + return; + } + } else if (!_statustext.queue.pop(_statustext.next)) { + return; + } + _statustext.available = true; + } + + _telem_type = get_custom_telem_frame_id(); + _telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT; + _telem.bcast.custom_telem.status_text.severity = _statustext.next.severity; + strncpy_noterm(_telem.bcast.custom_telem.status_text.text, _statustext.next.text, PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE); + // add a potentially missing terminator + _telem.bcast.custom_telem.status_text.text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE-1] = 0; + _telem_size = 2 + PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; // sub_type(1) + severity(1) + text(50) + _telem_pending = true; + _statustext.available = false; +} + +/* + Get 1 packet of passthrough telemetry data +*/ +void AP_CRSF_Telem::get_single_packet_passthrough_telem_data() +{ + _telem_pending = false; + uint8_t packet_count; + AP_Frsky_SPort::sport_packet_t packet; + if (!AP_Frsky_Telem::get_telem_data(&packet, packet_count, 1)) { + return; + } + _telem.bcast.custom_telem.single_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH; + _telem.bcast.custom_telem.single_packet_passthrough.appid = packet.appid; + _telem.bcast.custom_telem.single_packet_passthrough.data = packet.data; + _telem_size = sizeof(AP_CRSF_Telem::PassthroughSinglePacketFrame); + _telem_type = get_custom_telem_frame_id(); + _telem_pending = true; +} + +/* + Get up to PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packets of passthrough telemetry data (for slow links) + Note: we have 2 distinct frame types (single packet vs multi packet) because + whenever possible we use smaller frames for they have a higher "chance" + of being transmitted by the crossfire RX scheduler. +*/ +void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data() +{ + _telem_pending = false; + uint8_t count = 0; + AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {}; + // we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets + if (!AP_Frsky_Telem::get_telem_data(buffer, count, ARRAY_SIZE(buffer))) { + return; + } + _telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH; + for (uint8_t idx=0; idxtype = _telem_type; _telem_pending = false; - return true; } diff --git a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h index 3cba1aff8c..1847c2bfcc 100644 --- a/libraries/AP_RCTelemetry/AP_CRSF_Telem.h +++ b/libraries/AP_RCTelemetry/AP_CRSF_Telem.h @@ -49,29 +49,34 @@ public: virtual bool init() override; static AP_CRSF_Telem *get_singleton(void); + void queue_message(MAV_SEVERITY severity, const char *text) override; + + static const uint8_t PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE = 50U; + static const uint8_t PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE = 9U; + static const uint8_t CRSF_RX_DEVICE_PING_MAX_RETRY = 50U; // Broadcast frame definitions courtesy of TBS - struct GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1! + struct PACKED GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1! int32_t latitude; // ( degree / 10`000`000 ) int32_t longitude; // (degree / 10`000`000 ) uint16_t groundspeed; // ( km/h / 100 ) uint16_t gps_heading; // ( degree / 100 ) uint16_t altitude; // ( meter - 1000m offset ) uint8_t satellites; // in use ( counter ) - } PACKED; + }; struct HeartbeatFrame { uint8_t origin; // Device addres }; - struct BatteryFrame { + struct PACKED BatteryFrame { uint16_t voltage; // ( mV * 100 ) uint16_t current; // ( mA * 100 ) uint8_t capacity[3]; // ( mAh ) uint8_t remaining; // ( percent ) - } PACKED; + }; - struct VTXFrame { + struct PACKED VTXFrame { #if __BYTE_ORDER != __LITTLE_ENDIAN #error "Only supported on little-endian architectures" #endif @@ -88,45 +93,45 @@ public: uint16_t user_frequency; uint8_t power : 4; // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3 uint8_t pitmode : 4; // off = 0, In_Band = 1, Out_Band = 2; - } PACKED; + }; - struct VTXTelemetryFrame { + struct PACKED VTXTelemetryFrame { uint8_t origin; // address uint8_t power; // power in dBm uint16_t frequency; // frequency in Mhz uint8_t pitmode; // disable 0, enable 1 - } PACKED; + }; - struct AttitudeFrame { + struct PACKED AttitudeFrame { int16_t pitch_angle; // ( rad * 10000 ) int16_t roll_angle; // ( rad * 10000 ) int16_t yaw_angle; // ( rad * 10000 ) - } PACKED; + }; - struct FlightModeFrame { + struct PACKED FlightModeFrame { char flight_mode[16]; // ( Null-terminated string ) - } PACKED; + }; // CRSF_FRAMETYPE_COMMAND - struct CommandFrame { + struct PACKED CommandFrame { uint8_t destination; uint8_t origin; uint8_t command_id; uint8_t payload[9]; // 8 maximum for LED command + crc8 - } PACKED; + }; // CRSF_FRAMETYPE_PARAM_DEVICE_PING - struct ParameterPingFrame { + struct PACKED ParameterPingFrame { uint8_t destination; uint8_t origin; - } PACKED; + }; // CRSF_FRAMETYPE_PARAM_DEVICE_INFO - struct ParameterDeviceInfoFrame { + struct PACKED ParameterDeviceInfoFrame { uint8_t destination; uint8_t origin; uint8_t payload[58]; // largest possible frame is 60 - } PACKED; + }; enum ParameterType : uint8_t { @@ -144,57 +149,90 @@ public: }; // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY - struct ParameterSettingsEntryHeader { + struct PACKED ParameterSettingsEntryHeader { uint8_t destination; uint8_t origin; uint8_t param_num; uint8_t chunks_left; - } PACKED; + }; // CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY - struct ParameterSettingsEntry { + struct PACKED ParameterSettingsEntry { ParameterSettingsEntryHeader header; uint8_t payload[56]; // largest possible frame is 60 - } PACKED; + }; // CRSF_FRAMETYPE_PARAMETER_READ - struct ParameterSettingsReadFrame { + struct PACKED ParameterSettingsReadFrame { uint8_t destination; uint8_t origin; uint8_t param_num; uint8_t param_chunk; - } PACKED _param_request; + } _param_request; // CRSF_FRAMETYPE_PARAMETER_WRITE - struct ParameterSettingsWriteFrame { + struct PACKED ParameterSettingsWriteFrame { uint8_t destination; uint8_t origin; uint8_t param_num; uint8_t payload[57]; // largest possible frame is 60 - } PACKED; + }; - union BroadcastFrame { + // Frame to hold passthrough telemetry + struct PACKED PassthroughSinglePacketFrame { + uint8_t sub_type; + uint16_t appid; + uint32_t data; + }; + + // Frame to hold passthrough telemetry + struct PACKED PassthroughMultiPacketFrame { + uint8_t sub_type; + uint8_t size; + struct PACKED { + uint16_t appid; + uint32_t data; + } frames[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE]; + }; + + // Frame to hold status text message + struct PACKED StatusTextFrame { + uint8_t sub_type; + uint8_t severity; + char text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE]; // ( Null-terminated string ) + }; + + // ardupilot frametype container + union PACKED APCustomTelemFrame { + PassthroughSinglePacketFrame single_packet_passthrough; + PassthroughMultiPacketFrame multi_packet_passthrough; + StatusTextFrame status_text; + }; + + + union PACKED BroadcastFrame { GPSFrame gps; HeartbeatFrame heartbeat; BatteryFrame battery; VTXFrame vtx; AttitudeFrame attitude; FlightModeFrame flightmode; - } PACKED; + APCustomTelemFrame custom_telem; + }; - union ExtendedFrame { + union PACKED ExtendedFrame { CommandFrame command; ParameterPingFrame ping; ParameterDeviceInfoFrame info; ParameterSettingsEntry param_entry; ParameterSettingsReadFrame param_read; ParameterSettingsWriteFrame param_write; - } PACKED; + }; - union TelemetryPayload { + union PACKED TelemetryPayload { BroadcastFrame bcast; ExtendedFrame ext; - } PACKED; + }; // Process a frame from the CRSF protocol decoder static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data); @@ -213,6 +251,8 @@ private: BATTERY, GPS, FLIGHT_MODE, + PASSTHROUGH, + STATUS_TEXT, NUM_SENSORS }; @@ -220,6 +260,8 @@ private: bool is_packet_ready(uint8_t idx, bool queue_empty) override; void process_packet(uint8_t idx) override; void adjust_packet_weight(bool queue_empty) override; + void setup_custom_telemetry(); + void update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode); void calc_parameter_ping(); void calc_heartbeat(); @@ -228,33 +270,65 @@ private: void calc_attitude(); void calc_flight_mode(); void calc_device_info(); + void calc_device_ping(); void calc_parameter(); #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED void calc_text_selection( AP_OSD_ParamSetting* param, uint8_t chunk); #endif void update_params(); void update_vtx_params(); + void get_single_packet_passthrough_telem_data(); + void get_multi_packet_passthrough_telem_data(); + void calc_status_text(); + void process_rf_mode_changes(); + uint8_t get_custom_telem_frame_id() const; + AP_RCProtocol_CRSF::RFMode get_rf_mode() const; + bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const; void process_vtx_frame(VTXFrame* vtx); void process_vtx_telem_frame(VTXTelemetryFrame* vtx); void process_ping_frame(ParameterPingFrame* ping); void process_param_read_frame(ParameterSettingsReadFrame* read); void process_param_write_frame(ParameterSettingsWriteFrame* write); + void process_device_info_frame(ParameterDeviceInfoFrame* info); - // setup ready for passthrough operation + // setup ready for passthrough operation void setup_wfq_scheduler(void) override; - // get next telemetry data for external consumers + // setup the scheduler for parameters download + void enter_scheduler_params_mode(); + void exit_scheduler_params_mode(); + + // get next telemetry data for external consumers bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data); bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data); TelemetryPayload _telem; uint8_t _telem_size; uint8_t _telem_type; + AP_RCProtocol_CRSF::RFMode _telem_rf_mode; bool _telem_pending; bool _enable_telemetry; - uint8_t _request_pending; + + struct { + uint8_t destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST; + uint8_t frame_type; + } _pending_request; + + struct { + uint8_t minor; + uint8_t major; + uint8_t retry_count; + bool use_rf_mode; + bool pending = true; + } _crsf_version; + + struct { + bool init_done; + uint32_t params_mode_start_ms; + bool params_mode_active; + } _custom_telem; // vtx state bool _vtx_freq_update; // update using the frequency method or not diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp index 40921309b3..76ccec2d5f 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.cpp @@ -13,7 +13,7 @@ along with this program. If not, see . */ -/* +/* Abstract Telemetry library */ @@ -60,12 +60,12 @@ void AP_RCTelemetry::update_avg_packet_rate() uint32_t poll_now = AP_HAL::millis(); _scheduler.avg_packet_counter++; - + if (poll_now - _scheduler.last_poll_timer > 1000) { //average in last 1000ms // initialize if (_scheduler.avg_packet_rate == 0) _scheduler.avg_packet_rate = _scheduler.avg_packet_counter; // moving average - _scheduler.avg_packet_rate = (uint8_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f; + _scheduler.avg_packet_rate = (uint16_t)_scheduler.avg_packet_rate * 0.75f + _scheduler.avg_packet_counter * 0.25f; // reset _scheduler.last_poll_timer = poll_now; _scheduler.avg_packet_counter = 0; @@ -83,14 +83,16 @@ void AP_RCTelemetry::update_avg_packet_rate() /* * WFQ scheduler + * returns the actual packet type index (if any) sent by the scheduler */ -void AP_RCTelemetry::run_wfq_scheduler(void) +uint8_t AP_RCTelemetry::run_wfq_scheduler(const bool use_shaper) { update_avg_packet_rate(); + update_max_packet_rate(); uint32_t now = AP_HAL::millis(); int8_t max_delay_idx = -1; - + float max_delay = 0; float delay = 0; bool packet_ready = false; @@ -99,7 +101,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void) check_sensor_status_flags(); // build message queue for ekf_status check_ekf_status(); - + // dynamic priorities bool queue_empty; { @@ -108,7 +110,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void) } adjust_packet_weight(queue_empty); - + // search the packet with the longest delay after the scheduled time for (int i=0; i<_time_slots; i++) { // normalize packet delay relative to packet weight @@ -116,8 +118,8 @@ void AP_RCTelemetry::run_wfq_scheduler(void) // use >= so with equal delays we choose the packet with lowest priority // this is ensured by the packets being sorted by desc frequency // apply the rate limiter - if (delay >= max_delay && ((now - _scheduler.packet_timer[i]) >= _scheduler.packet_min_period[i])) { - packet_ready = is_packet_ready(i, queue_empty); + if (delay >= max_delay && check_scheduler_entry_time_constraints(now, i, use_shaper)) { + packet_ready = is_scheduler_entry_enabled(i) && is_packet_ready(i, queue_empty); if (packet_ready) { max_delay = delay; @@ -125,10 +127,10 @@ void AP_RCTelemetry::run_wfq_scheduler(void) } } } - if (max_delay_idx < 0) { // nothing was ready - return; + return max_delay_idx; } + now = AP_HAL::millis(); #ifdef TELEM_DEBUG _scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2; @@ -137,6 +139,32 @@ void AP_RCTelemetry::run_wfq_scheduler(void) //debug("process_packet(%d): %f", max_delay_idx, max_delay); // send packet process_packet(max_delay_idx); + // let the caller know which packet type was sent + return max_delay_idx; +} + +/* + * do not run the scheduler and process a specific entry + */ +bool AP_RCTelemetry::process_scheduler_entry(const uint8_t slot ) +{ + if (slot >= TELEM_TIME_SLOT_MAX) { + return false; + } + if (!is_scheduler_entry_enabled(slot)) { + return false; + } + bool queue_empty; + { + WITH_SEMAPHORE(_statustext.sem); + queue_empty = !_statustext.available && _statustext.queue.is_empty(); + } + if (!is_packet_ready(slot, queue_empty)) { + return false; + } + process_packet(slot); + + return true; } /* @@ -248,7 +276,7 @@ void AP_RCTelemetry::check_ekf_status(void) } } } - + uint32_t AP_RCTelemetry::sensor_status_flags() const { uint32_t present; @@ -258,4 +286,3 @@ uint32_t AP_RCTelemetry::sensor_status_flags() const return ~health & enabled & present; } - diff --git a/libraries/AP_RCTelemetry/AP_RCTelemetry.h b/libraries/AP_RCTelemetry/AP_RCTelemetry.h index 18b1ec9dba..d29a2a1d47 100644 --- a/libraries/AP_RCTelemetry/AP_RCTelemetry.h +++ b/libraries/AP_RCTelemetry/AP_RCTelemetry.h @@ -17,6 +17,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) @@ -34,23 +35,66 @@ public: AP_RCTelemetry &operator=(const AP_RCTelemetry&) = delete; // add statustext message to message queue - void queue_message(MAV_SEVERITY severity, const char *text); + virtual void queue_message(MAV_SEVERITY severity, const char *text); + + // scheduler entry helpers + void enable_scheduler_entry(const uint8_t slot) { + if (slot >= TELEM_TIME_SLOT_MAX) { + return; + } + BIT_CLEAR(_disabled_scheduler_entries_bitmask, slot); + } + void disable_scheduler_entry(const uint8_t slot) { + if (slot >= TELEM_TIME_SLOT_MAX) { + return; + } + BIT_SET(_disabled_scheduler_entries_bitmask, slot); + } + void set_scheduler_entry_min_period(const uint8_t slot, const uint32_t min_period_ms) + { + if (slot >= TELEM_TIME_SLOT_MAX) { + return; + } + _scheduler.packet_min_period[slot] = min_period_ms; + } + bool is_scheduler_entry_enabled(const uint8_t slot) const { + if (slot >= TELEM_TIME_SLOT_MAX) { + return false; + } + return !BIT_IS_SET(_disabled_scheduler_entries_bitmask, slot); + } + // each derived class might provide a way to reset telemetry rates to default + virtual void reset_scheduler_entry_min_periods() {} // update error mask of sensors and subsystems. The mask uses the // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it // indicates that the sensor or subsystem is present but not // functioning correctly uint32_t sensor_status_flags() const; + uint16_t get_avg_packet_rate() const { + return _scheduler.avg_packet_rate; + } + uint16_t get_max_packet_rate() const { + return _scheduler.max_packet_rate; + } protected: - void run_wfq_scheduler(); + uint8_t run_wfq_scheduler(const bool use_shaper = true); + // process a specific entry + bool process_scheduler_entry(const uint8_t slot ); // set an entry in the scheduler table void set_scheduler_entry(uint8_t slot, uint32_t weight, uint32_t min_period_ms) { + if (slot >= TELEM_TIME_SLOT_MAX) { + return; + } _scheduler.packet_weight[slot] = weight; _scheduler.packet_min_period[slot] = min_period_ms; } // add an entry to the scheduler table void add_scheduler_entry(uint32_t weight, uint32_t min_period_ms) { + if (_time_slots >= TELEM_TIME_SLOT_MAX) { + return; + } set_scheduler_entry(_time_slots++, weight, min_period_ms); } // setup ready for passthrough operation @@ -65,7 +109,8 @@ protected: uint32_t packet_timer[TELEM_TIME_SLOT_MAX]; uint32_t packet_weight[TELEM_TIME_SLOT_MAX]; uint32_t packet_min_period[TELEM_TIME_SLOT_MAX]; - uint8_t avg_packet_rate; + uint16_t avg_packet_rate; + uint16_t max_packet_rate; #ifdef TELEM_DEBUG uint8_t packet_rate[TELEM_TIME_SLOT_MAX]; #endif @@ -81,6 +126,7 @@ protected: private: uint32_t check_sensor_status_timer; uint32_t check_ekf_status_timer; + uint32_t _disabled_scheduler_entries_bitmask; // passthrough WFQ scheduler virtual void setup_wfq_scheduler() = 0; @@ -88,8 +134,17 @@ private: virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; } virtual void process_packet(uint8_t idx) = 0; virtual void adjust_packet_weight(bool queue_empty) {}; + bool check_scheduler_entry_time_constraints(const uint32_t now, uint8_t slot, const bool use_shaper) const { + if (!use_shaper) { + return true; + } + return ((now - _scheduler.packet_timer[slot]) >= _scheduler.packet_min_period[slot]); + } void update_avg_packet_rate(); + void update_max_packet_rate() { + _scheduler.max_packet_rate = MAX(_scheduler.avg_packet_rate, _scheduler.max_packet_rate); + } // methods to convert flight controller data to FrSky SPort Passthrough (OpenTX) format void check_sensor_status_flags(void);