diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 189f7b6632..c216743edb 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -28,7 +28,7 @@ AP_Frsky_Telem::AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) : _ahrs(ahrs), _battery(battery), _port(NULL), - _initialised(false), + _initialised_uart(false), _protocol(FrSkyUnknown), _crc(0), _last_frame1_ms(0), @@ -75,6 +75,7 @@ void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_DPort, frsky_serial)) { _port = frsky_serial.uart; _protocol = FrSkyDPORT; + _initialised_uart = true; // SerialManager initialises uart for us } // check for FRSky_SPort if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_SPort, frsky_serial)) { @@ -92,10 +93,6 @@ void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) _sport_status = 0; hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Frsky_Telem::sport_tick)); } - // initialise port ignoring serial ports baud rate parameter - if (_port != NULL) { - _initialised = true; - } } /* @@ -106,7 +103,7 @@ void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) void AP_Frsky_Telem::send_frames(uint8_t control_mode) { // return immediately if not initialised - if (!_initialised) { + if (!_initialised_uart) { return; } @@ -137,6 +134,23 @@ void AP_Frsky_Telem::send_frames(uint8_t control_mode) } } +/* + init_uart_for_sport - initialise uart for use by sport + this must be called from sport_tick which is called from the 1khz scheduler + because the UART begin must be called from the same thread as it is used from + */ +void AP_Frsky_Telem::init_uart_for_sport() +{ + // sanity check protocol + if (_protocol != FrSkySPORT) { + return; + } + + // initialise uart + _port->begin(AP_SERIALMANAGER_FRSKY_SPORT_BAUD, AP_SERIALMANAGER_FRSKY_BUFSIZE_RX, AP_SERIALMANAGER_FRSKY_BUFSIZE_TX); + _initialised_uart = true; +} + /* send_hub_frame - send frame1 and frame2 when protocol is FrSkyDPORT frame 1 is sent every 200ms with baro alt, nb sats, batt volts and amp, control_mode @@ -188,9 +202,9 @@ void AP_Frsky_Telem::send_hub_frame() */ void AP_Frsky_Telem::sport_tick(void) { - if (!_initialised) { - _port->begin(57600); - _initialised = true; + // check UART has been initialised + if (!_initialised_uart) { + init_uart_for_sport(); } int16_t numc; diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 06dbb54118..d13c59cd91 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -93,6 +93,9 @@ public: private: + // init_uart_for_sport - initialise uart for use by sport + void init_uart_for_sport(); + // send_hub_frame - main transmission function when protocol is FrSkyDPORT void send_hub_frame(); @@ -139,7 +142,7 @@ private: AP_AHRS &_ahrs; // reference to attitude estimate AP_BattMonitor &_battery; // reference to battery monitor object AP_HAL::UARTDriver *_port; // UART used to send data to receiver - bool _initialised; // true when we have detected the protocol to use + bool _initialised_uart; // true when we have detected the protocol and UART has been initialised enum FrSkyProtocol _protocol; // protocol used - detected using SerialManager's SERIALX_PROTOCOL parameter uint16_t _crc;