AP_VideoTX: add autobauding to Tramp

Record enabled backends

libraries/AP_VideoTX/AP_SmartAudio.cpp

@@ -62,6 +62,8 @@ bool AP_SmartAudio::init()
             return false;
         }
 
+        AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::SmartAudio);
+
         return true;
     }
     return false;

libraries/AP_VideoTX/AP_Tramp.cpp

@@ -77,12 +77,17 @@ void AP_Tramp::send_command(uint8_t cmd, uint16_t param)
     port->write(request_buffer, TRAMP_BUF_SIZE);
     port->flush();
 
+    _packets_sent++;
+
     debug("send command '%c': %u", cmd, param);
 }
 
 // Process response and return code if valid else 0
 char AP_Tramp::handle_response(void)
 {
+    _packets_rcvd++;
+    _packets_sent = _packets_rcvd;
+
     const uint8_t respCode = response_buffer[1];
 
     switch (respCode) {
@@ -135,8 +140,8 @@ char AP_Tramp::handle_response(void)
                 vtx.announce_vtx_settings();
             }
 
-            debug("device config: freq: %u, power: %u, pitmode: %u",
+            debug("device config: freq: %u, cfg pwr: %umw, act pwr: %umw, pitmode: %u",
-                unsigned(freq), unsigned(power), unsigned(pit_mode));
+                unsigned(freq), unsigned(power), unsigned(cur_act_power), unsigned(pit_mode));
 
 
             return 'v';
@@ -162,6 +167,7 @@ char AP_Tramp::handle_response(void)
 // Reset receiver state machine
 void AP_Tramp::reset_receiver(void)
 {
+    port->discard_input();
     receive_state = ReceiveState::S_WAIT_LEN;
     receive_pos = 0;
 }
@@ -316,6 +322,9 @@ void AP_Tramp::process_requests()
             // Device replied to freq / power / pit query, enter online
             set_status(TrampStatus::TRAMP_STATUS_ONLINE_MONITOR_FREQPWRPIT);
         } else if ((now - last_time_us) >= TRAMP_MIN_REQUEST_PERIOD_US) {
+
+            update_baud_rate();
+
             // Min request period exceeded, issue another query
             send_query('v');
 
@@ -332,12 +341,14 @@ void AP_Tramp::process_requests()
             AP_VideoTX& vtx = AP::vtx();
             // Config retries remain and min request period exceeded, check freq
             if (!is_race_lock_enabled() && vtx.update_frequency()) {
+                debug("Updating frequency to %uMhz", vtx.get_configured_frequency_mhz());
                 // Freq can be and needs to be updated, issue request
                 send_command('F', vtx.get_configured_frequency_mhz());
 
                 // Set flag
                 configUpdateRequired = true;
             } else if (!is_race_lock_enabled() && vtx.update_power()) {
+                debug("Updating power to %umw\n", vtx.get_configured_power_mw());
                 // Power can be and needs to be updated, issue request
                 send_command('P', vtx.get_configured_power_mw());
 
@@ -477,6 +488,35 @@ void AP_Tramp::update()
     process_requests();
 }
 
+// we missed a response too many times - update the baud rate in case the temperature has increased
+void AP_Tramp::update_baud_rate()
+{
+    // on my Unify Pro32 the VTX will respond immediately on power up to a settings request, so 5 packets is easily more than enough
+    // we want to bias autobaud to only frequency hop when the current frequency is clearly exhausted, but after that hop quickly
+    // on a Foxeer Reaper Infinity the actual response baudrate is more like 9400, so auto-bauding down in the first instance.
+    if (_packets_sent - _packets_rcvd < 5) {
+        return;
+    }
+
+    if (_smartbaud_direction == AutobaudDirection::UP
+        && _smartbaud == VTX_TRAMP_SMARTBAUD_MAX) {
+        _smartbaud_direction = AutobaudDirection::DOWN;
+    } else if (_smartbaud_direction == AutobaudDirection::DOWN
+        && _smartbaud == VTX_TRAMP_SMARTBAUD_MIN) {
+        _smartbaud_direction = AutobaudDirection::UP;
+    }
+
+    _smartbaud += VTX_TRAMP_SMARTBAUD_STEP * int32_t(_smartbaud_direction);
+
+    debug("autobaud: %d", int(_smartbaud));
+
+    port->discard_input();
+    port->begin(_smartbaud);
+
+    _packets_sent = _packets_rcvd = 0;
+}
+
+
 bool AP_Tramp::init(void)
 {
     if (AP::vtx().get_enabled() == 0) {
@@ -495,6 +535,8 @@ bool AP_Tramp::init(void)
         port->begin(AP_TRAMP_UART_BAUD, AP_TRAMP_UART_BUFSIZE_RX, AP_TRAMP_UART_BUFSIZE_TX);
         debug("port opened");
 
+        AP::vtx().set_provider_enabled(AP_VideoTX::VTXType::Tramp);
+
         return true;
     }
     return false;

libraries/AP_VideoTX/AP_Tramp.h

@@ -38,6 +38,11 @@
 // Race lock - settings can't be changed
 #define TRAMP_CONTROL_RACE_LOCK (0x01)
 
+#define VTX_TRAMP_UART_BAUD            9600
+#define VTX_TRAMP_SMARTBAUD_MIN        9120     // -5%
+#define VTX_TRAMP_SMARTBAUD_MAX        10080    // +5%
+#define VTX_TRAMP_SMARTBAUD_STEP       120
+
 class AP_Tramp
 {
 public:
@@ -73,6 +78,8 @@ private:
     void set_frequency(uint16_t freq);
     void set_power(uint16_t power);
     void set_pit_mode(uint8_t onoff);
+    // change baud automatically when request-response fails many times
+    void update_baud_rate();
 
     // serial interface
     AP_HAL::UARTDriver *port;                  // UART used to send data to Tramp VTX
@@ -115,6 +122,17 @@ private:
     int16_t cur_temp;
     uint8_t cur_control_mode;
 
+    // statistics
+    uint16_t _packets_sent;
+    uint16_t _packets_rcvd;
+
+    // value for current baud adjust
+    int32_t _smartbaud = VTX_TRAMP_UART_BAUD;
+    enum class AutobaudDirection {
+        UP = 1,
+        DOWN = -1
+    } _smartbaud_direction = AutobaudDirection::DOWN;
+
     // Retry count
     uint8_t retry_count = VTX_TRAMP_MAX_RETRIES;
 

libraries/AP_VideoTX/AP_VideoTX.cpp

@@ -113,7 +113,7 @@ AP_VideoTX::PowerLevel AP_VideoTX::_power_levels[VTX_MAX_POWER_LEVELS] = {
     { 1,    200,  23, 16   },
     { 0x12, 400,  26, 0xFF }, // only in SA 2.1
     { 2,    500,  27, 25   },
-    //{ 0x13, 600,  28, 0xFF },
+    { 0x12, 600,  28, 0xFF }, // Tramp lies above power levels and always returns 25/100/200/400/600
     { 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
@@ -342,13 +342,6 @@ void AP_VideoTX::update(void)
         return;
     }
 
-#if HAL_CRSF_TELEM_ENABLED
-    AP_CRSF_Telem* crsf = AP::crsf_telem();
-
-    if (crsf != nullptr) {
-        crsf->update();
-    }
-#endif
     // manipulate pitmode if pitmode-on-disarm or power-on-arm is set
     if (has_option(VideoOptions::VTX_PITMODE_ON_DISARM) || has_option(VideoOptions::VTX_PITMODE_UNTIL_ARM)) {
         if (hal.util->get_soft_armed() && has_option(VideoOptions::VTX_PITMODE)) {

libraries/AP_VideoTX/AP_VideoTX.h

@@ -21,7 +21,7 @@
 #include <AP_Param/AP_Param.h>
 
 #define VTX_MAX_CHANNELS 8
-#define VTX_MAX_POWER_LEVELS 9
+#define VTX_MAX_POWER_LEVELS 10
 
 class AP_VideoTX {
 public:
@@ -74,6 +74,12 @@ public:
         Inactive
     };
 
+    enum VTXType {
+        CRSF = 1U<<0,
+        SmartAudio = 1U<<1,
+        Tramp = 1U<<2
+    };
+
     struct PowerLevel {
         uint8_t level;
         uint16_t mw;
@@ -164,6 +170,10 @@ public:
     void set_configuration_finished(bool configuration_finished) { _configuration_finished = configuration_finished; }
     bool is_configuration_finished() { return _configuration_finished; }
 
+    // manage VTX backends
+    bool is_provider_enabled(VTXType type) const { return (_types & type) != 0; }
+    void set_provider_enabled(VTXType type) { _types |= type; }
+
     static AP_VideoTX *singleton;
 
 private:
@@ -197,6 +207,9 @@ private:
     bool _defaults_set;
     // true when configuration have been applied successfully to the VTX
     bool _configuration_finished;
+
+    // types of VTX providers
+    uint8_t _types;
 };
 
 namespace AP {