diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp index 84ec16f7ea..237ab0e89d 100644 --- a/Tools/AP_Periph/AP_Periph.cpp +++ b/Tools/AP_Periph/AP_Periph.cpp @@ -94,11 +94,15 @@ void AP_Periph_FW::init() } #ifdef HAL_PERIPH_ENABLE_GPS - gps.init(serial_manager); + if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE) { + gps.init(serial_manager); + } #endif #ifdef HAL_PERIPH_ENABLE_MAG - compass.init(); + if (compass.enabled()) { + compass.init(); + } #endif #ifdef HAL_PERIPH_ENABLE_BARO @@ -115,14 +119,18 @@ void AP_Periph_FW::init() #endif #ifdef HAL_PERIPH_ENABLE_AIRSPEED - airspeed.init(); + if (airspeed.enabled()) { + airspeed.init(); + } #endif #ifdef HAL_PERIPH_ENABLE_RANGEFINDER - const uint8_t sernum = 3; // uartB - hal.uartB->begin(g.rangefinder_baud); - serial_manager.set_protocol_and_baud(sernum, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud); - rangefinder.init(ROTATION_NONE); + if (rangefinder.get_type(0) != RangeFinder::Type::NONE) { + const uint8_t sernum = 3; // uartB + hal.uartB->begin(g.rangefinder_baud); + serial_manager.set_protocol_and_baud(sernum, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud); + rangefinder.init(ROTATION_NONE); + } #endif #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp index e0b26cf1f4..b3966acd46 100644 --- a/Tools/AP_Periph/Parameters.cpp +++ b/Tools/AP_Periph/Parameters.cpp @@ -6,6 +6,10 @@ extern const AP_HAL::HAL &hal; #define AP_PERIPH_LED_BRIGHT_DEFAULT 100 #endif +#ifndef HAL_PERIPH_ADSB_BAUD_DEFAULT +#define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600 +#endif + /* * AP_Periph parameter definitions * @@ -83,7 +87,7 @@ const AP_Param::Info AP_Periph_FW::var_info[] = { #endif #ifdef HAL_PERIPH_ENABLE_ADSB - GSCALAR(adsb_baudrate, "ADSB_BAUDRATE", 57600), + GSCALAR(adsb_baudrate, "ADSB_BAUDRATE", HAL_PERIPH_ADSB_BAUD_DEFAULT), #endif #ifdef HAL_PERIPH_ENABLE_PWM_HARDPOINT diff --git a/Tools/AP_Periph/adsb.cpp b/Tools/AP_Periph/adsb.cpp index 5212482fba..018ab3eb26 100644 --- a/Tools/AP_Periph/adsb.cpp +++ b/Tools/AP_Periph/adsb.cpp @@ -31,7 +31,9 @@ extern const AP_HAL::HAL &hal; */ void AP_Periph_FW::adsb_init(void) { - ADSB_PORT->begin(AP_SerialManager::map_baudrate(g.adsb_baudrate), 256, 256); + if (g.adsb_baudrate > 0) { + ADSB_PORT->begin(AP_SerialManager::map_baudrate(g.adsb_baudrate), 256, 256); + } } @@ -40,6 +42,9 @@ void AP_Periph_FW::adsb_init(void) */ void AP_Periph_FW::adsb_update(void) { + if (g.adsb_baudrate <= 0) { + return; + } // look for incoming MAVLink ADSB_VEHICLE packets const uint16_t nbytes = ADSB_PORT->available(); for (uint16_t i=0; i