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

AP_VideoTX: add lookup tables for VTX power settings 

correct settings when power set is received
add support for capturing all supported power levels
learn power levels in SmartAudio 2.1
add better support for VTX power levels
don't set power to 0 if in pitmode
add option for iNav compatibility
support non-conforming SmartAudio implementations
re-enable pitmode on SmartAudio 2.0
add support for "blind" VTX setting
This commit is contained in:
Andy Piper 2021-12-12 16:35:10 +00:00 committed by Peter Barker
parent a653b5e8d1
commit 0658f06030
4 changed files with 348 additions and 222 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

86
libraries/AP_VideoTX/AP_SmartAudio.cpp
View File

@ -51,7 +51,7 @@ bool AP_SmartAudio::init()
_port = AP::serialmanager().find_serial(AP_SerialManager::SerialProtocol_SmartAudio, 0);
if (_port!=nullptr) {
_port->configure_parity(0);
_port->set_stop_bits(2);
_port->set_stop_bits(AP::vtx().has_option(AP_VideoTX::VideoOptions::VTX_SA_ONE_STOP_BIT) ? 1 : 2);
_port->set_flow_control(AP_HAL::UARTDriver::FLOW_CONTROL_DISABLE);
_port->set_options((_port->get_options() & ~AP_HAL::UARTDriver::OPTION_RXINV)
| AP_HAL::UARTDriver::OPTION_HDPLEX | AP_HAL::UARTDriver::OPTION_PULLDOWN_TX | AP_HAL::UARTDriver::OPTION_PULLDOWN_RX);
@ -123,9 +123,13 @@ void AP_SmartAudio::loop()
// and the initial response is 40ms long so we should wait at least 140ms before giving up
if (now - _last_request_sent_ms < 200 && _is_waiting_response) {
// setup sheduler delay to 50 ms again after response processes
// setup scheduler delay to 50 ms again after response processes
if (!read_response(_response_buffer)) {
hal.scheduler->delay(10);
} else {
// successful response, wait another 100ms to give the VTX a chance to recover
// before sending another command. This is needed on the Atlatl v1.
hal.scheduler->delay(100);
}
} else if (_is_waiting_response) { // timeout
@ -134,13 +138,14 @@ void AP_SmartAudio::loop()
_port->discard_input();
_inline_buffer_length = 0;
_is_waiting_response = false;
} else {
debug("response timeout");
} else if (_initialised) {
if (AP::vtx().have_params_changed() ||_vtx_power_change_pending
|| _vtx_freq_change_pending || _vtx_options_change_pending) {
update_vtx_params();
set_configuration_pending(true);
vtx.set_configuration_finished(false);
// we've tried to udpate something, re-request the settings so that they
// we've tried to update something, re-request the settings so that they
// are reflected correctly
request_settings();
} else if (is_configuration_pending()) {
@ -176,22 +181,20 @@ void AP_SmartAudio::update_vtx_params()
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());
vtx.get_options() & 0xF, vtx.get_configured_options() & 0xF);
uint8_t opts = vtx.get_configured_options();
uint8_t pitModeRunning = (vtx.get_options() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE));
uint8_t pitMode = opts & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE);
uint8_t pitMode = vtx.get_configured_pitmode();
uint8_t mode;
// check if we are turning pitmode on or off, but only on SA 2.1 as older versions
// appear not to work properly
if (pitMode != pitModeRunning && _protocol_version >= SMARTAUDIO_SPEC_PROTOCOL_v21) {
if (pitModeRunning) {
// check if we are turning pitmode on or off, but only on SA 2.0+
if (pitMode != vtx.get_pitmode() && _protocol_version >= SMARTAUDIO_SPEC_PROTOCOL_v2) {
if (vtx.get_pitmode()) {
debug("Turning OFF pitmode");
// turn it off
mode = 0x04 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
} else {
debug("Turning ON pitmode");
// turn it on
// turn it on (in range pitmode flag)
mode = 0x01 | ((opts & uint8_t(AP_VideoTX::VideoOptions::VTX_UNLOCKED)) << 2);
}
} else {
@ -218,7 +221,7 @@ void AP_SmartAudio::update_vtx_params()
set_channel(vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel());
}
} else if (_vtx_power_change_pending) {
debug("update power");
debug("update power (ver %u)", _protocol_version);
switch (_protocol_version) {
case SMARTAUDIO_SPEC_PROTOCOL_v21:
set_power(vtx.get_configured_power_dbm() | 0x80);
@ -264,7 +267,7 @@ void AP_SmartAudio::send_request(const Frame& requestFrame, uint8_t size)
_packets_sent++;
#ifdef SA_DEBUG
print_bytes_to_hex_string("send_request():", request, size,0);
print_bytes_to_hex_string("send_request():", request, size);
#endif
}
@ -316,6 +319,10 @@ bool AP_SmartAudio::read_response(uint8_t *response_buffer)
FrameHeader* header = (FrameHeader*)response_buffer;
incoming_bytes_count -= response_header_size;
// implementations that ignore the CRC also appear to not account for it in the frame length
if (ignore_crc()) {
header->length++;
}
_packet_size = header->length;
}
@ -339,7 +346,7 @@ bool AP_SmartAudio::read_response(uint8_t *response_buffer)
}
#ifdef SA_DEBUG
print_bytes_to_hex_string("read_response():", response_buffer, incoming_bytes_count,(_inline_buffer_length-incoming_bytes_count)>=0?(_inline_buffer_length-incoming_bytes_count):0);
print_bytes_to_hex_string("read_response():", response_buffer, _inline_buffer_length);
#endif
_is_waiting_response = false;
@ -357,6 +364,8 @@ bool AP_SmartAudio::read_response(uint8_t *response_buffer)
void AP_SmartAudio::push_command_only_frame(uint8_t cmd_id)
{
Packet command;
// according to the spec the length should include the CRC, but no implementation appears to
// do this
command.frame.header.init(cmd_id, 0);
command.frame_size = SMARTAUDIO_COMMAND_FRAME_SIZE;
command.frame.payload[0] = crc8_dvb_s2_update(0, &command.frame, SMARTAUDIO_COMMAND_FRAME_SIZE - 1);
@ -451,31 +460,32 @@ void AP_SmartAudio::unpack_frequency(AP_SmartAudio::Settings *settings, const ui
}
}
// SmartAudio v1/v2
void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsResponseFrame *frame)
{
settings->channel = frame->channel % VTX_MAX_CHANNELS;
settings->band = frame->channel / VTX_MAX_CHANNELS;
settings->power = frame->power;
settings->mode = frame->operationMode;
settings->num_power_levels = 0;
unpack_frequency(settings, be16toh(frame->frequency));
}
// SmartAudio v2.1
void AP_SmartAudio::unpack_settings(Settings *settings, const SettingsExtendedResponseFrame *frame)
{
settings->channel = frame->channel % VTX_MAX_CHANNELS;
settings->band = frame->channel / VTX_MAX_CHANNELS;
settings->power = frame->power;
unpack_settings(settings, &frame->settings);
settings->power_in_dbm = frame->power_dbm;
settings->mode = frame->operationMode;
unpack_frequency(settings, be16toh(frame->frequency));
settings->num_power_levels = frame->num_power_levels + 1;
memcpy(settings->power_levels, frame->power_levels, frame->num_power_levels + 1);
}
#ifdef SA_DEBUG
void AP_SmartAudio::print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t len,uint8_t offset)
void AP_SmartAudio::print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t len)
{
hal.console->printf("SA: %s ", msg);
for (uint8_t i = 0; i < len; i++) {
hal.console->printf("0x%02X ", buf[i+offset]);
hal.console->printf("0x%02X ", buf[i]);
}
hal.console->printf("\n");
}
@ -485,11 +495,11 @@ void AP_SmartAudio::print_settings(const Settings* settings)
{
debug("SETTINGS: VER: %u, MD: '%c%c%c%c%c', CH: %u, PWR: %u, DBM: %u FREQ: %u, BND: %u",
settings->version,
(settings->mode & 0x10) ? 'U' : 'L',
(settings->mode & 0x8) ? 'O' : ' ',
(settings->mode & 0x4) ? 'I' : ' ',
(settings->mode & 0x2) ? 'P' : ' ',
(settings->mode & 0x1) ? 'F' : 'C',
(settings->mode & 0x10) ? 'U' : 'L',// (L)ocked or (U)nlocked
(settings->mode & 0x8) ? 'O' : ' ', // (O)ut-range pitmode
(settings->mode & 0x4) ? 'I' : ' ', // (I)n-range pitmode
(settings->mode & 0x2) ? 'P' : ' ', // (P)itmode running
(settings->mode & 0x1) ? 'F' : 'C', // Set (F)requency or (C)hannel
settings->channel, settings->power, settings->power_in_dbm, settings->frequency, settings->band);
}
@ -501,10 +511,13 @@ void AP_SmartAudio::update_vtx_settings(const Settings& settings)
vtx.set_frequency_mhz(settings.frequency);
vtx.set_band(settings.band);
vtx.set_channel(settings.channel);
// SA21 sends us a complete packet with the supported power levels
if (settings.version == SMARTAUDIO_SPEC_PROTOCOL_v21) {
vtx.set_power_dbm(settings.power_in_dbm);
// learn them all
vtx.update_all_power_dbm(settings.num_power_levels, settings.power_levels);
} else {
vtx.set_power_level(settings.power);
vtx.set_power_level(settings.power, AP_VideoTX::PowerActive::Active);
}
// it seems like the spec is wrong, on a unify pro32 this setting is inverted
_vtx_use_set_freq = !(settings.mode & 1);
@ -530,9 +543,10 @@ bool AP_SmartAudio::parse_response_buffer(const uint8_t *buffer)
const uint8_t *startPtr = buffer + 2;
const uint8_t *endPtr = buffer + headerPayloadLength;
if (crc8_dvb_s2_update(0x00, startPtr, headerPayloadLength-2)!=*(endPtr)
if ((crc8_dvb_s2_update(0x00, startPtr, headerPayloadLength-2)!=*(endPtr) && !ignore_crc())
|| header->headerByte != SMARTAUDIO_HEADER_BYTE
|| header->syncByte != SMARTAUDIO_SYNC_BYTE) {
debug("parse_response_buffer() failed - invalid CRC or header");
return false;
}
// SEND TO GCS A MESSAGE TO UNDERSTAND WHATS HAPPENING
@ -590,20 +604,24 @@ bool AP_SmartAudio::parse_response_buffer(const uint8_t *buffer)
const uint8_t power = resp->payload & 0xFF;
switch (_protocol_version) {
case SMARTAUDIO_SPEC_PROTOCOL_v21:
if (vtx.get_configured_power_dbm() != power) {
vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
}
vtx.set_power_dbm(power);
vtx.set_configured_power_mw(vtx.get_power_mw());
break;
case SMARTAUDIO_SPEC_PROTOCOL_v2:
if (vtx.get_configured_power_level() != power) {
vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
}
vtx.set_power_level(power);
vtx.set_configured_power_mw(vtx.get_power_mw());
break;
default:
switch(power) {
case 16: vtx.set_power_level(1); break; // 200mw
case 25: vtx.set_power_level(2); break; // 500mw
case 40: vtx.set_power_level(3); break; // 800mw
default: vtx.set_power_level(0); break; // 25mw
if (vtx.get_configured_power_dac() != power) {
vtx.update_power_dbm(vtx.get_configured_power_dbm(), AP_VideoTX::PowerActive::Inactive);
}
vtx.set_power_dac(power);
vtx.set_configured_power_mw(vtx.get_power_mw());
break;
}

19
libraries/AP_VideoTX/AP_SmartAudio.h
View File

@ -85,7 +85,8 @@ public:
uint16_t frequency;
uint8_t band;
uint8_t* power_levels;
uint8_t num_power_levels;
uint8_t power_levels[8];
uint8_t power_in_dbm;
uint16_t pitmodeFrequency;
@ -136,14 +137,10 @@ public:
} PACKED;
struct SettingsExtendedResponseFrame {
FrameHeader header;
uint8_t channel;
uint8_t power;
uint8_t operationMode;
uint16_t frequency;
uint8_t power_dbm;
uint8_t power_levels_len;
uint8_t power_dbm_levels; // first in the list of dbm levels
SettingsResponseFrame settings;
uint8_t power_dbm; // current power
uint8_t num_power_levels;
uint8_t power_levels[8]; // first in the list of dbm levels
//uint8_t crc;
} PACKED;
@ -217,13 +214,15 @@ private:
#ifdef SA_DEBUG
// utility method for debugging.
void print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t x,uint8_t offset);
void print_bytes_to_hex_string(const char* msg, const uint8_t buf[], uint8_t length);
#endif
void print_settings(const Settings* settings);
void update_vtx_params();
void update_vtx_settings(const Settings& settings);
bool ignore_crc() const { return AP::vtx().has_option(AP_VideoTX::VideoOptions::VTX_SA_IGNORE_CRC); }
// looping over requests
void loop();
// send a frame over the wire

