AP_RCTelemetry: CRSF support

battery and heartbeat telemetry
parse VTX packets and VTX telemetry
provide GPS, Battery, Attitude, FlightMode telemetry
issue parameter updates at scheduled rate
support setting power levels with dbm
support standalone mode
set defaults from incoming VTX packets
output configured VTX settings
This commit is contained in:
Andy Piper 2020-04-18 11:54:34 +01:00 committed by Andrew Tridgell
parent 4e88adf86e
commit 5bc1b11a80
6 changed files with 1044 additions and 4 deletions

View File

@ -0,0 +1,463 @@
/*
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.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 <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, 120); // Attitude and compass 8Hz
add_scheduler_entry(200, 1000); // parameters 1Hz
add_scheduler_entry(1300, 500); // battery 2Hz
add_scheduler_entry(550, 280); // GPS 3Hz
add_scheduler_entry(550, 500); // flight mode 2Hz
}
void AP_CRSF_Telem::adjust_packet_weight(bool queue_empty)
{
}
// WFQ scheduler
bool AP_CRSF_Telem::is_packet_ready(uint8_t idx, bool queue_empty)
{
switch (idx) {
case PARAMETERS:
return AP::vtx().have_params_changed() ||_vtx_power_change_pending || _vtx_freq_change_pending || _vtx_options_change_pending;
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 ATTITUDE:
calc_attitude();
break;
case PARAMETERS: // update various parameters
update_params();
break;
case BATTERY: // BATTERY
calc_battery();
break;
case GPS: // GPS
calc_gps();
break;
case FLIGHT_MODE: // GPS
calc_flight_mode();
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 OS
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;
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();
apvtx.set_enabled(vtx->is_vtx_available);
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() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} else {
apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
}
// make sure the configured values now reflect reality
apvtx.set_defaults();
_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();
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() | uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
} else {
apvtx.set_options(apvtx.get_options() & ~uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
}
// make sure the configured values now reflect reality
apvtx.set_defaults();
_vtx_power_change_pending = _vtx_freq_change_pending = _vtx_options_change_pending = false;
}
// process any changed settings and schedule for transmission
void AP_CRSF_Telem::update()
{
}
void AP_CRSF_Telem::update_params()
{
AP_VideoTX& vtx = AP::vtx();
_vtx_freq_change_pending = vtx.update_band() || vtx.update_channel() || _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) {
debug("update_params(): freq %d, chan: %d->%d, band: %d->%d, pwr: %d->%d",
vtx.get_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());
_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;
// make the desired frequency match the desired band and channel
if (_vtx_freq_change_pending) {
vtx.set_frequency_mhz(AP_VideoTX::get_frequency_mhz(vtx.get_configured_band(), vtx.get_configured_channel()));
}
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);
_telem.ext.command.payload[1] = 0;
} 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);
}
_telem.ext.command.payload[1] = 1;
} 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);
}
_telem.ext.command.payload[1] = 2;
} else {
vtx.set_configured_power_mw(800);
_telem.ext.command.payload[1] = 3;
}
_vtx_dbm_update = true;
} else if (_vtx_options_change_pending) {
_telem.ext.command.payload[0] = AP_RCProtocol_CRSF::CRSF_COMMAND_VTX_PITMODE;
_telem.ext.command.payload[1] = vtx.get_configured_options() & uint16_t(AP_VideoTX::VideoOptions::VTX_PITMODE);
}
_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);
int32_t capacity = _battery.pack_capacity_mah(0);
_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(loc.alt + 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());
_telem.bcast.attitude.roll_angle = htobe16(roundf(_ahrs.roll * 10000.0f));
_telem.bcast.attitude.pitch_angle = htobe16(roundf(_ahrs.pitch * 10000.0f));
_telem.bcast.attitude.yaw_angle = htobe16(roundf(_ahrs.yaw * 10000.0f));
_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;
}
}
/*
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

View File

@ -0,0 +1,212 @@
/*
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/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#ifndef HAL_CRSF_TELEM_ENABLED
#define HAL_CRSF_TELEM_ENABLED !HAL_MINIMIZE_FEATURES
#endif
#if HAL_CRSF_TELEM_ENABLED
#include <AP_Notify/AP_Notify.h>
#include <AP_SerialManager/AP_SerialManager.h>
#include <AP_HAL/utility/RingBuffer.h>
#include <AP_RCProtocol/AP_RCProtocol_CRSF.h>
#include "AP_RCTelemetry.h"
#include <AP_HAL/utility/sparse-endian.h>
class AP_CRSF_Telem : public AP_RCTelemetry {
public:
AP_CRSF_Telem();
~AP_CRSF_Telem() override;
/* Do not allow copies */
AP_CRSF_Telem(const AP_CRSF_Telem &other) = delete;
AP_CRSF_Telem &operator=(const AP_CRSF_Telem&) = delete;
// init - perform required initialisation
virtual bool init() override;
static AP_CRSF_Telem *get_singleton(void);
// Broadcast frame definitions courtesy of TBS
struct GPSFrame { // curious fact, calling this GPS makes sizeof(GPS) return 1!
int32_t latitude; // ( degree / 10`000`000 )
int32_t longitude; // (degree / 10`000`000 )
uint16_t groundspeed; // ( km/h / 100 )
uint16_t gps_heading; // ( degree / 100 )
uint16_t altitude; // ( meter - 1000m offset )
uint8_t satellites; // in use ( counter )
} PACKED;
struct HeartbeatFrame {
uint8_t origin; // Device addres
};
struct BatteryFrame {
uint16_t voltage; // ( mV * 100 )
uint16_t current; // ( mA * 100 )
uint8_t capacity[3]; // ( mAh )
uint8_t remaining; // ( percent )
} PACKED;
struct VTXFrame {
#if __BYTE_ORDER != __LITTLE_ENDIAN
#error "Only supported on little-endian architectures"
#endif
uint8_t origin; // address
// status
uint8_t is_in_pitmode : 1;
uint8_t is_in_user_frequency_mode : 1;
uint8_t unused : 2;
uint8_t is_vtx_available : 1;
uint8_t smart_audio_ver : 3; // SmartAudio_V1 = 0, SmartAudio_V2 = 1
// band / channel
uint8_t channel : 3; // 1x-8x
uint8_t band : 5; // A, B, E, AirWave, Race
uint16_t user_frequency;
uint8_t power : 4; // 25mW = 0, 200mW = 1, 500mW = 2, 800mW = 3
uint8_t pitmode : 4; // off = 0, In_Band = 1, Out_Band = 2;
} PACKED;
struct VTXTelemetryFrame {
uint8_t origin; // address
uint8_t power; // power in dBm
uint16_t frequency; // frequency in Mhz
bool pitmode; // disable 0, enable 1
} PACKED;
struct LinkStatisticsFrame {
uint8_t uplink_rssi_ant1; // ( dBm * -1 )
uint8_t uplink_rssi_ant2; // ( dBm * -1 )
uint8_t uplink_status; // Package success rate / Link quality ( % )
int8_t uplink_snr; // ( db )
uint8_t active_antenna; // Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
uint8_t rf_mode; // ( enum 4fps = 0 , 50fps, 150hz)
uint8_t uplink_tx_power; // ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
uint8_t downlink_rssi; // ( dBm * -1 )
uint8_t downlink_status; // Downlink package success rate / Link quality ( % ) ● int8_t Downlink SNR ( db )
} PACKED;
struct AttitudeFrame {
int16_t pitch_angle; // ( rad / 10000 )
int16_t roll_angle; // ( rad / 10000 )
int16_t yaw_angle; // ( rad / 10000 )
} PACKED;
struct FlightModeFrame {
char flight_mode[16]; // ( Null-terminated string )
} PACKED;
// CRSF_FRAMETYPE_COMMAND
struct CommandFrame {
uint8_t destination;
uint8_t origin;
uint8_t command_id;
uint8_t payload[9]; // 8 maximum for LED command + crc8
} PACKED;
// CRSF_FRAMETYPE_PARAM_DEVICE_PING
struct ParameterPingFrame {
uint8_t destination;
uint8_t origin;
} PACKED;
union BroadcastFrame {
GPSFrame gps;
HeartbeatFrame heartbeat;
BatteryFrame battery;
VTXFrame vtx;
LinkStatisticsFrame link;
AttitudeFrame attitude;
FlightModeFrame flightmode;
} PACKED;
union ExtendedFrame {
CommandFrame command;
ParameterPingFrame ping;
} PACKED;
union TelemetryPayload {
BroadcastFrame bcast;
ExtendedFrame ext;
} PACKED;
// Process a frame from the CRSF protocol decoder
static bool process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
// process any changed settings and schedule for transmission
void update();
// get next telemetry data for external consumers of SPort data
static bool get_telem_data(AP_RCProtocol_CRSF::Frame* frame);
private:
enum SensorType {
HEARTBEAT,
ATTITUDE,
PARAMETERS,
BATTERY,
GPS,
FLIGHT_MODE,
NUM_SENSORS
};
// passthrough WFQ scheduler
bool is_packet_ready(uint8_t idx, bool queue_empty) override;
void process_packet(uint8_t idx) override;
void adjust_packet_weight(bool queue_empty) override;
void calc_parameter_ping();
void calc_heartbeat();
void calc_battery();
void calc_gps();
void calc_attitude();
void calc_flight_mode();
void update_params();
void process_vtx_frame(VTXFrame* vtx);
void process_vtx_telem_frame(VTXTelemetryFrame* vtx);
// setup ready for passthrough operation
void setup_wfq_scheduler(void) override;
// get next telemetry data for external consumers
bool _get_telem_data(AP_RCProtocol_CRSF::Frame* data);
bool _process_frame(AP_RCProtocol_CRSF::FrameType frame_type, void* data);
TelemetryPayload _telem;
uint8_t _telem_size;
uint8_t _telem_type;
bool _telem_pending;
bool _enable_telemetry;
// vtx state
bool _vtx_freq_update; // update using the frequency method or not
bool _vtx_dbm_update; // update using the dbm method or not
bool _vtx_freq_change_pending; // a vtx command has been issued but not confirmed by a vtx broadcast frame
bool _vtx_power_change_pending;
bool _vtx_options_change_pending;
static AP_CRSF_Telem *singleton;
};
namespace AP {
AP_CRSF_Telem *crsf_telem();
};
#endif

