diff --git a/libraries/AP_VideoTX/AP_SmartAudio.cpp b/libraries/AP_VideoTX/AP_SmartAudio.cpp index f95bca0614..ef396c345b 100644 --- a/libraries/AP_VideoTX/AP_SmartAudio.cpp +++ b/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; diff --git a/libraries/AP_VideoTX/AP_Tramp.cpp b/libraries/AP_VideoTX/AP_Tramp.cpp index 74581b09d3..54f62a382f 100644 --- a/libraries/AP_VideoTX/AP_Tramp.cpp +++ b/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", - unsigned(freq), unsigned(power), unsigned(pit_mode)); + debug("device config: freq: %u, cfg pwr: %umw, act pwr: %umw, pitmode: %u", + 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; diff --git a/libraries/AP_VideoTX/AP_Tramp.h b/libraries/AP_VideoTX/AP_Tramp.h index 7d812cb3a5..bec9382677 100644 --- a/libraries/AP_VideoTX/AP_Tramp.h +++ b/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; diff --git a/libraries/AP_VideoTX/AP_VideoTX.cpp b/libraries/AP_VideoTX/AP_VideoTX.cpp index 2ccbe27996..d0887080d6 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.cpp +++ b/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)) { diff --git a/libraries/AP_VideoTX/AP_VideoTX.h b/libraries/AP_VideoTX/AP_VideoTX.h index a1321fb096..67e7444937 100644 --- a/libraries/AP_VideoTX/AP_VideoTX.h +++ b/libraries/AP_VideoTX/AP_VideoTX.h @@ -21,7 +21,7 @@ #include #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 {