From db06de5c9c9f57c3362192d0dbf99d2bed48f885 Mon Sep 17 00:00:00 2001 From: Andy Piper Date: Thu, 6 May 2021 21:26:10 +0100 Subject: [PATCH] AP_VideoTX: correctly deal with unresolvable options requests move power changes to middle of RC range --- libraries/AP_VideoTX/AP_SmartAudio.cpp | 16 ++++++++-- libraries/AP_VideoTX/AP_VideoTX.cpp | 42 +++++++++++++++++++------- libraries/AP_VideoTX/AP_VideoTX.h | 2 +- 3 files changed, 45 insertions(+), 15 deletions(-) diff --git a/libraries/AP_VideoTX/AP_SmartAudio.cpp b/libraries/AP_VideoTX/AP_SmartAudio.cpp index 6fb81371fc..f7a6e2ab47 100644 --- a/libraries/AP_VideoTX/AP_SmartAudio.cpp +++ b/libraries/AP_VideoTX/AP_SmartAudio.cpp @@ -208,14 +208,18 @@ void AP_SmartAudio::update_vtx_params() // prioritize pitmode changes if (_vtx_options_change_pending) { + debug("update mode '%c%c%c%c'", (mode & 0x8) ? 'U' : 'L', + (mode & 0x4) ? 'N' : ' ', (mode & 0x2) ? 'O' : ' ', (mode & 0x1) ? 'I' : ' '); set_operation_mode(mode); } else if (_vtx_freq_change_pending) { + debug("update frequency"); if (_vtx_use_set_freq) { set_frequency(vtx.get_configured_frequency_mhz(), false); } else { set_channel(vtx.get_configured_band() * VTX_MAX_CHANNELS + vtx.get_configured_channel()); } } else if (_vtx_power_change_pending) { + debug("update power"); switch (_protocol_version) { case SMARTAUDIO_SPEC_PROTOCOL_v21: set_power(vtx.get_configured_power_dbm() | 0x80); @@ -474,8 +478,14 @@ void AP_SmartAudio::print_bytes_to_hex_string(const char* msg, const uint8_t buf void AP_SmartAudio::print_settings(const Settings* settings) { - debug("SETTINGS: VER: %u, MD: 0x%x, CH: %u, PWR: %u, FREQ: %u, BND: %u", - settings->version, settings->mode, settings->channel, settings->power, settings->frequency, settings->band); + 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->channel, settings->power, settings->power_in_dbm, settings->frequency, settings->band); } void AP_SmartAudio::update_vtx_settings(const Settings& settings) @@ -496,7 +506,7 @@ void AP_SmartAudio::update_vtx_settings(const Settings& settings) // PITMODE | UNLOCKED // SmartAudio 2.1 dropped support for outband pitmode so we won't support it - uint8_t opts = ((settings.mode & 0x2) >> 1) | ((settings.mode & 0x10) >> 3); + uint8_t opts = ((settings.mode & 0x2) >> 1) | ((settings.mode & 0x10) >> 1); vtx.set_options(opts); // make sure the configured values now reflect reality diff --git a/libraries/AP_VideoTX/AP_VideoTX.cpp b/libraries/AP_VideoTX/AP_VideoTX.cpp index f009977e69..32070b0150 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.cpp +++ b/libraries/AP_VideoTX/AP_VideoTX.cpp @@ -218,7 +218,7 @@ void AP_VideoTX::set_enabled(bool enabled) { } } -// peiodic update +// periodic update void AP_VideoTX::update(void) { #if HAL_CRSF_TELEM_ENABLED @@ -239,6 +239,27 @@ void AP_VideoTX::update(void) } } +bool AP_VideoTX::update_options() const +{ + if (!_defaults_set) { + return false; + } + // check pitmode + if ((_options & uint8_t(VideoOptions::VTX_PITMODE)) + != (_current_options & uint8_t(VideoOptions::VTX_PITMODE))) { + return true; + } + + // check unlock only + if ((_options & uint8_t(VideoOptions::VTX_UNLOCKED)) != 0 + && (_current_options & uint8_t(VideoOptions::VTX_UNLOCKED)) == 0) { + return true; + } + + // ignore everything else + return false; +} + bool AP_VideoTX::have_params_changed() const { return _enabled @@ -335,6 +356,7 @@ void AP_VideoTX::announce_vtx_settings() const } // change the video power based on switch input +// 6-pos range is in the middle of the available range void AP_VideoTX::change_power(int8_t position) { if (position < 0 || position > 5) { @@ -345,7 +367,6 @@ void AP_VideoTX::change_power(int8_t position) // 0 or 25 if (_max_power_mw < 100) { switch (position) { - case 2: case 3: case 4: case 5: @@ -359,29 +380,28 @@ void AP_VideoTX::change_power(int8_t position) // 0, 25 or 100 else if (_max_power_mw < 200) { switch (position) { - case 2: - case 3: - power = 25; + case 0: + power = 0; break; - case 4: case 5: power = 100; break; default: - power = 0; + 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 4: case 5: power = 200; break; @@ -394,15 +414,15 @@ void AP_VideoTX::change_power(int8_t position) else if (_max_power_mw < 800) { switch (position) { case 1: + case 2: power = 25; break; - case 2: + case 3: power = 100; break; - case 3: + case 4: power = 200; break; - case 4: case 5: power = 500; break; diff --git a/libraries/AP_VideoTX/AP_VideoTX.h b/libraries/AP_VideoTX/AP_VideoTX.h index b1960bbaab..dc5c9ad006 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.h +++ b/libraries/AP_VideoTX/AP_VideoTX.h @@ -99,7 +99,7 @@ public: 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); } - bool update_options() const { return _defaults_set && _options != _current_options; } + bool update_options() const; // get / set whether the vtx is enabled void set_enabled(bool enabled); bool get_enabled() const { return _enabled; }