View File

@ -89,7 +89,7 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
update_avg_packet_rate();
uint32_t now = AP_HAL::millis();
uint8_t max_delay_idx = 0;
int8_t max_delay_idx = -1;
float max_delay = 0;
float delay = 0;
@ -125,6 +125,10 @@ void AP_RCTelemetry::run_wfq_scheduler(void)
}
}
}
if (max_delay_idx < 0) { // nothing was ready
return;
}
now = AP_HAL::millis();
#ifdef TELEM_DEBUG
_scheduler.packet_rate[max_delay_idx] = (_scheduler.packet_rate[max_delay_idx] + 1000 / (now - _scheduler.packet_timer[max_delay_idx])) / 2;

View File

@ -84,10 +84,10 @@ private:
// passthrough WFQ scheduler
virtual void setup_wfq_scheduler() = 0;
virtual bool get_next_msg_chunk(void) = 0;
virtual bool is_packet_ready(uint8_t idx, bool queue_empty) = 0;
virtual bool get_next_msg_chunk(void) { return false; }
virtual bool is_packet_ready(uint8_t idx, bool queue_empty) { return true; }
virtual void process_packet(uint8_t idx) = 0;
virtual void adjust_packet_weight(bool queue_empty) = 0;
virtual void adjust_packet_weight(bool queue_empty) {};
void update_avg_packet_rate();

