mirror of https://github.com/ArduPilot/ardupilot
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:
parent
4e88adf86e
commit
5bc1b11a80
|
@ -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
|
|
@ -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
|
|
@ -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;
|
||||
|
|
|
@ -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();
|
||||
|
||||
|
|
|
@ -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();
|
||||
}
|
||||
};
|
|
@ -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();
|
||||
};
|
Loading…
Reference in New Issue