mirror of https://github.com/ArduPilot/ardupilot
1263 lines
46 KiB
C++
1263 lines
46 KiB
C++
/*
|
|
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_Common/AP_FWVersion.h>
|
|
#include <AP_OSD/AP_OSD.h>
|
|
#include <AP_Frsky_Telem/AP_Frsky_SPort_Passthrough.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
|
|
#if HAL_CRSF_TELEM_ENABLED
|
|
|
|
// #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;
|
|
|
|
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().find_serial(AP_SerialManager::SerialProtocol_RCIN, 0)
|
|
&& !AP::serialmanager().find_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])) )
|
|
|
|
// CSRF telemetry rate is 150Hz (4ms) max, so these rates must fit
|
|
add_scheduler_entry(50, 100); // heartbeat 10Hz
|
|
add_scheduler_entry(50, 50); // parameters 20Hz (generally not active unless requested by the TX)
|
|
add_scheduler_entry(50, 120); // Attitude and compass 8Hz
|
|
add_scheduler_entry(200, 1000); // VTX parameters 1Hz
|
|
add_scheduler_entry(1300, 500); // battery 2Hz
|
|
add_scheduler_entry(550, 280); // GPS 3Hz
|
|
add_scheduler_entry(550, 500); // flight mode 2Hz
|
|
add_scheduler_entry(5000, 100); // passthrough max 10Hz
|
|
add_scheduler_entry(5000, 500); // status text max 2Hz
|
|
}
|
|
|
|
void AP_CRSF_Telem::setup_custom_telemetry()
|
|
{
|
|
if (_custom_telem.init_done) {
|
|
return;
|
|
}
|
|
|
|
if (!rc().crsf_custom_telemetry()) {
|
|
return;
|
|
}
|
|
|
|
// we need crossfire firmware version
|
|
if (_crsf_version.pending) {
|
|
return;
|
|
}
|
|
|
|
AP_Frsky_SPort_Passthrough* passthrough = AP::frsky_passthrough_telem();
|
|
if (passthrough == nullptr) {
|
|
return;
|
|
}
|
|
|
|
// setup the frsky scheduler for crossfire
|
|
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LAT);
|
|
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::GPS_LON);
|
|
passthrough->disable_scheduler_entry(AP_Frsky_SPort_Passthrough::TEXT);
|
|
passthrough->set_scheduler_entry_min_period(AP_Frsky_SPort_Passthrough::ATTITUDE, 350); // 3Hz
|
|
|
|
// setup the crossfire scheduler for custom telemetry
|
|
set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
|
|
set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
|
|
set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz
|
|
set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
|
|
|
|
_telem_rf_mode = get_rf_mode();
|
|
// setup custom telemetry for current rf_mode
|
|
update_custom_telemetry_rates(_telem_rf_mode);
|
|
|
|
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: custom telem init done, fw %d.%02d", _crsf_version.major, _crsf_version.minor);
|
|
_custom_telem.init_done = true;
|
|
}
|
|
|
|
void AP_CRSF_Telem::update_custom_telemetry_rates(AP_RCProtocol_CRSF::RFMode rf_mode)
|
|
{
|
|
// ignore rf mode changes if we are processing parameter packets
|
|
if (_custom_telem.params_mode_active) {
|
|
return;
|
|
}
|
|
|
|
if (is_high_speed_telemetry(rf_mode)) {
|
|
// custom telemetry for high data rates
|
|
set_scheduler_entry(GPS, 550, 500); // 2.0Hz
|
|
set_scheduler_entry(PASSTHROUGH, 100, 100); // 10Hz
|
|
set_scheduler_entry(STATUS_TEXT, 200, 750); // 1.5Hz
|
|
} else {
|
|
// custom telemetry for low data rates
|
|
set_scheduler_entry(GPS, 550, 1000); // 1.0Hz
|
|
set_scheduler_entry(PASSTHROUGH, 500, 3000); // 0.3Hz
|
|
set_scheduler_entry(STATUS_TEXT, 600, 2000); // 0.5Hz
|
|
}
|
|
}
|
|
|
|
void AP_CRSF_Telem::process_rf_mode_changes()
|
|
{
|
|
const AP_RCProtocol_CRSF::RFMode current_rf_mode = get_rf_mode();
|
|
uint32_t now = AP_HAL::millis();
|
|
// report a change in RF mode or a chnage of more than 10Hz if we haven't done so in the last 5s
|
|
if ((now - _telem_last_report_ms > 5000) &&
|
|
(_telem_rf_mode != current_rf_mode || abs(int16_t(_telem_last_avg_rate) - int16_t(_scheduler.avg_packet_rate)) > 25)) {
|
|
gcs().send_text(MAV_SEVERITY_INFO, "CRSF: RF mode %d, rate is %dHz", (uint8_t)current_rf_mode, _scheduler.avg_packet_rate);
|
|
update_custom_telemetry_rates(current_rf_mode);
|
|
_telem_rf_mode = current_rf_mode;
|
|
_telem_last_avg_rate = _scheduler.avg_packet_rate;
|
|
_telem_last_report_ms = now;
|
|
}
|
|
}
|
|
|
|
// 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))) {
|
|
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM;
|
|
}
|
|
return AP_RCProtocol_CRSF::CRSF_FRAMETYPE_AP_CUSTOM_TELEM_LEGACY;
|
|
}
|
|
|
|
AP_RCProtocol_CRSF::RFMode AP_CRSF_Telem::get_rf_mode() const
|
|
{
|
|
AP_RCProtocol_CRSF* crsf = AP::crsf();
|
|
if (crsf == nullptr) {
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_UNKNOWN;
|
|
}
|
|
|
|
if (!_crsf_version.pending && _crsf_version.use_rf_mode) {
|
|
return crsf->get_link_status().rf_mode;
|
|
} else if (_crsf_version.is_tracer) {
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
|
|
}
|
|
|
|
/*
|
|
Note:
|
|
- rf mode 2 on UARTS with DMA runs @160Hz
|
|
- rf mode 2 on UARTS with no DMA runs @70Hz
|
|
*/
|
|
if (get_avg_packet_rate() < 40U) {
|
|
// no DMA rf mode 1
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
|
|
}
|
|
if (get_avg_packet_rate() > 120U) {
|
|
// DMA rf mode 2
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
|
|
}
|
|
if (get_max_packet_rate() < 120U) {
|
|
// no DMA rf mode 2
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ;
|
|
}
|
|
return AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_50HZ;
|
|
}
|
|
|
|
bool AP_CRSF_Telem::is_high_speed_telemetry(const AP_RCProtocol_CRSF::RFMode rf_mode) const
|
|
{
|
|
return rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_150HZ || rf_mode == AP_RCProtocol_CRSF::RFMode::CRSF_RF_MODE_250HZ;
|
|
}
|
|
|
|
void AP_CRSF_Telem::queue_message(MAV_SEVERITY severity, const char *text)
|
|
{
|
|
// no need to queue status text messages when crossfire
|
|
// custom telemetry is not enabled
|
|
if (!rc().crsf_custom_telemetry()) {
|
|
return;
|
|
}
|
|
AP_RCTelemetry::queue_message(severity, text);
|
|
}
|
|
|
|
void AP_CRSF_Telem::enter_scheduler_params_mode()
|
|
{
|
|
set_scheduler_entry(HEARTBEAT, 50, 100); // heartbeat 10Hz
|
|
set_scheduler_entry(ATTITUDE, 50, 120); // Attitude and compass 8Hz
|
|
set_scheduler_entry(BATTERY, 1300, 500); // battery 2Hz
|
|
set_scheduler_entry(GPS, 550, 280); // GPS 3Hz
|
|
set_scheduler_entry(FLIGHT_MODE, 550, 500); // flight mode 2Hz
|
|
|
|
disable_scheduler_entry(PASSTHROUGH);
|
|
disable_scheduler_entry(STATUS_TEXT);
|
|
}
|
|
|
|
void AP_CRSF_Telem::exit_scheduler_params_mode()
|
|
{
|
|
// setup the crossfire scheduler for custom telemetry
|
|
set_scheduler_entry(BATTERY, 1000, 1000); // 1Hz
|
|
set_scheduler_entry(ATTITUDE, 1000, 1000); // 1Hz
|
|
set_scheduler_entry(FLIGHT_MODE, 1200, 2000); // 0.5Hz
|
|
set_scheduler_entry(HEARTBEAT, 2000, 5000); // 0.2Hz
|
|
|
|
enable_scheduler_entry(PASSTHROUGH);
|
|
enable_scheduler_entry(STATUS_TEXT);
|
|
|
|
update_custom_telemetry_rates(_telem_rf_mode);
|
|
}
|
|
|
|
void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
|
|
{
|
|
uint32_t now_ms = AP_HAL::millis();
|
|
setup_custom_telemetry();
|
|
|
|
/*
|
|
whenever we detect a pending request we configure the scheduler
|
|
to allow faster parameters processing.
|
|
We start a "fast parameter window" that we close after 5sec
|
|
*/
|
|
bool expired = (now_ms - _custom_telem.params_mode_start_ms) > 5000;
|
|
if (!_custom_telem.params_mode_active && _pending_request.frame_type > 0) {
|
|
// fast window start
|
|
_custom_telem.params_mode_start_ms = now_ms;
|
|
_custom_telem.params_mode_active = true;
|
|
enter_scheduler_params_mode();
|
|
} else if (expired && _custom_telem.params_mode_active) {
|
|
// fast window stop
|
|
_custom_telem.params_mode_active = false;
|
|
exit_scheduler_params_mode();
|
|
}
|
|
}
|
|
|
|
// WFQ scheduler
|
|
bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
|
|
{
|
|
process_rf_mode_changes();
|
|
|
|
switch (idx) {
|
|
case PARAMETERS:
|
|
// to get crossfire firmware version we send an RX device ping until we get a response
|
|
// but only if there are no other requests pending
|
|
if (_crsf_version.pending && _pending_request.frame_type == 0) {
|
|
if (_crsf_version.retry_count++ > CRSF_RX_DEVICE_PING_MAX_RETRY) {
|
|
_crsf_version.pending = false;
|
|
_crsf_version.minor = 0;
|
|
_crsf_version.major = 0;
|
|
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: RX device ping failed");
|
|
} else {
|
|
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_CRSF_RECEIVER;
|
|
_pending_request.frame_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO;
|
|
gcs().send_text(MAV_SEVERITY_DEBUG,"CRSF: requesting RX device info");
|
|
}
|
|
}
|
|
return _pending_request.frame_type > 0;
|
|
case VTX_PARAMETERS:
|
|
return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
|
|
case PASSTHROUGH:
|
|
return rc().crsf_custom_telemetry();
|
|
case STATUS_TEXT:
|
|
return rc().crsf_custom_telemetry() && !queue_empty;
|
|
default:
|
|
return _enable_telemetry;
|
|
}
|
|
}
|
|
|
|
// 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
|
|
update_params();
|
|
break;
|
|
case ATTITUDE:
|
|
calc_attitude();
|
|
break;
|
|
case VTX_PARAMETERS: // update various VTX parameters
|
|
update_vtx_params();
|
|
break;
|
|
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)
|
|
get_multi_packet_passthrough_telem_data();
|
|
}
|
|
break;
|
|
case STATUS_TEXT:
|
|
calc_status_text();
|
|
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;
|
|
|
|
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;
|
|
|
|
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;
|
|
|
|
default:
|
|
break;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
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(200);
|
|
break;
|
|
case 2:
|
|
apvtx.set_power_mw(500);
|
|
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;
|
|
}
|
|
|
|
// 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_PING;
|
|
}
|
|
|
|
// 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.is_tracer = true;
|
|
}
|
|
/*
|
|
fw major ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 3rd byte of sw id = 11bytes
|
|
fw minor ver = offset + terminator (8bits) + serial (32bits) + hw id (32bits) + 4th byte of sw id = 12bytes
|
|
*/
|
|
_crsf_version.major = info->payload[offset+11];
|
|
_crsf_version.minor = info->payload[offset+12];
|
|
|
|
// should we use rf_mode reported by link statistics?
|
|
if (!_crsf_version.is_tracer && (_crsf_version.major > 3 || (_crsf_version.major == 3 && _crsf_version.minor >= 72))) {
|
|
_crsf_version.use_rf_mode = true;
|
|
}
|
|
|
|
_crsf_version.pending = false;
|
|
}
|
|
|
|
void AP_CRSF_Telem::process_param_read_frame(ParameterSettingsReadFrame* read_frame)
|
|
{
|
|
//debug("process_param_read_frame: %d -> %d for %d[%d]", read_frame->origin, read_frame->destination,
|
|
// read_frame->param_number, 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::update_params()
|
|
{
|
|
// handle general parameter requests
|
|
switch (_pending_request.frame_type) {
|
|
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING:
|
|
calc_device_info();
|
|
break;
|
|
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAMETER_READ:
|
|
calc_parameter();
|
|
break;
|
|
case AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_INFO:
|
|
calc_device_ping();
|
|
break;
|
|
default:
|
|
break;
|
|
}
|
|
}
|
|
|
|
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;
|
|
_vtx_power_change_pending = 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;
|
|
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;
|
|
} else 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;
|
|
}
|
|
}
|
|
_telem_pending = true;
|
|
// calculate command crc
|
|
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;
|
|
}
|
|
}
|
|
|
|
// 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;
|
|
}
|
|
|
|
_telem.bcast.battery.remaining = _battery.capacity_remaining_pct(0);
|
|
|
|
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.roll) * 10000.0f), -INT_PI, INT_PI));
|
|
_telem.bcast.attitude.pitch_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.pitch) * 10000.0f), -INT_PI, INT_PI));
|
|
_telem.bcast.attitude.yaw_angle = htobe16(constrain_int16(roundf(wrap_PI(_ahrs.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) {
|
|
hal.util->snprintf(_telem.bcast.flightmode.flight_mode, 16, "%s", notify->get_flight_mode_str());
|
|
|
|
_telem_size = sizeof(AP_CRSF_Telem::FlightModeFrame);
|
|
_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_string);
|
|
strncpy((char*)_telem.ext.info.payload, fwver.fw_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() {
|
|
_telem.ext.ping.destination = _pending_request.destination;
|
|
_telem.ext.ping.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
|
|
_telem_size = 2;
|
|
_telem_type = AP_RCProtocol_CRSF::CRSF_FRAMETYPE_PARAM_DEVICE_PING;
|
|
|
|
_pending_request.destination = AP_RCProtocol_CRSF::CRSF_ADDRESS_BROADCAST;
|
|
_pending_request.frame_type = 0;
|
|
|
|
_telem_pending = true;
|
|
}
|
|
|
|
// return parameter information
|
|
void AP_CRSF_Telem::calc_parameter() {
|
|
#if OSD_PARAM_ENABLED
|
|
AP_OSD* osd = AP::osd();
|
|
|
|
if (osd == nullptr) {
|
|
return;
|
|
}
|
|
|
|
_telem.ext.param_entry.header.destination = _param_request.origin;
|
|
_telem.ext.param_entry.header.origin = AP_RCProtocol_CRSF::CRSF_ADDRESS_FLIGHT_CONTROLLER;
|
|
|
|
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
|
|
size_t idx = 0;
|
|
_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 (!is_high_speed_telemetry(_telem_rf_mode)) {
|
|
// keep only warning/error/critical/alert/emergency status text messages
|
|
bool got_message = false;
|
|
while (_statustext.queue.pop(_statustext.next)) {
|
|
if (_statustext.next.severity <= MAV_SEVERITY_WARNING) {
|
|
got_message = true;
|
|
break;
|
|
}
|
|
}
|
|
if (!got_message) {
|
|
return;
|
|
}
|
|
} else if (!_statustext.queue.pop(_statustext.next)) {
|
|
return;
|
|
}
|
|
_statustext.available = true;
|
|
}
|
|
|
|
_telem_type = get_custom_telem_frame_id();
|
|
_telem.bcast.custom_telem.status_text.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_STATUS_TEXT;
|
|
_telem.bcast.custom_telem.status_text.severity = _statustext.next.severity;
|
|
strncpy_noterm(_telem.bcast.custom_telem.status_text.text, _statustext.next.text, PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE);
|
|
// add a potentially missing terminator
|
|
_telem.bcast.custom_telem.status_text.text[PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE-1] = 0;
|
|
_telem_size = 2 + PASSTHROUGH_STATUS_TEXT_FRAME_MAX_SIZE; // sub_type(1) + severity(1) + text(50)
|
|
_telem_pending = true;
|
|
_statustext.available = false;
|
|
}
|
|
|
|
/*
|
|
Get 1 packet of passthrough telemetry data
|
|
*/
|
|
void AP_CRSF_Telem::get_single_packet_passthrough_telem_data()
|
|
{
|
|
_telem_pending = false;
|
|
uint8_t packet_count;
|
|
AP_Frsky_SPort::sport_packet_t packet;
|
|
if (!AP_Frsky_Telem::get_telem_data(&packet, packet_count, 1)) {
|
|
return;
|
|
}
|
|
_telem.bcast.custom_telem.single_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_SINGLE_PACKET_PASSTHROUGH;
|
|
_telem.bcast.custom_telem.single_packet_passthrough.appid = packet.appid;
|
|
_telem.bcast.custom_telem.single_packet_passthrough.data = packet.data;
|
|
_telem_size = sizeof(AP_CRSF_Telem::PassthroughSinglePacketFrame);
|
|
_telem_type = get_custom_telem_frame_id();
|
|
_telem_pending = true;
|
|
}
|
|
|
|
/*
|
|
Get up to PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packets of passthrough telemetry data (for slow links)
|
|
Note: we have 2 distinct frame types (single packet vs multi packet) because
|
|
whenever possible we use smaller frames for they have a higher "chance"
|
|
of being transmitted by the crossfire RX scheduler.
|
|
*/
|
|
void AP_CRSF_Telem::get_multi_packet_passthrough_telem_data()
|
|
{
|
|
_telem_pending = false;
|
|
uint8_t count = 0;
|
|
AP_Frsky_SPort::sport_packet_t buffer[PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE] {};
|
|
// we request a PASSTHROUGH_MULTI_PACKET_FRAME_MAX_SIZE packet array, i.e. 9 packets
|
|
if (!AP_Frsky_Telem::get_telem_data(buffer, count, ARRAY_SIZE(buffer))) {
|
|
return;
|
|
}
|
|
_telem.bcast.custom_telem.multi_packet_passthrough.sub_type = AP_RCProtocol_CRSF::CustomTelemSubTypeID::CRSF_AP_CUSTOM_TELEM_MULTI_PACKET_PASSTHROUGH;
|
|
for (uint8_t idx=0; idx<count; idx++) {
|
|
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].appid = buffer[idx].appid;
|
|
_telem.bcast.custom_telem.multi_packet_passthrough.frames[idx].data = buffer[idx].data;
|
|
}
|
|
_telem.bcast.custom_telem.multi_packet_passthrough.size = count;
|
|
_telem_size = sizeof(AP_CRSF_Telem::PassthroughMultiPacketFrame);
|
|
_telem_type = get_custom_telem_frame_id();
|
|
_telem_pending = true;
|
|
}
|
|
|
|
/*
|
|
fetch CRSF frame data
|
|
*/
|
|
bool AP_CRSF_Telem::_get_telem_data(AP_RCProtocol_CRSF::Frame* data)
|
|
{
|
|
memset(&_telem, 0, sizeof(TelemetryPayload));
|
|
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)
|
|
{
|
|
if (!get_singleton()) {
|
|
return false;
|
|
}
|
|
return singleton->_get_telem_data(data);
|
|
}
|
|
|
|
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
|