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

AP_RCTelemetry: added ExpressLRS support

This commit is contained in:
yaapu 2021-11-01 18:13:21 +01:00 committed by Andrew Tridgell
parent 55bfa41d13
commit c0663da918
2 changed files with 107 additions and 45 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

129
libraries/AP_RCTelemetry/AP_CRSF_Telem.cpp
View File

@ -39,6 +39,9 @@
#endif
extern const AP_HAL::HAL& hal;
const uint8_t AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE;
const uint8_t AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE;
const uint8_t AP_CRSF_Telem::CRSF_RX_DEVICE_PING_MAX_RETRY;
AP_CRSF_Telem *AP_CRSF_Telem::singleton;
@ -97,7 +100,7 @@ void AP_CRSF_Telem::setup_custom_telemetry()
// check if passthru already assigned
const int8_t frsky_port = AP::serialmanager().find_portnum(AP_SerialManager::SerialProtocol_FrSky_SPort_Passthrough,0);
if (frsky_port != -1) {
gcs().send_text(MAV_SEVERITY_CRITICAL, "CRSF: passthrough telemetry conflict on SERIAL%d",frsky_port);
gcs().send_text(MAV_SEVERITY_CRITICAL, "%s: passthrough telemetry conflict on SERIAL%d", get_protocol_string(), frsky_port);
_custom_telem.init_done = true;
return;
}
@ -112,15 +115,13 @@ void AP_CRSF_Telem::setup_custom_telemetry()
return;
}
// setup the frsky scheduler for crossfire
// setup the frsky scheduler for crossfire and elrs
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
@ -128,7 +129,8 @@ void AP_CRSF_Telem::setup_custom_telemetry()
// 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);
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: custom telem init done, fw %d.%02d", get_protocol_string(), _crsf_version.major, _crsf_version.minor);
_custom_telem.init_done = true;
}
@ -140,15 +142,28 @@ void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_
}
if (is_high_speed_telemetry(rf_mode)) {
// standard telemetry for high data rates
set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
// 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
set_scheduler_entry(GPS, 550, 500); // 2.0Hz
set_scheduler_entry(PASSTHROUGH, 100, 100); // 8Hz
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
// standard telemetry for low data rates
set_scheduler_entry(BATTERY, 1000, 2000); // 0.5Hz
set_scheduler_entry(ATTITUDE, 1000, 3000); // 0.33Hz
if (_crsf_version.is_elrs) {
// ELRS custom telemetry for low data rates
set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
set_scheduler_entry(PASSTHROUGH, 350, 500); // 2.0Hz
set_scheduler_entry(STATUS_TEXT, 500, 2000); // 0.5Hz
} else {
// CRSF 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
}
}
}
@ -168,22 +183,25 @@ void AP_CRSF_Telem::process_rf_mode_changes()
}
// warn the user if their setup is sub-optimal
if (_telem_last_report_ms == 0 && !uart->is_dma_enabled()) {
gcs().send_text(MAV_SEVERITY_WARNING, "CRSF: running on non-DMA serial port");
gcs().send_text(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string());
}
// note if option was set to show LQ in place of RSSI
bool current_lq_as_rssi_active = bool(rc().use_crsf_lq_as_rssi());
if(_telem_last_report_ms == 0 || _noted_lq_as_rssi_active != current_lq_as_rssi_active){
_noted_lq_as_rssi_active = current_lq_as_rssi_active;
gcs().send_text(MAV_SEVERITY_INFO, "CRSF: RSSI now displays %s", current_lq_as_rssi_active ? " as LQ" : "normally");
gcs().send_text(MAV_SEVERITY_INFO, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally");
}
// report a change in RF mode or a chnage of more than 10Hz if we haven't done so in the last 5s
if ((now - _telem_last_report_ms > 5000) &&
(_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
if (!rc().suppress_crsf_message()) {
gcs().send_text(MAV_SEVERITY_INFO, "CRSFv%d: RF mode %d, rate is %dHz", uint8_t(2 + AP::crsf()->is_crsf_v3_active()),
(uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
const bool is_high_speed = is_high_speed_telemetry(current_rf_mode);
if ((now - _telem_last_report_ms > 5000)) {
// report an RF mode change or a change in telemetry rate if we haven't done so in the last 5s
if (!rc().suppress_crsf_message() && (_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
gcs().send_text(MAV_SEVERITY_INFO, "%s: RF Mode %d, telemetry rate is %dHz", get_protocol_string(), uint8_t(current_rf_mode) - (_crsf_version.is_elrs ? uint8_t(AP_RCProtocol_CRSF::RFMode::ELRS_RF_MODE_4HZ) : 0), get_telemetry_rate());
}
update_custom_telemetry_rates(current_rf_mode);
// tune the scheduler based on telemetry speed high/low transitions
if (_telem_is_high_speed != is_high_speed) {
update_custom_telemetry_rates(current_rf_mode);
}
_telem_is_high_speed = is_high_speed;
_telem_rf_mode = current_rf_mode;
_telem_last_avg_rate = _scheduler.avg_packet_rate;
_telem_last_report_ms = now;
@ -193,7 +211,8 @@ void AP_CRSF_Telem::process_rf_mode_changes()
// 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.major == 4 && _crsf_version.minor >= 6))) {
if (!_crsf_version.pending &&
((_crsf_version.major > 4 || (_crsf_version.major == 4 && _crsf_version.minor >= 6)) || _crsf_version.is_elrs)) {
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM;
}
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY;
@ -203,30 +222,33 @@ 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;
return AP_RCProtocol_CRSF::RFMode::RF_MODE_UNKNOWN;
}
if (!_crsf_version.pending && _crsf_version.use_rf_mode) {
return crsf->get_link_status().rf_mode;
if (_crsf_version.is_elrs) {
return static_cast<AP_RCProtocol_CRSF::RFMode>(uint8_t(AP_RCProtocol_CRSF::RFMode::ELRS_RF_MODE_4HZ) + crsf->get_link_status().rf_mode);
}
return static_cast<AP_RCProtocol_CRSF::RFMode>(crsf->get_link_status().rf_mode);
} else if (_crsf_version.is_tracer) {
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
}
/*
Note:
- rf mode 2 on UARTS with DMA runs @160Hz
- rf mode 2 on UARTS with no DMA runs @70Hz
- CRSF rf mode 2 on UARTS with DMA runs @160Hz
- CRSF rf mode 2 on UARTS with no DMA runs @70Hz
*/
if (get_avg_packet_rate() < 40U) {
// no DMA rf mode 1
// no DMA CRSF rf mode 1
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
}
if (get_avg_packet_rate() > 120U) {
// DMA rf mode 2
// DMA CRSF rf mode 2
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
}
if (get_max_packet_rate() < 120U) {
// no DMA rf mode 2
// no CRSF DMA rf mode 2
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
}
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
@ -234,7 +256,26 @@ AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const
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 || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
if (!_crsf_version.is_elrs) {
return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
}
return get_telemetry_rate() > 30;
}
uint16_t AP_CRSF_Telem::get_telemetry_rate() const
{
if (!_crsf_version.is_elrs) {
return get_avg_packet_rate();
}
AP_RCProtocol_CRSF* crsf = AP::crsf();
if (crsf == nullptr) {
return get_avg_packet_rate();
}
// ELRS sends 1 telemetry frame every n RC frames
// the 1:n ratio is user selected
// RC rate is measured by get_avg_packet_rate()
// telemetry rate = air rate - RC rate
return uint16_t(AP_RCProtocol_CRSF::elrs_air_rates[MIN(crsf->get_link_status().rf_mode, 7U)] - get_avg_packet_rate());
}
void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
@ -316,11 +357,11 @@ bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
_crsf_version.pending = false;
_crsf_version.minor = 0;
_crsf_version.major = 0;
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed");
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string());
} else {
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info");
gcs().send_text(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string());
}
}
return _pending_request.frame_type > 0;
@ -371,7 +412,8 @@ void AP_CRSF_Telem::process_packet(uint8_t idx)
} else {
// on slower links we pack many passthrough
// frames in a single crossfire one (up to 9)
get_multi_packet_passthrough_telem_data();
const uint8_t size = _crsf_version.is_elrs ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE;
get_multi_packet_passthrough_telem_data(size);
}
break;
case STATUS_TEXT:
@ -551,6 +593,10 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
const uint8_t offset = strnlen((char*)info->payload,42U);
if (strncmp((char*)info->payload, "Tracer", 6) == 0) {
_crsf_version.is_tracer = true;
} else if (strncmp((char*)&info->payload[offset+1], "ELRS", 4) == 0) {
// ELRS magic number is ELRS encoded in the serial number
// 0x45 'E' 0x4C 'L' 0x52 'R' 0x53 'S'
_crsf_version.is_elrs = true;
}
/*
fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes
@ -560,7 +606,7 @@ void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info)
_crsf_version.minor = info->payload[offset+12];
// should we use rf_mode reported by link statistics?
if (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72))) {
if (_crsf_version.is_elrs || (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) {
_crsf_version.use_rf_mode = true;
}
@ -1263,7 +1309,7 @@ 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)) {
if (!_crsf_version.is_elrs && !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)) {
@ -1318,22 +1364,23 @@ void AP_CRSF_Telem::get_single_packet_passthrough_telem_data()
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()
void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data(uint8_t size)
{
size = MIN(size, AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
_telem_pending = false;
uint8_t count = 0;
AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {};
AP_Frsky_SPort::sport_packet_t buffer[AP_CRSF_Telem::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))) {
if (!AP_Frsky_Telem::get_telem_data(buffer, count, size)) {
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; idx<count; idx++) {
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].appid = buffer[idx].appid;
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].data = buffer[idx].data;
_telem.bcast.custom_telem.multi_packet_passthrough.packets[idx].appid = buffer[idx].appid;
_telem.bcast.custom_telem.multi_packet_passthrough.packets[idx].data = buffer[idx].data;
}
_telem.bcast.custom_telem.multi_packet_passthrough.size = count;
_telem_size = sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame);
_telem_size = 2 + sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame::PassthroughTelemetryPacket)*count; //subtype + size + 6*count
_telem_type = get_custom_telem_frame_id();
_telem_pending = true;
}

23
libraries/AP_RCTelemetry/AP_CRSF_Telem.h
View File

@ -190,10 +190,10 @@ public:
struct PACKED PassthroughMultiPacketFrame {
uint8_t sub_type;
uint8_t size;
struct PACKED {
struct PACKED PassthroughTelemetryPacket {
uint16_t appid;
uint32_t data;
} frames[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
} packets[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE];
};
// Frame to hold status text message
@ -235,6 +235,18 @@ public:
ExtendedFrame ext;
};
// get the protocol string
const char* get_protocol_string() const {
if (_crsf_version.is_elrs) {
return "ELRS";
} else {
const AP_RCProtocol_CRSF* crsf = AP::crsf();
if (crsf && crsf->is_crsf_v3_active()) {
return "CRSFv3";
}
return "CRSFv2";
}
};
// Process a frame from the CRSF protocol decoder
static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
// process any changed settings and schedule for transmission
@ -263,7 +275,7 @@ private:
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 update_custom_telemetry_rates(const AP_RCProtocol_CRSF::RFMode rf_mode);
void calc_parameter_ping();
void calc_heartbeat();
@ -281,11 +293,12 @@ private:
void update_params();
void update_vtx_params();
void get_single_packet_passthrough_telem_data();
void get_multi_packet_passthrough_telem_data();
void get_multi_packet_passthrough_telem_data(uint8_t size = PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE);
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;
uint16_t get_telemetry_rate() const;
bool is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const;
void process_vtx_frame(VTXFrame* vtx);
@ -315,6 +328,7 @@ private:
uint32_t _telem_last_report_ms;
uint16_t _telem_last_avg_rate;
bool _telem_is_high_speed;
bool _telem_pending;
bool _enable_telemetry;
@ -330,6 +344,7 @@ private:
bool use_rf_mode;
bool is_tracer;
bool pending = true;
bool is_elrs;
} _crsf_version;
struct {