/* This program is free software: you can redistribute it and/or modify it under the terms of the GNU General Public License as published by the Free Software Foundation, either version 3 of the License, or (at your option) any later version. This program is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for more details. You should have received a copy of the GNU General Public License along with this program. If not, see . */ #include "AP_RCTelemetry_config.h" #if HAL_CRSF_TELEM_ENABLED #include "AP_CRSF_Telem.h" #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include //#define CRSF_DEBUG #ifdef CRSF_DEBUG # define debug(fmt, args...) hal.console->printf("CRSF: " fmt "\n", ##args) #else # define debug(fmt, args...) do {} while(0) #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; AP_CRSF_Telem::AP_CRSF_Telem() : AP_RCTelemetry(0) { singleton = this; } AP_CRSF_Telem::~AP_CRSF_Telem(void) { singleton = nullptr; } bool AP_CRSF_Telem::init(void) { // sanity check that we are using a UART for RC input if (!AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_RCIN, 0) && !AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) { return false; } // Someone explicitly configure CRSF control for VTX if (AP::serialmanager().have_serial(AP_SerialManager::SerialProtocol_CRSF, 0)) { AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::CRSF); } return AP_RCTelemetry::init(); } /* setup ready for passthrough telem */ void AP_CRSF_Telem::setup_wfq_scheduler(void) { // initialize packet weights for the WFQ scheduler // priority[i] = 1/_scheduler.packet_weight[i] // rate[i] = LinkRate * ( priority[i] / (sum(priority[1-n])) ) // 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 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 add_scheduler_entry(5, 20); // command 50Hz (generally not active unless requested by the TX) add_scheduler_entry(5, 500); // version ping 2Hz (only active at startup) add_scheduler_entry(5, 100); // device ping 10Hz (only active during TX loss, also see CRSF_RX_TIMEOUT) disable_scheduler_entry(DEVICE_PING); } void AP_CRSF_Telem::setup_custom_telemetry() { if (_custom_telem.init_done) { return; } if (!rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) { return; } // 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, "%s: passthrough telemetry conflict on SERIAL%d", get_protocol_string(), frsky_port); _custom_telem.init_done = true; 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 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(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,"%s: custom telem init done, fw %d.%02d", get_protocol_string(), _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)) { // 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 set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz } else { // 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 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 } } } bool AP_CRSF_Telem::process_rf_mode_changes() { const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode(); uint32_t now = AP_HAL::millis(); // the presence of a uart indicates that we are using CRSF for RC control AP_RCProtocol_CRSF* crsf = AP::crsf(); AP_HAL::UARTDriver* uart = nullptr; if (crsf != nullptr) { uart = crsf->get_UART(); } if (uart == nullptr) { return true; } if (!crsf->is_detected()) { return false; } // not ready yet if (!uart->is_initialized()) { return false; } #if !defined (STM32H7) // warn the user if their setup is sub-optimal, H7 does not need DMA on serial port if (_telem_bootstrap_msg_pending && !uart->is_dma_enabled()) { GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "%s: running on non-DMA serial port", get_protocol_string()); } #endif // note if option was set to show LQ in place of RSSI bool current_lq_as_rssi_active = rc().option_is_enabled(RC_Channels::Option::USE_CRSF_LQ_AS_RSSI); if(_telem_bootstrap_msg_pending || _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, "%s: RSSI now displays %s", get_protocol_string(), current_lq_as_rssi_active ? " as LQ" : "normally"); } _telem_bootstrap_msg_pending = false; 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().option_is_enabled(RC_Channels::Option::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: Link rate %dHz, Telemetry rate %dHz", get_protocol_string(), crsf->get_link_rate(_crsf_version.protocol), get_telemetry_rate()); } // 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; if (_telem_last_report_ms == 0) { // only want to show bootstrap messages once _telem_bootstrap_msg_pending = true; } _telem_last_report_ms = now; } return true; } // 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)) || is_elrs())) { 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::RF_MODE_UNKNOWN; } if (!_crsf_version.pending && _crsf_version.use_rf_mode) { if (is_elrs()) { return static_cast(uint8_t(AP_RCProtocol_CRSF::RFMode::CRSF_RF_MAX_MODES) + crsf->get_link_status().rf_mode); } return static_cast(crsf->get_link_status().rf_mode); } else if (is_tracer()) { return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ; } /* Note: - 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 CRSF rf mode 1 return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ; } if (get_avg_packet_rate() > 120U) { // DMA CRSF rf mode 2 return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ; } if (get_max_packet_rate() < 120U) { // no CRSF 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 { if (_crsf_version.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_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.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_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 crsf->get_link_rate(_crsf_version.protocol) - get_avg_packet_rate(); } 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().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY)) { return; } AP_RCTelemetry::queue_message(severity, text); } /* disable telemetry entries that require a transmitter to be present */ 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); disable_scheduler_entry(PASSTHROUGH); disable_scheduler_entry(STATUS_TEXT); // GENERAL_COMMAND and PARAMETERS will only be sent under very specific circumstances } /* enable telemetry entries that require a transmitter to be present */ 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); enable_scheduler_entry(PASSTHROUGH); enable_scheduler_entry(STATUS_TEXT); update_custom_telemetry_rates(_telem_rf_mode); } void AP_CRSF_Telem::enter_scheduler_params_mode() { debug("parameter passthrough enabled"); set_scheduler_entry(HEARTBEAT, 50, 200); // heartbeat 5Hz disable_tx_entries(); } void AP_CRSF_Telem::exit_scheduler_params_mode() { debug("parameter passthrough disabled"); // setup the crossfire scheduler for custom telemetry set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz enable_tx_entries(); } 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 && _pending_request.frame_type != AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO && !hal.util->get_soft_armed()) { // 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) { if (!process_rf_mode_changes()) { return false; } switch (idx) { case PARAMETERS: return _pending_request.frame_type > 0; case VTX_PARAMETERS: #if AP_VIDEOTX_ENABLED return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending; #else return false; #endif case PASSTHROUGH: return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY); case STATUS_TEXT: return rc().option_is_enabled(RC_Channels::Option::CRSF_CUSTOM_TELEMETRY) && !queue_empty; case GENERAL_COMMAND: return _baud_rate_request.pending || _bind_request_pending; case VERSION_PING: return _crsf_version.pending && AP::crsf()->is_detected(); // only send pings if protocol has been detected case HEARTBEAT: return true; // always send heartbeat if enabled case DEVICE_PING: return !_crsf_version.pending; // only send pings if version has been negotiated default: return _enable_telemetry; } } // WFQ scheduler void AP_CRSF_Telem::process_packet(uint8_t idx) { // send packet switch (idx) { case HEARTBEAT: // HEARTBEAT calc_heartbeat(); break; 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; #if AP_VIDEOTX_ENABLED case VTX_PARAMETERS: // update various VTX parameters update_vtx_params(); break; #endif case BATTERY: // BATTERY calc_battery(); break; case GPS: // GPS calc_gps(); break; 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) const uint8_t size = is_elrs() ? 3 : AP_CRSF_Telem::PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE; get_multi_packet_passthrough_telem_data(size); } break; case STATUS_TEXT: calc_status_text(); break; case GENERAL_COMMAND: if (_bind_request_pending) { calc_bind(); } else { calc_command_response(); } break; case VERSION_PING: // to get crossfire firmware version we send an RX device ping if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) { _crsf_version.pending = false; _crsf_version.minor = 0; _crsf_version.major = 0; disable_scheduler_entry(VERSION_PING); GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: RX device ping failed", get_protocol_string()); } else { calc_device_ping(AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER); uint32_t tnow_ms = AP_HAL::millis(); if ((tnow_ms - _crsf_version.last_request_info_ms) > 5000) { _crsf_version.last_request_info_ms = tnow_ms; GCS_SEND_TEXT(MAV_SEVERITY_DEBUG,"%s: requesting RX device info", get_protocol_string()); } } break; case DEVICE_PING: calc_device_ping(AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER); break; default: break; } } // Process a frame from the CRSF protocol decoder bool AP_CRSF_Telem::_process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data) { switch (frame_type) { // this means we are connected to an RC receiver and can send telemetry case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_RC_CHANNELS_PACKED: // the EVO sends battery frames and we should send telemetry back to populate the OSD case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR: _enable_telemetry = true; break; #if AP_VIDEOTX_ENABLED case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX: process_vtx_frame((VTXFrame*)data); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_VTX_TELEM: process_vtx_telem_frame((VTXTelemetryFrame*)data); break; #endif case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING: process_ping_frame((ParameterPingFrame*)data); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ: process_param_read_frame((ParameterSettingsReadFrame*)data); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_WRITE: process_param_write_frame((ParameterSettingsWriteFrame*)data); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO: process_device_info_frame((ParameterDeviceInfoFrame*)data); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND: process_command_frame((CommandFrame*)data); break; default: break; } return true; } #if AP_VIDEOTX_ENABLED void AP_CRSF_Telem::process_vtx_frame(VTXFrame* vtx) { vtx->user_frequency = be16toh(vtx->user_frequency); debug("VTX: SmartAudio: %d, Avail: %d, FreqMode: %d, Band: %d, Channel: %d, Freq: %d, PitMode: %d, Pwr: %d, Pit: %d", vtx->smart_audio_ver, vtx->is_vtx_available, vtx->is_in_user_frequency_mode, vtx->band, vtx->channel, vtx->is_in_user_frequency_mode ? vtx->user_frequency : AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel), vtx->is_in_pitmode, vtx->power, vtx->pitmode); AP_VideoTX& apvtx = AP::vtx(); // the user may have a VTX connected but not want AP to control it // (for instance because they are using myVTX on the transmitter) if (!apvtx.get_enabled()) { return; } apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF); apvtx.set_band(vtx->band); apvtx.set_channel(vtx->channel); if (vtx->is_in_user_frequency_mode) { apvtx.set_frequency_mhz(vtx->user_frequency); } else { apvtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx->band, vtx->channel)); } // 14dBm (25mW), 20dBm (100mW), 26dBm (400mW), 29dBm (800mW) switch (vtx->power) { case 0: apvtx.set_power_mw(25); break; case 1: apvtx.set_power_mw(100); break; case 2: apvtx.set_power_mw(400); break; case 3: apvtx.set_power_mw(800); break; } if (vtx->is_in_pitmode) { apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); } else { apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); } // make sure the configured values now reflect reality if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) { AP::vtx().announce_vtx_settings(); } _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false; } void AP_CRSF_Telem::process_vtx_telem_frame(VTXTelemetryFrame* vtx) { vtx->frequency = be16toh(vtx->frequency); debug("VTXTelemetry: Freq: %d, PitMode: %d, Power: %d", vtx->frequency, vtx->pitmode, vtx->power); AP_VideoTX& apvtx = AP::vtx(); if (!apvtx.get_enabled()) { return; } apvtx.set_provider_enabled(AP_VideoTX::VTXType::CRSF); apvtx.set_frequency_mhz(vtx->frequency); AP_VideoTX::VideoBand band; uint8_t channel; if (AP_VideoTX::get_band_and_channel(vtx->frequency, band, channel)) { apvtx.set_band(uint8_t(band)); apvtx.set_channel(channel); } apvtx.set_power_dbm(vtx->power); if (vtx->pitmode) { apvtx.set_options(apvtx.get_options() | uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); } else { apvtx.set_options(apvtx.get_options() & ~uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)); } // make sure the configured values now reflect reality if (!apvtx.set_defaults() && (_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending)) { AP::vtx().announce_vtx_settings(); } _vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false; } #endif // AP_VIDEOTX_ENABLED // request for device info void AP_CRSF_Telem::process_ping_frame(ParameterPingFrame* ping) { debug("process_ping_frame: %d -> %d", ping->origin, ping->destination); if (ping->destination != 0 && ping->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) { return; // request was not for us } _param_request.origin = ping->origin; _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO; _pending_request.destination = ping->origin; } // request for device info void AP_CRSF_Telem::process_device_info_frame(ParameterDeviceInfoFrame* info) { debug("process_device_info_frame: 0x%x -> 0x%x", 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); if (strncmp((char*)info->payload, "Tracer", 6) == 0) { _crsf_version.protocol = AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_TRACER; } 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.protocol = AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_ELRS; } if (!is_elrs()) { /* 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]; } else { // ELRS does not populate the version field so cook up something sensible _crsf_version.major = 1; _crsf_version.minor = 0; } // should we use rf_mode reported by link statistics? if (is_elrs() || (!is_tracer() && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72)))) { _crsf_version.use_rf_mode = true; } _crsf_version.pending = false; disable_scheduler_entry(VERSION_PING); } // request for a general command void AP_CRSF_Telem::process_command_frame(CommandFrame* command) { debug("process_command_frame: 0x%x -> 0x%x: 0x%x", command->origin, command->destination, command->payload[0]); if (command->destination != 0 && command->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) { return; // request was not for us } // we are only interested in commands from the RX if (command->origin != 0 && command->origin != AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER) { return; } switch (command->payload[0]) { case AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_PROPOSAL: { uint32_t baud_rate = command->payload[2] << 24 | command->payload[3] << 16 | command->payload[4] << 8 | command->payload[5]; _baud_rate_request.port_id = command->payload[1]; _baud_rate_request.valid = AP::crsf()->change_baud_rate(baud_rate); _baud_rate_request.pending = true; debug("requested baud rate change %lu", baud_rate); break; } default: break; // do nothing } } void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame) { debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination, read_frame->param_num, read_frame->param_chunk); if (read_frame->destination != 0 && read_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) { return; // request was not for us } _param_request = *read_frame; _pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ; } void AP_CRSF_Telem::process_pending_requests() { // handle general parameter requests switch (_pending_request.frame_type) { // construct a response to a ping frame case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO: _custom_telem.params_mode_start_ms = AP_HAL::millis(); calc_device_info(); break; // construct a ping frame originating here case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING: calc_device_ping(_pending_request.destination); break; case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ: // reset parameter passthrough timeout _custom_telem.params_mode_start_ms = AP_HAL::millis(); calc_parameter(); break; default: break; } _pending_request.frame_type = 0; } #if AP_VIDEOTX_ENABLED void AP_CRSF_Telem::update_vtx_params() { AP_VideoTX& vtx = AP::vtx(); // This function does ugly things with the vtx parameters which will upset other providers if (!vtx.get_enabled() || !vtx.is_provider_enabled(AP_VideoTX::VTXType::CRSF)) { return; } _vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || vtx.update_frequency() || _vtx_freq_change_pending; // don't update the power if we are supposed to be in pitmode as this will take us out of pitmode const bool pitmode = vtx.get_configured_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE); _vtx_power_change_pending = !pitmode && (vtx.update_power() || _vtx_power_change_pending); _vtx_options_change_pending = vtx.update_options() || _vtx_options_change_pending; if (_vtx_freq_change_pending || _vtx_power_change_pending || _vtx_options_change_pending) { // make the desired frequency match the desired band and channel if (_vtx_freq_change_pending) { if (vtx.update_band() || vtx.update_channel()) { vtx.update_configured_frequency(); } else { vtx.update_configured_channel_and_band(); } } debug("update_params(): freq %d->%d, chan: %d->%d, band: %d->%d, pwr: %d->%d, opts: %d->%d", vtx.get_frequency_mhz(), vtx.get_configured_frequency_mhz(), vtx.get_channel(), vtx.get_configured_channel(), vtx.get_band(), vtx.get_configured_band(), vtx.get_power_mw(), vtx.get_configured_power_mw(), vtx.get_options(), vtx.get_configured_options()); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND; _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX; _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX; uint8_t len = 5; // prioritize option changes so that the pilot can get in and out of pitmode if (_vtx_options_change_pending) { _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE; if (vtx.get_configured_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE)) { _telem.ext.command.payload[1] = 1; } else { _telem.ext.command.payload[1] = 0; } } else if (_vtx_freq_change_pending && _vtx_freq_update) { _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_FREQ; _telem.ext.command.payload[1] = (vtx.get_frequency_mhz() & 0xFF00) >> 8; _telem.ext.command.payload[2] = (vtx.get_frequency_mhz() & 0xFF); _vtx_freq_update = false; len++; } else if (_vtx_freq_change_pending) { _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_CHANNEL; _telem.ext.command.payload[1] = vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel(); _vtx_freq_update = true; } else if (_vtx_power_change_pending && _vtx_dbm_update) { _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_POWER_DBM; _telem.ext.command.payload[1] = vtx.get_configured_power_dbm(); _vtx_dbm_update = false; } else if (_vtx_power_change_pending) { _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_POWER; if (vtx.get_configured_power_mw() < 26) { vtx.set_configured_power_mw(25); } else if (vtx.get_configured_power_mw() < 201) { if (vtx.get_configured_power_mw() < 101) { vtx.set_configured_power_mw(100); } else { vtx.set_configured_power_mw(200); } } else if (vtx.get_configured_power_mw() < 501) { if (vtx.get_configured_power_mw() < 401) { vtx.set_configured_power_mw(400); } else { vtx.set_configured_power_mw(500); } } else { vtx.set_configured_power_mw(800); } _telem.ext.command.payload[1] = vtx.get_configured_power_level(); _vtx_dbm_update = true; } _telem_pending = true; // calculate command crc #pragma GCC diagnostic push #if defined(__GNUC__) && __GNUC__ >= 10 #pragma GCC diagnostic ignored "-Wstringop-overflow" #endif uint8_t* crcptr = &_telem.ext.command.destination; uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA); for (uint8_t i = 0; i < len; i++) { crc = crc8_dvb(crc, crcptr[i], 0xBA); } crcptr[len] = crc; _telem_size = len + 1; #pragma GCC diagnostic pop } } #endif // AP_VIDEOTX_ENABLED // prepare parameter ping data void AP_CRSF_Telem::calc_parameter_ping() { _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING; _telem.ext.ping.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_VTX; _telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem_size = sizeof(ParameterPingFrame); _telem_pending = true; } // prepare qos data - mandatory frame that must be sent periodically void AP_CRSF_Telem::calc_heartbeat() { _telem.bcast.heartbeat.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem_size = sizeof(HeartbeatFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_HEARTBEAT; _telem_pending = true; } // prepare battery data void AP_CRSF_Telem::calc_battery() { const AP_BattMonitor &_battery = AP::battery(); _telem.bcast.battery.voltage = htobe16(uint16_t(roundf(_battery.voltage(0) * 10.0f))); float current; if (!_battery.current_amps(current, 0)) { current = 0; } _telem.bcast.battery.current = htobe16(int16_t(roundf(current * 10.0f))); float used_mah; if (!_battery.consumed_mah(used_mah, 0)) { used_mah = 0; } uint8_t percentage = 0; IGNORE_RETURN(_battery.capacity_remaining_pct(percentage, 0)); _telem.bcast.battery.remaining = percentage; const int32_t capacity = used_mah; _telem.bcast.battery.capacity[0] = (capacity & 0xFF0000) >> 16; _telem.bcast.battery.capacity[1] = (capacity & 0xFF00) >> 8; _telem.bcast.battery.capacity[2] = (capacity & 0xFF); _telem_size = sizeof(BatteryFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_BATTERY_SENSOR; _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() { const Location &loc = AP::gps().location(0); // use the first gps instance (same as in send_mavlink_gps_raw) _telem.bcast.gps.latitude = htobe32(loc.lat); _telem.bcast.gps.longitude = htobe32(loc.lng); _telem.bcast.gps.groundspeed = htobe16(roundf(AP::gps().ground_speed() * 100000 / 3600)); _telem.bcast.gps.altitude = htobe16(constrain_int16(loc.alt / 100, 0, 5000) + 1000); _telem.bcast.gps.gps_heading = htobe16(roundf(AP::gps().ground_course() * 100.0f)); _telem.bcast.gps.satellites = AP::gps().num_sats(); _telem_size = sizeof(AP_CRSF_Telem::GPSFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_GPS; _telem_pending = true; } // prepare attitude data void AP_CRSF_Telem::calc_attitude() { AP_AHRS &_ahrs = AP::ahrs(); WITH_SEMAPHORE(_ahrs.get_semaphore()); const int16_t INT_PI = 31415; // units are radians * 10000 _telem.bcast.attitude.roll_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_roll()) * 10000.0f), -INT_PI, INT_PI)); _telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_pitch()) * 10000.0f), -INT_PI, INT_PI)); _telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.get_yaw()) * 10000.0f), -INT_PI, INT_PI)); _telem_size = sizeof(AP_CRSF_Telem::AttitudeFrame); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_ATTITUDE; _telem_pending = true; } // prepare flight mode data void AP_CRSF_Telem::calc_flight_mode() { AP_Notify * notify = AP_Notify::get_singleton(); if (notify) { // Note: snprintf() always terminates the string hal.util->snprintf( _telem.bcast.flightmode.flight_mode, sizeof(AP_CRSF_Telem::FlightModeFrame), "%s%s", notify->get_flight_mode_str(), rc().option_is_enabled(RC_Channels::Option::CRSF_FM_DISARM_STAR) && !hal.util->get_soft_armed() ? "*" : "" ); // Note: strlen(_telem.bcast.flightmode.flight_mode) is safe because called on a guaranteed null terminated string _telem_size = strlen(_telem.bcast.flightmode.flight_mode) + 1; //send the terminator as well _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_FLIGHT_MODE; _telem_pending = true; } } // return device information about ArduPilot void AP_CRSF_Telem::calc_device_info() { #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) _telem.ext.info.destination = _param_request.origin; _telem.ext.info.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; const AP_FWVersion &fwver = AP::fwversion(); // write out the name with version, max width is 60 - 18 = the meaning of life int32_t n = strlen(fwver.fw_short_string); strncpy((char*)_telem.ext.info.payload, fwver.fw_short_string, 41); n = MIN(n + 1, 42); put_be32_ptr(&_telem.ext.info.payload[n], // serial number uint32_t(fwver.major) << 24 | uint32_t(fwver.minor) << 16 | uint32_t(fwver.patch) << 8 | uint32_t(fwver.fw_type)); n += 4; put_be32_ptr(&_telem.ext.info.payload[n], // hardware id uint32_t(fwver.vehicle_type) << 24 | uint32_t(fwver.board_type) << 16 | uint32_t(fwver.board_subtype)); n += 4; put_be32_ptr(&_telem.ext.info.payload[n], fwver.os_sw_version); // software id n += 4; #if OSD_PARAM_ENABLED _telem.ext.info.payload[n++] = AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; // param count #else _telem.ext.info.payload[n++] = 0; // param count #endif _telem.ext.info.payload[n++] = 0; // param version _telem_size = n + 2; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO; _pending_request.frame_type = 0; _telem_pending = true; #endif } // send a device ping void AP_CRSF_Telem::calc_device_ping(uint8_t destination) { _telem.ext.ping.destination = 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.frame_type = 0; _telem_pending = true; } // send a command response void AP_CRSF_Telem::calc_command_response() { _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER; _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL; _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_GENERAL_CRSF_SPEED_RESPONSE; _telem.ext.command.payload[1] = _baud_rate_request.port_id; _telem.ext.command.payload[2] = _baud_rate_request.valid; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND; // calculate command crc uint8_t len = 6; uint8_t* crcptr = &_telem.ext.command.destination; uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA); for (uint8_t i = 0; i < len; i++) { crc = crc8_dvb(crc, crcptr[i], 0xBA); } crcptr[len] = crc; _telem_size = len + 1; _pending_request.frame_type = 0; _baud_rate_request.pending = false; debug("sent baud rate response: %u", _baud_rate_request.valid); _telem_pending = true; } // send a command response void AP_CRSF_Telem::calc_bind() { _telem.ext.command.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER; _telem.ext.command.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; _telem.ext.command.command_id = AP_RCProtocol_CRSF::CRSF_COMMAND_RX; _telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_RX_BIND; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND; // calculate command crc uint8_t len = 4; uint8_t* crcptr = &_telem.ext.command.destination; uint8_t crc = crc8_dvb(0, AP_RCProtocol_CRSF::CRSF_FRAMETYPE_COMMAND, 0xBA); for (uint8_t i = 0; i < len; i++) { crc = crc8_dvb(crc, crcptr[i], 0xBA); } crcptr[len] = crc; _telem_size = len + 1; _pending_request.frame_type = 0; _bind_request_pending = false; debug("sent bind request"); _telem_pending = true; } // return parameter information void AP_CRSF_Telem::calc_parameter() { #if OSD_PARAM_ENABLED _telem.ext.param_entry.header.destination = _param_request.origin; _telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; size_t idx = 0; // root folder request if (_param_request.param_num == 0) { _telem.ext.param_entry.header.param_num = 0; _telem.ext.param_entry.header.chunks_left = 0; _telem.ext.param_entry.payload[idx++] = 0; // parent folder _telem.ext.param_entry.payload[idx++] = ParameterType::FOLDER; // type _telem.ext.param_entry.payload[idx++] = 'r'; // "root" name _telem.ext.param_entry.payload[idx++] = 'o'; _telem.ext.param_entry.payload[idx++] = 'o'; _telem.ext.param_entry.payload[idx++] = 't'; _telem.ext.param_entry.payload[idx++] = 0; // null terminator // write out all of the ids we are going to send for (uint8_t i = 0; i < AP_OSD_ParamScreen::NUM_PARAMS * AP_OSD_NUM_PARAM_SCREENS; i++) { _telem.ext.param_entry.payload[idx++] = i + 1; } _telem.ext.param_entry.payload[idx] = 0xFF; // terminator _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY; _pending_request.frame_type = 0; _telem_pending = true; return; } AP_OSD* osd = AP::osd(); if (osd == nullptr) { return; } AP_OSD_ParamSetting* param = osd->get_setting((_param_request.param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS, (_param_request.param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS); if (param == nullptr) { return; } _telem.ext.param_entry.header.param_num = _param_request.param_num; #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED if (param->get_custom_metadata() != nullptr) { calc_text_selection(param, _param_request.param_chunk); return; } #endif _telem.ext.param_entry.header.chunks_left = 0; _telem.ext.param_entry.payload[idx++] = 0; // parent folder idx++; // leave a gap for the type param->copy_name_camel_case((char*)&_telem.ext.param_entry.payload[idx], 17); idx += strnlen((char*)&_telem.ext.param_entry.payload[idx], 16) + 1; switch (param->_param_type) { case AP_PARAM_INT8: { AP_Int8* p = (AP_Int8*)param->_param; _telem.ext.param_entry.payload[1] = ParameterType::INT8; _telem.ext.param_entry.payload[idx] = p->get(); // value _telem.ext.param_entry.payload[idx+1] = int8_t(param->_param_min); // min _telem.ext.param_entry.payload[idx+2] = int8_t(param->_param_max); // max _telem.ext.param_entry.payload[idx+3] = int8_t(0); // default idx += 4; break; } case AP_PARAM_INT16: { AP_Int16* p = (AP_Int16*)param->_param; _telem.ext.param_entry.payload[1] = ParameterType::INT16; put_be16_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value put_be16_ptr(&_telem.ext.param_entry.payload[idx+2], param->_param_min); // min put_be16_ptr(&_telem.ext.param_entry.payload[idx+4], param->_param_max); // max put_be16_ptr(&_telem.ext.param_entry.payload[idx+6], 0); // default idx += 8; break; } case AP_PARAM_INT32: { AP_Int32* p = (AP_Int32*)param->_param; _telem.ext.param_entry.payload[1] = ParameterType::FLOAT; #define FLOAT_ENCODE(f) (int32_t(roundf(f))) put_be32_ptr(&_telem.ext.param_entry.payload[idx], p->get()); // value put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(param->_param_min)); // min put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(param->_param_max)); // max put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default #undef FLOAT_ENCODE _telem.ext.param_entry.payload[idx+16] = 0; // decimal point put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], 1); // step size idx += 21; break; } case AP_PARAM_FLOAT: { AP_Float* p = (AP_Float*)param->_param; _telem.ext.param_entry.payload[1] = ParameterType::FLOAT; uint8_t digits = 0; const float incr = MAX(0.001f, param->_param_incr); // a bug in OpenTX prevents this going any smaller for (float floatp = incr; floatp < 1.0f; floatp *= 10) { digits++; } const float mult = powf(10, digits); #define FLOAT_ENCODE(f) (int32_t(roundf(mult * f))) put_be32_ptr(&_telem.ext.param_entry.payload[idx], FLOAT_ENCODE(p->get())); // value put_be32_ptr(&_telem.ext.param_entry.payload[idx+4], FLOAT_ENCODE(param->_param_min)); // min put_be32_ptr(&_telem.ext.param_entry.payload[idx+8], FLOAT_ENCODE(param->_param_max)); // max put_be32_ptr(&_telem.ext.param_entry.payload[idx+12], FLOAT_ENCODE(0.0f)); // default _telem.ext.param_entry.payload[idx+16] = digits; // decimal point put_be32_ptr(&_telem.ext.param_entry.payload[idx+17], FLOAT_ENCODE(incr)); // step size #undef FLOAT_ENCODE //debug("Encoding param %f(%f -> %f, %f) as %d(%d) (%d -> %d, %d)", p->get(), // param->_param_min.get(), param->_param_max.get(), param->_param_incr.get(), // int(FLOAT_ENCODE(p->get())), digits, int(FLOAT_ENCODE(param->_param_min)), // int(FLOAT_ENCODE(param->_param_max)), int(FLOAT_ENCODE(param->_param_incr))); idx += 21; break; } default: return; } _telem.ext.param_entry.payload[idx] = 0; // units _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + 1 + idx; _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY; _pending_request.frame_type = 0; _telem_pending = true; #endif // OSD_PARAM_ENABLED } #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED // class that spits out a chunk of data from a larger stream of contiguous chunks // the caller describes which chunk it needs and provides this class with all of the data // data is not written until the start position is reached and after a whole chunk // is accumulated the rest of the data is skipped in order to determine how many chunks // are left to be sent class BufferChunker { public: BufferChunker(uint8_t* buf, uint16_t chunk_size, uint16_t start_chunk) : _buf(buf), _idx(0), _start_chunk(start_chunk), _chunk_size(chunk_size), _chunk(0), _bytes(0) { } // accumulate a string, writing to the underlying buffer as required void put_string(const char* str, uint16_t str_len) { // skip over data we have already written or have yet to write if (_chunk != _start_chunk) { if (skip_bytes(str_len)) { // partial write strncpy((char*)_buf, &str[str_len - _idx], _idx); _bytes += _idx; } return; } uint16_t rem = remaining(); if (rem > str_len) { strncpy_noterm((char*)&_buf[_idx], str, str_len); _idx += str_len; _bytes += str_len; } else { strncpy_noterm((char*)&_buf[_idx], str, rem); _chunk++; _idx += str_len; _bytes += rem; _idx %= _chunk_size; } } // accumulate a byte, writing to the underlying buffer as required void put_byte(uint8_t b) { if (_chunk != _start_chunk) { if (skip_bytes(1)) { _buf[0] = b; _bytes++; } return; } if (remaining() > 0) { _buf[_idx++] = b; _bytes++; } else { _chunk++; _idx = 0; } } uint8_t chunks_remaining() const { return _chunk - _start_chunk; } uint8_t bytes_written() const { return _bytes; } private: uint16_t remaining() const { return _chunk_size - _bytes; } // skip over the requested number of bytes // returns true if we overflow into a chunk that needs to be written bool skip_bytes(uint16_t len) { _idx += len; if (_idx >= _chunk_size) { _chunk++; _idx %= _chunk_size; // partial write if (_chunk == _start_chunk && _idx > 0) { return true; } } return false; } uint8_t* _buf; uint16_t _idx; uint16_t _bytes; uint8_t _chunk; const uint16_t _start_chunk; const uint16_t _chunk_size; }; // provide information about a text selection, possibly over multiple chunks void AP_CRSF_Telem::calc_text_selection(AP_OSD_ParamSetting* param, uint8_t chunk) { const uint8_t CHUNK_SIZE = 56; const AP_OSD_ParamSetting::ParamMetadata* metadata = param->get_custom_metadata(); // chunk the output BufferChunker chunker(_telem.ext.param_entry.payload, CHUNK_SIZE, chunk); chunker.put_byte(0); // parent folder chunker.put_byte(ParameterType::TEXT_SELECTION); // parameter type char name[17]; param->copy_name_camel_case(name, 17); chunker.put_string(name, strnlen(name, 16)); // parameter name chunker.put_byte(0); // trailing null for (uint8_t i = 0; i < metadata->values_max; i++) { uint8_t len = strnlen(metadata->values[i], 16); if (len == 0) { chunker.put_string("---", 3); } else { chunker.put_string(metadata->values[i], len); } if (i == metadata->values_max - 1) { chunker.put_byte(0); } else { chunker.put_byte(';'); } } int32_t val = -1; switch (param->_param_type) { case AP_PARAM_INT8: val = ((AP_Int8*)param->_param)->get(); break; case AP_PARAM_INT16: val = ((AP_Int16*)param->_param)->get(); break; case AP_PARAM_INT32: val = ((AP_Int32*)param->_param)->get(); break; default: return; } // out of range values really confuse the TX val = constrain_int16(val, 0, metadata->values_max - 1); chunker.put_byte(val); // value chunker.put_byte(0); // min chunker.put_byte(metadata->values_max); // max chunker.put_byte(0); // default chunker.put_byte(0); // units _telem.ext.param_entry.header.chunks_left = chunker.chunks_remaining(); _telem_size = sizeof(AP_CRSF_Telem::ParameterSettingsEntryHeader) + chunker.bytes_written(); _telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY; _pending_request.frame_type = 0; _telem_pending = true; } #endif // HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED // write parameter information back into AP - assumes we already know the encoding for floats void AP_CRSF_Telem::process_param_write_frame(ParameterSettingsWriteFrame* write_frame) { debug("process_param_write_frame: %d -> %d", write_frame->origin, write_frame->destination); if (write_frame->destination != AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER) { return; // request was not for us } #if OSD_PARAM_ENABLED AP_OSD* osd = AP::osd(); if (osd == nullptr) { return; } AP_OSD_ParamSetting* param = osd->get_setting((write_frame->param_num - 1) / AP_OSD_ParamScreen::NUM_PARAMS, (write_frame->param_num - 1) % AP_OSD_ParamScreen::NUM_PARAMS); if (param == nullptr) { return; } #if HAL_CRSF_TELEM_TEXT_SELECTION_ENABLED bool text_selection = param->get_custom_metadata() != nullptr; #else bool text_selection = false; #endif switch (param->_param_type) { case AP_PARAM_INT8: { AP_Int8* p = (AP_Int8*)param->_param; p->set_and_save(write_frame->payload[0]); break; } case AP_PARAM_INT16: { AP_Int16* p = (AP_Int16*)param->_param; if (text_selection) { // if we have custom metadata then the parameter is a text selection p->set_and_save(write_frame->payload[0]); } else { p->set_and_save(be16toh_ptr(write_frame->payload)); } break; } case AP_PARAM_INT32: { AP_Int32* p = (AP_Int32*)param->_param; if (text_selection) { // if we have custom metadata then the parameter is a text selection p->set_and_save(write_frame->payload[0]); } else { p->set_and_save(be32toh_ptr(write_frame->payload)); } break; } case AP_PARAM_FLOAT: { AP_Float* p = (AP_Float*)param->_param; const int32_t val = be32toh_ptr(write_frame->payload); uint8_t digits = 0; const float incr = MAX(0.001f, param->_param_incr); // a bug in OpenTX prevents this going any smaller for (float floatp = incr; floatp < 1.0f; floatp *= 10) { digits++; } p->set_and_save(float(val) / powf(10, digits)); break; } default: break; } #endif // OSD_PARAM_ENABLED } // get status text data void AP_CRSF_Telem::calc_status_text() { if (!_statustext.available) { WITH_SEMAPHORE(_statustext.sem); // check link speed if (_crsf_version.protocol != AP_RCProtocol_CRSF::ProtocolType::PROTOCOL_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)) { 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; // Note: snprintf() always terminates the string hal.util->snprintf(_telem.bcast.custom_telem.status_text.text, AP_CRSF_Telem::PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE, "%s", _statustext.next.text); // frame size = sub_type(1) + severity(1) + strlen(text) + terminator // Note: strlen(_telem.bcast.custom_telem.status_text.text) is safe because called on a guaranteed null terminated string _telem_size = 2 + strlen(_telem.bcast.custom_telem.status_text.text) + 1; _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(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[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, 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; idxpayload, &_telem, _telem_size); data->device_address = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER; // sync byte data->length = _telem_size + 2; data->type = _telem_type; _telem_pending = false; return true; } /* fetch data for an external transport, such as CRSF */ bool AP_CRSF_Telem::process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data) { if (!get_singleton()) { return false; } return singleton->_process_frame(frame_type, data); } /* fetch data for an external transport, such as CRSF */ bool AP_CRSF_Telem::get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active) { if (!get_singleton()) { return false; } return singleton->_get_telem_data(data, is_tx_active); } AP_CRSF_Telem *AP_CRSF_Telem::get_singleton(void) { if (!singleton && !hal.util->get_soft_armed()) { // if telem data is requested when we are disarmed and don't // yet have a AP_CRSF_Telem object then try to allocate one new AP_CRSF_Telem(); // initialize the passthrough scheduler if (singleton) { singleton->init(); } } return singleton; } namespace AP { AP_CRSF_Telem *crsf_telem() { return AP_CRSF_Telem::get_singleton(); } }; #endif // HAL_CRSF_TELEM_ENABLED