AP_Periph: added enable mechanisms for all sensor types

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

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

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<nbytes; i++) {

Tools/AP_Periph/can.cpp

@@ -1139,6 +1139,9 @@ void AP_Periph_FW::can_update()
 void AP_Periph_FW::can_mag_update(void)
 {
 #ifdef HAL_PERIPH_ENABLE_MAG
+    if (!compass.enabled()) {
+        return;
+    }
     compass.read();
 #if CAN_PROBE_CONTINUOUS
     if (compass.get_count() == 0) {
@@ -1184,6 +1187,9 @@ void AP_Periph_FW::can_mag_update(void)
 void AP_Periph_FW::can_gps_update(void)
 {
 #ifdef HAL_PERIPH_ENABLE_GPS
+    if (gps.get_type(0) == AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+        return;
+    }
     gps.update();
     if (last_gps_update_ms == gps.last_message_time_ms()) {
         return;
@@ -1450,6 +1456,9 @@ void AP_Periph_FW::can_baro_update(void)
 void AP_Periph_FW::can_airspeed_update(void)
 {
 #ifdef HAL_PERIPH_ENABLE_AIRSPEED
+    if (!airspeed.enabled()) {
+        return;
+    }
 #if CAN_PROBE_CONTINUOUS
     if (!airspeed.healthy()) {
         uint32_t now = AP_HAL::millis();
@@ -1511,6 +1520,9 @@ void AP_Periph_FW::can_airspeed_update(void)
 void AP_Periph_FW::can_rangefinder_update(void)
 {
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
+    if (rangefinder.get_type(0) == RangeFinder::Type::NONE) {
+        return;
+    }
     if (rangefinder.num_sensors() == 0) {
         uint32_t now = AP_HAL::millis();
         static uint32_t last_probe_ms;