AP_VideoTX: correctly deal with unresolvable options requests

move power changes to middle of RC range

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",
+    debug("SETTINGS: VER: %u, MD: '%c%c%c%c%c', CH: %u, PWR: %u, DBM: %u FREQ: %u, BND: %u",
-            settings->version, settings->mode, settings->channel, settings->power, settings->frequency, settings->band);
+            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

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 0:
-            case 3:
+                power = 0;
-                power = 25;
                 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;

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; }