diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp index 71b666dcab..c58bc57873 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.cpp @@ -185,17 +185,8 @@ void AP_RCProtocol_SRXL2::update(void) } } -void AP_RCProtocol_SRXL2::capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi) -{ - AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); - - if (srxl2 != nullptr) { - srxl2->_capture_scaled_input(values_p, in_failsafe, new_rssi); - } -} - // capture SRXL2 encoded values -void AP_RCProtocol_SRXL2::_capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi) +void AP_RCProtocol_SRXL2::capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t new_rssi) { _in_failsafe = in_failsafe; // AP rssi: -1 for unknown, 0 for no link, 255 for maximum link @@ -273,7 +264,7 @@ void AP_RCProtocol_SRXL2::process_handshake(uint32_t baudrate) _handshake_start_ms = now; // it seems the handshake protocol only sets the baudrate after receiving data // since we are sending data unprompted make sure that the uart is set up correctly - _change_baud_rate(baudrate); + change_baud_rate(baudrate); } // we have not bootstrapped and attempts to listen first have failed @@ -295,16 +286,6 @@ void AP_RCProtocol_SRXL2::process_handshake(uint32_t baudrate) // send data to the uart void AP_RCProtocol_SRXL2::send_on_uart(uint8_t* pBuffer, uint8_t length) -{ - AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); - - if (srxl2 != nullptr) { - srxl2->_send_on_uart(pBuffer, length); - } -} - -// send data to the uart -void AP_RCProtocol_SRXL2::_send_on_uart(uint8_t* pBuffer, uint8_t length) { AP_HAL::UARTDriver* uart = get_available_UART(); @@ -331,16 +312,6 @@ void AP_RCProtocol_SRXL2::_send_on_uart(uint8_t* pBuffer, uint8_t length) // change the uart baud rate void AP_RCProtocol_SRXL2::change_baud_rate(uint32_t baudrate) -{ - AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); - - if (srxl2 != nullptr) { - srxl2->_change_baud_rate(baudrate); - } -} - -// change the uart baud rate -void AP_RCProtocol_SRXL2::_change_baud_rate(uint32_t baudrate) { AP_HAL::UARTDriver* uart = get_available_UART(); if (uart != nullptr) { @@ -358,8 +329,11 @@ void AP_RCProtocol_SRXL2::_change_baud_rate(uint32_t baudrate) // baudRate - the actual baud rate (currently either 115200 or 400000) void srxlChangeBaudRate(uint8_t uart, uint32_t baudRate) { - AP_RCProtocol_SRXL2::change_baud_rate(baudRate); + AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); + if (srxl2 != nullptr) { + srxl2->change_baud_rate(baudRate); + } } // User-provided routine to actually transmit a packet on the given UART: @@ -368,7 +342,11 @@ void srxlChangeBaudRate(uint8_t uart, uint32_t baudRate) // length - the number of bytes contained in pBuffer that should be sent void srxlSendOnUart(uint8_t uart, uint8_t* pBuffer, uint8_t length) { - AP_RCProtocol_SRXL2::send_on_uart(pBuffer, length); + AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); + + if (srxl2 != nullptr) { + srxl2->send_on_uart(pBuffer, length); + } } // User-provided callback routine to fill in the telemetry data to send to the master when requested: @@ -389,10 +367,16 @@ void srxlFillTelemetry(SrxlTelemetryData* pTelemetryData) // so be very careful to only do local operations void srxlReceivedChannelData(SrxlChannelData* pChannelData, bool isFailsafe) { + AP_RCProtocol_SRXL2* srxl2 = AP_RCProtocol_SRXL2::get_singleton(); + + if (srxl2 == nullptr) { + return; + } + if (isFailsafe) { - AP_RCProtocol_SRXL2::capture_scaled_input((const uint8_t *)pChannelData->values, true, pChannelData->rssi); + srxl2->capture_scaled_input((const uint8_t *)pChannelData->values, true, pChannelData->rssi); } else { - AP_RCProtocol_SRXL2::capture_scaled_input((const uint8_t *)srxlChData.values, false, srxlChData.rssi); + srxl2->capture_scaled_input((const uint8_t *)srxlChData.values, false, srxlChData.rssi); } } diff --git a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h index ab6b0d09b8..d508be7b29 100644 --- a/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h +++ b/libraries/AP_RCProtocol/AP_RCProtocol_SRXL2.h @@ -39,20 +39,15 @@ public: return _singleton; } - // static functions for SRXL2 callbacks - static void capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi); - static void send_on_uart(uint8_t* pBuffer, uint8_t length); - static void change_baud_rate(uint32_t baudrate); - // configure the VTX from Spektrum data + void capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi); + void send_on_uart(uint8_t* pBuffer, uint8_t length); + void change_baud_rate(uint32_t baudrate); private: static AP_RCProtocol_SRXL2* _singleton; void _process_byte(uint32_t timestamp_us, uint8_t byte); - void _send_on_uart(uint8_t* pBuffer, uint8_t length); - void _change_baud_rate(uint32_t baudrate); - void _capture_scaled_input(const uint8_t *values_p, bool in_failsafe, int16_t rssi); void _bootstrap(uint8_t device_id); bool is_bootstrapped() const { return _device_id != 0; }