380
libraries/AP_VideoTX/AP_VideoTX.cpp
View File

@ -61,9 +61,9 @@ const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
// @Param: OPTIONS
// @DisplayName: Video Transmitter Options
// @Description: Video Transmitter Options. Pitmode puts the VTX in a low power state. Unlocked enables certain restricted frequencies and power levels. Do not enable the Unlocked option unless you have appropriate permissions in your jurisdiction to transmit at high power levels.
// @Description: Video Transmitter Options. Pitmode puts the VTX in a low power state. Unlocked enables certain restricted frequencies and power levels. Do not enable the Unlocked option unless you have appropriate permissions in your jurisdiction to transmit at high power levels. One stop-bit may be required for VTXs that erroneously mimic iNav behaviour.
// @User: Advanced
// @Bitmask: 0:Pitmode,1:Pitmode until armed,2:Pitmode when disarmed,3:Unlocked,4:Add leading zero byte to requests
// @Bitmask: 0:Pitmode,1:Pitmode until armed,2:Pitmode when disarmed,3:Unlocked,4:Add leading zero byte to requests,5:Use 1 stop-bit in SmartAudio,6:Ignore CRC in SmartAudio,7:Ignore status updates in CRSF and blindly set VTX options
AP_GROUPINFO("OPTIONS", 6, AP_VideoTX, _options, 0),
// @Param: MAX_POWER
@ -75,6 +75,13 @@ const AP_Param::GroupInfo AP_VideoTX::var_info[] = {
AP_GROUPEND
};
//#define VTX_DEBUG
#ifdef VTX_DEBUG
# define debug(fmt, args...) hal.console->printf("VTX: " fmt "\n", ##args)
#else
# define debug(fmt, args...) do {} while(0)
#endif
extern const AP_HAL::HAL& hal;
const char * AP_VideoTX::band_names[] = {"A","B","E","F","R","L"};
@ -89,6 +96,23 @@ const uint16_t AP_VideoTX::VIDEO_CHANNELS[AP_VideoTX::MAX_BANDS][VTX_MAX_CHANNEL
{ 5621, 5584, 5547, 5510, 5473, 5436, 5399, 5362} /* LO Race */
};
// mapping of power level to milliwatt to dbm
// valid power levels from SmartAudio spec, the adjacent levels might be the actual values
// so these are marked as level + 0x10 and will be switched if a dbm message proves it
AP_VideoTX::PowerLevel AP_VideoTX::_power_levels[VTX_MAX_POWER_LEVELS] = {
// level, mw, dbm, dac
{ 0xFF, 0, 0, 0 }, // only in SA 2.1
{ 0, 25, 14, 7 },
{ 0x11, 100, 20, 0xFF }, // only in SA 2.1
{ 1, 200, 23, 16 },
{ 0x12, 400, 26, 0xFF }, // only in SA 2.1
{ 2, 500, 27, 25 },
//{ 0x13, 600, 28, 0xFF },
{ 3, 800, 29, 40 },
{ 0x13, 1000, 30, 0xFF }, // only in SA 2.1
{ 0xFF, 0, 0, 0XFF, PowerActive::Inactive } // slot reserved for a custom power level
};
AP_VideoTX::AP_VideoTX()
{
if (singleton) {
@ -111,7 +135,23 @@ bool AP_VideoTX::init(void)
return false;
}
_current_power = _power_mw;
// PARAMETER_CONVERSION - Added: Sept-2022
_options.convert_parameter_width(AP_PARAM_INT16);
// find the index into the power table
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_mw <= _power_levels[i].mw) {
if (_power_mw != _power_levels[i].mw) {
if (i > 0) {
_current_power = i - 1;
}
_power_mw.set_and_save(get_power_mw());
} else {
_current_power = i;
}
break;
}
}
_current_band = _band;
_current_channel = _channel;
_current_frequency = _frequency_mhz;
@ -137,91 +177,158 @@ bool AP_VideoTX::get_band_and_channel(uint16_t freq, VideoBand& band, uint8_t& c
}
// set the current power
void AP_VideoTX::set_configured_power_mw(uint16_t power) {
void AP_VideoTX::set_configured_power_mw(uint16_t power)
{
_power_mw.set_and_save_ifchanged(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 * 0.1f)));
break;
uint8_t AP_VideoTX::find_current_power() const
{
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_mw == _power_levels[i].mw) {
return i;
}
}
// 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)));
}
}
// get the power "level"
uint8_t AP_VideoTX::get_configured_power_level() const {
if (_power_mw < 26) {
return 0;
} else if (_power_mw < 201) {
return 1;
} else if (_power_mw < 501) {
return 2;
} else { // 800
return 3;
}
// set the power in dbm, rounding appropriately
void AP_VideoTX::set_power_dbm(uint8_t power, PowerActive active)
{
if (power == _power_levels[_current_power].dbm
&& _power_levels[_current_power].active == active) {
return;
}
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (power == _power_levels[i].dbm) {
_current_power = i;
_power_levels[i].active = active;
debug("learned power %ddbm", power);
// now unlearn the "other" power level since we have no other way of guessing
// the supported levels
if ((_power_levels[i].level & 0xF0) == 0x10) {
_power_levels[i].level = _power_levels[i].level & 0xF;
}
if (i > 0 && _power_levels[i-1].level == _power_levels[i].level) {
debug("invalidated power %dwm, level %d is now %dmw", _power_levels[i-1].mw, _power_levels[i].level, _power_levels[i].mw);
_power_levels[i-1].level = 0xFF;
_power_levels[i-1].active = PowerActive::Inactive;
} else if (i < VTX_MAX_POWER_LEVELS-1 && _power_levels[i+1].level == _power_levels[i].level) {
debug("invalidated power %dwm, level %d is now %dmw", _power_levels[i+1].mw, _power_levels[i].level, _power_levels[i].mw);
_power_levels[i+1].level = 0xFF;
_power_levels[i+1].active = PowerActive::Inactive;
}
return;
}
}
// learn the non-standard power
_current_power = update_power_dbm(power, active);
}
// add an active power setting in dbm
uint8_t AP_VideoTX::update_power_dbm(uint8_t power, PowerActive active)
{
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (power == _power_levels[i].dbm) {
if (_power_levels[i].active != active) {
_power_levels[i].active = active;
debug("%s power %ddbm", active == PowerActive::Active ? "learned" : "invalidated", power);
}
return i;
}
}
// handed a non-standard value, use the last slot
_power_levels[VTX_MAX_POWER_LEVELS-1].dbm = power;
_power_levels[VTX_MAX_POWER_LEVELS-1].level = 255;
_power_levels[VTX_MAX_POWER_LEVELS-1].dac = 255;
_power_levels[VTX_MAX_POWER_LEVELS-1].mw = uint16_t(roundf(powf(10, power / 10.0f)));
_power_levels[VTX_MAX_POWER_LEVELS-1].active = active;
debug("non-standard power %ddbm -> %dmw", power, _power_levels[VTX_MAX_POWER_LEVELS-1].mw);
return VTX_MAX_POWER_LEVELS-1;
}
// add all active power setting in dbm
void AP_VideoTX::update_all_power_dbm(uint8_t nlevels, const uint8_t power[])
{
for (uint8_t i = 0; i < nlevels; i++) {
update_power_dbm(power[i], PowerActive::Active);
}
// invalidate the remaining ones
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_levels[i].active == PowerActive::Unknown) {
_power_levels[i].active = PowerActive::Inactive;
}
}
}
// set the power in mw
void AP_VideoTX::set_power_mw(uint16_t power)
{
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (power == _power_levels[i].mw) {
_current_power = i;
break;
}
}
}
// set the power "level"
void AP_VideoTX::set_power_level(uint8_t level) {
switch (level) {
case 1:
_current_power = 200;
break;
case 2:
_current_power = 500;
break;
case 3:
_current_power = 800;
break;
case 0:
default:
_current_power = 25;
void AP_VideoTX::set_power_level(uint8_t level, PowerActive active)
{
if (level == _power_levels[_current_power].level
&& _power_levels[_current_power].active == active) {
return;
}
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (level == _power_levels[i].level) {
_current_power = i;
_power_levels[i].active = active;
debug("learned power level %d: %dmw", level, get_power_mw());
break;
}
}
}
// set the power dac
void AP_VideoTX::set_power_dac(uint16_t power, PowerActive active)
{
if (power == _power_levels[_current_power].dac
&& _power_levels[_current_power].active == active) {
return;
}
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (power == _power_levels[i].dac) {
_current_power = i;
_power_levels[i].active = active;
debug("learned power %dmw", get_power_mw());
}
}
}
// set the current channel
void AP_VideoTX::set_enabled(bool enabled) {
void AP_VideoTX::set_enabled(bool enabled)
{
_current_enabled = enabled;
if (!_enabled.configured()) {
_enabled.set_and_save(enabled);
}
}
void AP_VideoTX::set_power_is_current()
{
set_power_dbm(get_configured_power_dbm());
}
void AP_VideoTX::set_freq_is_current()
{
_current_frequency = _frequency_mhz;
_current_band = _band;
_current_channel = _channel;
}
// periodic update
void AP_VideoTX::update(void)
{
@ -241,6 +348,15 @@ void AP_VideoTX::update(void)
_options.set(_options | uint8_t(VideoOptions::VTX_PITMODE));
}
}
// check that the requested power is actually allowed
// reset if not
if (_power_mw != get_power_mw()) {
if (_power_levels[find_current_power()].active == PowerActive::Inactive) {
// reset to something we know works
debug("power reset to %dmw from %dmw", get_power_mw(), _power_mw.get());
_power_mw.set_and_save(get_power_mw());
}
}
}
bool AP_VideoTX::update_options() const
@ -254,6 +370,12 @@ bool AP_VideoTX::update_options() const
return true;
}
#if HAL_CRSF_TELEM_ENABLED
// using CRSF so unlock is not an option
if (AP::crsf_telem() != nullptr) {
return false;
}
#endif
// check unlock only
if ((_options & uint8_t(VideoOptions::VTX_UNLOCKED)) != 0
&& (_current_options & uint8_t(VideoOptions::VTX_UNLOCKED)) == 0) {
@ -264,6 +386,21 @@ bool AP_VideoTX::update_options() const
return false;
}
bool AP_VideoTX::update_power() const {
if (!_defaults_set || _power_mw == get_power_mw() || get_pitmode()) {
return false;
}
// check that the requested power is actually allowed
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_mw == _power_levels[i].mw
&& _power_levels[i].active != PowerActive::Inactive) {
return true;
}
}
// asked for something unsupported - only SA2.1 allows this and will have already provided a list
return false;
}
bool AP_VideoTX::have_params_changed() const
{
return _enabled
@ -301,7 +438,7 @@ bool AP_VideoTX::set_defaults()
return false;
}
// check that our current view of freqency matches band/channel
// check that our current view of frequency matches band/channel
// if not then force one to be correct
uint16_t calced_freq = get_frequency_mhz(_current_band, _current_channel);
if (_current_frequency != calced_freq) {
@ -326,10 +463,10 @@ bool AP_VideoTX::set_defaults()
_channel.set_and_save(_current_channel);
}
if (!_band.configured()) {
_frequency_mhz.set_and_save(_current_band);
_band.set_and_save(_current_band);
}
if (!_power_mw.configured()) {
_power_mw.set_and_save(_current_power);
_power_mw.set_and_save(get_power_mw());
}
if (!_frequency_mhz.configured()) {
_frequency_mhz.set_and_save(_current_frequency);
@ -366,95 +503,24 @@ void AP_VideoTX::change_power(int8_t position)
if (position < 0 || position > 5) {
return;
}
// first find out how many possible levels there are
uint8_t num_active_levels = 0;
for (uint8_t i = 0; i < VTX_MAX_POWER_LEVELS; i++) {
if (_power_levels[i].active != PowerActive::Inactive) {
num_active_levels++;
}
}
// iterate through to find the level
uint16_t level = constrain_int16(roundf((num_active_levels * (position + 1)/ 6.0f) - 1), 0, num_active_levels - 1);
debug("looking for pos %d power level %d from %d", position, level, num_active_levels);
uint16_t power = 0;
// 0 or 25
if (_max_power_mw < 100) {
switch (position) {
case 3:
case 4:
case 5:
power = 25;
break;
default:
power = 0;
break;
for (uint8_t i = 0, j = 0; i < num_active_levels; i++, j++) {
while (_power_levels[j].active == PowerActive::Inactive && j < VTX_MAX_POWER_LEVELS-1) {
j++;
}
}
// 0, 25 or 100
else if (_max_power_mw < 200) {
switch (position) {
case 0:
power = 0;
break;
case 5:
power = 100;
break;
default:
power = 25;
break;
}
}
// 0, 25, 100 or 200
else if (_max_power_mw < 500) {
switch (position) {
case 1:
case 2:
power = 25;
break;
case 3:
case 4:
power = 100;
break;
case 5:
power = 200;
break;
default:
power = 0;
break;
}
}
// 0, 25, 100, 200 or 500
else if (_max_power_mw < 800) {
switch (position) {
case 1:
case 2:
power = 25;
break;
case 3:
power = 100;
break;
case 4:
power = 200;
break;
case 5:
power = 500;
break;
default:
power = 0;
break;
}
}
// full range
else {
switch (position) {
case 1:
power = 25;
break;
case 2:
power = 100;
break;
case 3:
power = 200;
break;
case 4:
power = 500;
break;
case 5:
power = _max_power_mw; // some VTX's support 1000mw
break;
default:
power = 0;
if (i == level) {
power = _power_levels[j].mw;
debug("selected power %dmw", power);
break;
}
}

73
libraries/AP_VideoTX/AP_VideoTX.h
View File

@ -17,6 +17,7 @@
#include <AP_Param/AP_Param.h>
#define VTX_MAX_CHANNELS 8
#define VTX_MAX_POWER_LEVELS 9
class AP_VideoTX {
public:
@ -44,6 +45,9 @@ public:
VTX_PITMODE_ON_DISARM = (1 << 2),
VTX_UNLOCKED = (1 << 3),
VTX_PULLDOWN = (1 << 4),
VTX_SA_ONE_STOP_BIT = (1 << 5),
VTX_SA_IGNORE_CRC = (1 << 6),
VTX_CRSF_IGNORE_STAT = (1 << 7),
};
static const char *band_names[];
@ -58,6 +62,22 @@ public:
MAX_BANDS
};
enum class PowerActive {
Unknown,
Active,
Inactive
};
struct PowerLevel {
uint8_t level;
uint16_t mw;
uint8_t dbm;
uint8_t dac; // SmartAudio v1 dac value
PowerActive active;
};
static PowerLevel _power_levels[VTX_MAX_POWER_LEVELS];
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]; }
@ -70,15 +90,31 @@ public:
bool update_frequency() const { return _defaults_set && _frequency_mhz != _current_frequency; }
void update_configured_frequency();
// get / set power level
void set_power_mw(uint16_t power) { _current_power = power; }
void set_power_level(uint8_t level);
void set_power_dbm(uint8_t power);
void set_power_mw(uint16_t power);
void set_power_level(uint8_t level, PowerActive active=PowerActive::Active);
void set_power_dbm(uint8_t power, PowerActive active=PowerActive::Active);
void set_power_dac(uint16_t power, PowerActive active=PowerActive::Active);
// add a new dbm setting to those supported
uint8_t update_power_dbm(uint8_t power, PowerActive active=PowerActive::Active);
void update_all_power_dbm(uint8_t nlevels, const uint8_t levels[]);
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;
uint8_t get_configured_power_level() const;
bool update_power() const { return _defaults_set && _power_mw != _current_power; }
uint16_t get_power_mw() const { return _power_levels[_current_power].mw; }
// get the power in dbm, rounding appropriately
uint8_t get_configured_power_dbm() const {
return _power_levels[find_current_power()].dbm;
}
// get the power "level"
uint8_t get_configured_power_level() const {
return _power_levels[find_current_power()].level & 0xF;
}
// get the power "dac"
uint8_t get_configured_power_dac() const {
return _power_levels[find_current_power()].dac;
}
bool update_power() const;
// change the video power based on switch input
void change_power(int8_t position);
// get / set the frequency band
@ -95,11 +131,13 @@ public:
bool update_channel() const { return _defaults_set && _channel != _current_channel; }
void update_configured_channel_and_band();
// get / set vtx option
void set_options(uint8_t options) { _current_options = options; }
void set_configured_options(uint8_t options) { _options.set_and_save_ifchanged(options); }
uint8_t get_configured_options() const { return _options; }
uint8_t get_options() const { return _current_options; }
bool has_option(VideoOptions option) const { return _options.get() & uint8_t(option); }
void set_options(uint16_t options) { _current_options = options; }
void set_configured_options(uint16_t options) { _options.set_and_save_ifchanged(options); }
uint16_t get_configured_options() const { return _options; }
uint16_t get_options() const { return _current_options; }
bool has_option(VideoOptions option) const { return _options.get() & uint16_t(option); }
bool get_configured_pitmode() const { return _options.get() & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE); }
bool get_pitmode() const { return _current_options & uint8_t(AP_VideoTX::VideoOptions::VTX_PITMODE); }
bool update_options() const;
// get / set whether the vtx is enabled
void set_enabled(bool enabled);
@ -112,6 +150,10 @@ public:
bool set_defaults();
// display the current VTX settings in the GCS
void announce_vtx_settings() const;
// force the current values to reflect the configured values
void set_power_is_current();
void set_freq_is_current();
void set_options_are_current() { _current_options = _options; }
void set_configuration_finished(bool configuration_finished) { _configuration_finished = configuration_finished; }
bool is_configuration_finished() { return _configuration_finished; }
@ -119,6 +161,7 @@ public:
static AP_VideoTX *singleton;
private:
uint8_t find_current_power() const;
// channel frequency
AP_Int16 _frequency_mhz;
uint16_t _current_frequency;
@ -137,8 +180,8 @@ private:
uint8_t _current_channel;
// vtx options
AP_Int8 _options;
uint8_t _current_options;
AP_Int16 _options;
uint16_t _current_options;
AP_Int8 _enabled;
bool _current_enabled;
@ -146,7 +189,7 @@ private:
bool _initialized;
// when defaults have been configured
bool _defaults_set;
// true when configuration have been applied succesfully to the VTX
// true when configuration have been applied successfully to the VTX
bool _configuration_finished;
};