View File

@ -0,0 +1,233 @@
/*
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_VideoTX.h"
#include "AP_CRSF_Telem.h"
#include <GCS_MAVLink/GCS.h>
extern const AP_HAL::HAL& hal;
AP_VideoTX *AP_VideoTX::singleton;
const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
// @Param: ENABLE
// @DisplayName: Is the Video Transmitter enabled or not
// @Description: Toggles the Video Transmitter on and off
// @Values: 0:Disable,1:Enable
AP_GROUPINFO_FLAGS("ENABLE", 1, AP_VideoTX, _enabled, 0, AP_PARAM_FLAG_ENABLE),
// @Param: POWER
// @DisplayName: Video Transmitter Power Level
// @Description: Video Transmitter Power Level. Different VTXs support different power levels, the power level chosen will be rounded down to the nearest supported power level
// @Range: 1 1000
AP_GROUPINFO("POWER", 2, AP_VideoTX, _power_mw, 0),
// @Param: CHANNEL
// @DisplayName: Video Transmitter Channel
// @Description: Video Transmitter Channel
// @User: Standard
// @Range: 0 7
AP_GROUPINFO("CHANNEL", 3, AP_VideoTX, _channel, 0),
// @Param: BAND
// @DisplayName: Video Transmitter Band
// @Description: Video Transmitter Band
// @User: Standard
// @Values: 0:Band A,1:Band B,2:Band E,3:Airwave,4:RaceBand,5:Low RaceBand
AP_GROUPINFO("BAND", 4, AP_VideoTX, _band, 0),
// @Param: FREQ
// @DisplayName: Video Transmitter Frequency
// @Description: Video Transmitter Frequency. The frequency is derived from the setting of BAND and CHANNEL
// @User: Standard
// @ReadOnly: True
// @Range: 5000 6000
AP_GROUPINFO("FREQ", 5, AP_VideoTX, _frequency_mhz, 0),
// @Param: OPTIONS
// @DisplayName: Video Transmitter Options
// @Description: Video Transmitter Options.
// @User: Advanced
// @Bitmask: 0:Pitmode
AP_GROUPINFO("OPTIONS", 6, AP_VideoTX, _options, 0),
AP_GROUPEND
};
extern const AP_HAL::HAL& hal;
const uint16_t AP_VideoTX::VIDEO_CHANNELS[AP_VideoTX::MAX_BANDS][VTX_MAX_CHANNELS] =
{
{ 5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725}, /* Band A */
{ 5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866}, /* Band B */
{ 5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945}, /* Band E */
{ 5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880}, /* Airwave */
{ 5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917}, /* Race */
{ 5621, 5584, 5547, 5510, 5473, 5436, 5399, 5362} /* LO Race */
};
AP_VideoTX::AP_VideoTX()
{
if (singleton) {
AP_HAL::panic("Too many VTXs");
return;
}
singleton = this;
AP_Param::setup_object_defaults(this, var_info);
}
AP_VideoTX::~AP_VideoTX(void)
{
singleton = nullptr;
}
bool AP_VideoTX::init(void)
{
if (_initialized) {
return false;
}
_current_power = _power_mw;
_current_band = _band;
_current_channel = _channel;
_current_options = _options;
_current_enabled = _enabled;
_initialized = true;
return true;
}
bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel)
{
for (uint8_t i = 0; i < AP_VideoTX::MAX_BANDS; i++) {
for (uint8_t j = 0; j < VTX_MAX_CHANNELS; j++) {
if (VIDEO_CHANNELS[i][j] == freq) {
band = VideoBand(i);
channel = j;
return true;
}
}
}
return false;
}
// set the current power
void AP_VideoTX::set_configured_power_mw(uint16_t power) {
_power_mw.set_and_save(power);
}
// set the power in dbm, rounding appropriately
void AP_VideoTX::set_power_dbm(uint8_t power) {
switch (power) {
case 14:
_current_power = 25;
break;
case 20:
_current_power = 100;
break;
case 23:
_current_power = 200;
break;
case 26:
_current_power = 400;
break;
case 27:
_current_power = 500;
break;
case 29:
_current_power = 800;
break;
default:
_current_power = uint16_t(roundf(powf(10, power / 10.0f)));
break;
}
}
// get the power in dbm, rounding appropriately
uint8_t AP_VideoTX::get_configured_power_dbm() const {
switch (_power_mw.get()) {
case 25: return 14;
case 100: return 20;
case 200: return 23;
case 400: return 26;
case 500: return 27;
case 800: return 29;
default:
return uint8_t(roundf(10.0f * log10f(_power_mw)));
}
}
// set the current channel
void AP_VideoTX::set_enabled(bool enabled) {
_current_enabled = enabled;
if (!_enabled.configured_in_storage()) {
_enabled.set_and_save(enabled);
}
}
// peiodic update
void AP_VideoTX::update(void)
{
#if HAL_CRSF_TELEM_ENABLED
AP_CRSF_Telem* crsf = AP::crsf_telem();
if (crsf != nullptr) {
crsf->update();
}
#endif
}
bool AP_VideoTX::have_params_changed() const
{
return update_power()
|| update_band()
|| update_channel()
|| update_options();
}
// set the current configured values if not currently set in storage
// this is necessary so that the current settings can be seen
void AP_VideoTX::set_defaults()
{
if (_defaults_set) {
return;
}
if (!_options.configured()) {
_options.set_and_save(_current_options);
}
if (!_channel.configured()) {
_channel.set_and_save(_current_channel);
}
if (!_band.configured()) {
_band.set_and_save(_current_band);
}
if (!_power_mw.configured()) {
_power_mw.set_and_save(_current_power);
}
_defaults_set = true;
// Output a friendly message so the user knows the VTX has been detected
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "VTX: Freq: %dMHz, Power: %dmw, Band: %d, Chan: %d",
_frequency_mhz.get(), _power_mw.get(), _band.get() + 1, _channel.get() + 1);
}
namespace AP {
AP_VideoTX& vtx() {
return *AP_VideoTX::get_singleton();
}
};

