diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp index 8fb337145f..9c64cdf38e 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.cpp @@ -23,15 +23,21 @@ #include extern const AP_HAL::HAL& hal; -void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol) +void AP_Frsky_Telem::init(const AP_SerialManager& serial_manager) { - if (port == NULL) { - return; + // initialise port to null + _port = NULL; + + // check for FRSky_DPort + AP_SerialManager::serial_state frsky_serial; + if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_DPort, frsky_serial)) { + _port = frsky_serial.uart; + _protocol = FrSkyDPORT; } - _port = port; - _protocol = protocol; - if (_protocol == FrSkySPORT) { - _port->begin(57600); + // check for FRSky_SPort + if (serial_manager.find_serial(AP_SerialManager::SerialProtocol_FRSky_SPort, frsky_serial)) { + _port = frsky_serial.uart; + _protocol = FrSkySPORT; _gps_call = 0; _fas_call = 0; _vario_call = 0 ; @@ -43,10 +49,10 @@ void AP_Frsky_Telem::init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol) _sats_data_ready = false; _sport_status = 0; hal.scheduler->register_io_process(AP_HAL_MEMBERPROC(&AP_Frsky_Telem::sport_tick)); - } else { - // if this is D-port then spec says 9600 baud - _port->begin(9600); - _initialised = true; + } + // initialise port ignoring serial ports baud rate parameter + if (_port != NULL) { + _initialised = true; } } @@ -459,6 +465,7 @@ void AP_Frsky_Telem::send_hub_frame() */ void AP_Frsky_Telem::send_frames(uint8_t control_mode) { + // return immediately if not initialised if (!_initialised) { return; } diff --git a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h index 7912760725..c2a7a260ed 100644 --- a/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h +++ b/libraries/AP_Frsky_Telem/AP_Frsky_Telem.h @@ -24,6 +24,7 @@ #include #include #include +#include /* FrSky sensor hub data IDs */ #define FRSKY_ID_GPS_ALT_BP 0x01 @@ -77,18 +78,21 @@ class AP_Frsky_Telem public: //constructor AP_Frsky_Telem(AP_AHRS &ahrs, AP_BattMonitor &battery) : + _port(NULL), + _protocol(FrSkyUnknown), _initialised(false), - _ahrs(ahrs), - _battery(battery) + _ahrs(ahrs), + _battery(battery) {} // these enums must match up with TELEM2_PROTOCOL in vehicle code enum FrSkyProtocol { + FrSkyUnknown = 0, FrSkyDPORT = 2, FrSkySPORT = 3 }; - void init(AP_HAL::UARTDriver *port, enum FrSkyProtocol protocol); + void init(const AP_SerialManager& serial_manager); void send_frames(uint8_t control_mode);