FrSky_Telem: init uart when protocol is S-Port

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;

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;