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

1550 lines
58 KiB
C++
Raw Blame History

/*
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 <http://www.gnu.org/licenses/>.
*/
#include "AP_CRSF_Telem.h"
#include <AP_VideoTX/AP_VideoTX.h>
#include <AP_HAL/utility/sparse-endian.h>
#include <AP_BattMonitor/AP_BattMonitor.h>
#include <AP_Common/AP_FWVersion.h>
#include <AP_GPS/AP_GPS.h>
#include <GCS_MAVLink/GCS.h>
#include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Notify/AP_Notify.h>
#include <AP_OSD/AP_OSD.h>
#include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
#include <math.h>
#include <stdio.h>
#include <AP_HAL/AP_HAL.h>
#if HAL_CRSF_TELEM_ENABLED
#include <AP_Vehicle/AP_Vehicle_Type.h>
//#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;
}
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, 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
// 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
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<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 (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(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(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;
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 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:
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_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_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;
}
// process any changed settings and schedule for transmission
void AP_CRSF_Telem::update()
{
}
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();
if (!vtx.get_enabled()) {
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;
}
// 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;
}
// 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
}
#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
// 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
}
// 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; idx<count; idx++) {
_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 = 2 + sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame::PassthroughTelemetryPacket)*count; //subtype + size + 6*count
_telem_type = get_custom_telem_frame_id();
_telem_pending = true;
}
/*
fetch CRSF frame data
if is_tx_active is true then this will be a request for telemetry after receiving an RC frame
*/
bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data, bool is_tx_active)
{
memset(&_telem, 0, sizeof(TelemetryPayload));
// update telemetry tasks if we either lost or regained the transmitter
if (_is_tx_active != is_tx_active) {
if (is_tx_active) {
disable_scheduler_entry(DEVICE_PING);
enable_tx_entries();
} else {
disable_tx_entries();
enable_scheduler_entry(DEVICE_PING);
}
_is_tx_active = is_tx_active;
}
run_wfq_scheduler();
if (!_telem_pending) {
return false;
}
memcpy(data->payload, &_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
Reference in New Issue View Git Blame Copy Permalink