View File

@ -0,0 +1,128 @@
/*
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/>.
*/
#pragma once
#include <AP_HAL/AP_HAL.h>
#include <AP_Param/AP_Param.h>
#define VTX_MAX_CHANNELS 8
class AP_VideoTX {
public:
AP_VideoTX();
~AP_VideoTX();
/* Do not allow copies */
AP_VideoTX(const AP_VideoTX &other) = delete;
AP_VideoTX &operator=(const AP_VideoTX&) = delete;
// init - perform required initialisation
bool init();
// run any required updates
void update();
static AP_VideoTX *get_singleton(void) {
return singleton;
}
static const struct AP_Param::GroupInfo var_info[];
enum class VideoOptions {
VTX_PITMODE = 1 << 0
};
enum VideoBand {
VTX_BAND_A,
VTX_BAND_B,
VTX_BAND_E,
VTX_BAND_FATSHARK,
VTX_BAND_RACEBAND,
VTX_BAND_LOW_RACEBAND,
MAX_BANDS
};
static const uint16_t VIDEO_CHANNELS[MAX_BANDS][VTX_MAX_CHANNELS];
static uint16_t get_frequency_mhz(uint8_t band, uint8_t channel) { return VIDEO_CHANNELS[band][channel]; }
static bool get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& channel);
void set_frequency_mhz(uint16_t freq) { _frequency_mhz.set_and_save(freq); }
uint16_t get_frequency_mhz() const { return _frequency_mhz; }
// get / set power level
void set_power_mw(uint16_t power) { _current_power = power; }
void set_power_dbm(uint8_t power);
void set_configured_power_mw(uint16_t power);
uint16_t get_configured_power_mw() const { return _power_mw; }
uint16_t get_power_mw() const { return _current_power; }
uint8_t get_configured_power_dbm() const;
bool update_power() const { return _defaults_set && _power_mw != _current_power; }
// get / set the frequency band
void set_band(uint8_t band) { _current_band = band; }
uint8_t get_configured_band() const { return _band; }
uint8_t get_band() const { return _current_band; }
bool update_band() const { return _defaults_set && _band != _current_band; }
// get / set the frequency channel
void set_channel(uint8_t channel) { _current_channel = channel; }
uint8_t get_configured_channel() const { return _channel; }
uint8_t get_channel() const { return _current_channel; }
bool update_channel() const { return _defaults_set && _channel != _current_channel; }
// get / set vtx option
void set_options(uint8_t options) { _current_options = options; }
uint8_t get_configured_options() const { return _options; }
uint8_t get_options() const { return _current_options; }
bool update_options() const { return _defaults_set && _options != _current_options; }
// get / set whether the vtx is enabled
void set_enabled(bool enabled);
bool get_enabled() const { return _enabled; }
bool update_enabled() const { return _defaults_set && _enabled != _current_enabled; }
// have the parameters been updated
bool have_params_changed() const;
// set configured defaults from current settings
void set_defaults();
static AP_VideoTX *singleton;
private:
// channel frequency
AP_Int16 _frequency_mhz;
// power output in mw
AP_Int16 _power_mw;
uint16_t _current_power;
// frequency band
AP_Int8 _band;
uint16_t _current_band;
// frequency channel
AP_Int8 _channel;
uint8_t _current_channel;
// vtx options
AP_Int8 _options;
uint8_t _current_options;
AP_Int8 _enabled;
bool _current_enabled;
bool _initialized;
// when defaults have been configured
bool _defaults_set;
};
namespace AP {
AP_VideoTX& vtx();
};