AP_Periph: added MSP_PORT parameter

allows MSP to be enabled/disabled

Tools/AP_Periph/AP_Periph.cpp

@@ -106,7 +106,7 @@ void AP_Periph_FW::init()
     }
 
 #ifdef HAL_PERIPH_ENABLE_GPS
-    if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+    if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE && g.gps_port >= 0) {
         serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, AP_SERIALMANAGER_GPS_BAUD);
         gps.init(serial_manager);
     }
@@ -149,7 +149,7 @@ void AP_Periph_FW::init()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
-    if (rangefinder.get_type(0) != RangeFinder::Type::NONE) {
+    if (rangefinder.get_type(0) != RangeFinder::Type::NONE && g.rangefinder_port >= 0) {
         auto *uart = hal.serial(g.rangefinder_port);
         if (uart != nullptr) {
             uart->begin(g.rangefinder_baud);
@@ -168,7 +168,9 @@ void AP_Periph_FW::init()
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_MSP
-    msp_init(hal.serial(2));
+    if (g.msp_port >= 0) {
+        msp_init(hal.serial(g.msp_port));
+    }
 #endif
     
     start_ms = AP_HAL::native_millis();

Tools/AP_Periph/Parameters.cpp

@@ -25,6 +25,10 @@ extern const AP_HAL::HAL &hal;
 #define HAL_PERIPH_ADSB_PORT_DEFAULT 1
 #endif
 
+#ifndef AP_PERIPH_MSP_PORT_DEFAULT
+#define AP_PERIPH_MSP_PORT_DEFAULT 1
+#endif
+
 /*
  *  AP_Periph parameter definitions
  *
@@ -140,6 +144,10 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     GOBJECT(servo_channels, "OUT",     SRV_Channels),
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_MSP
+    GSCALAR(msp_port, "MSP_PORT", AP_PERIPH_MSP_PORT_DEFAULT),
+#endif
+    
     AP_VAREND
 };
 

Tools/AP_Periph/Parameters.h

@@ -35,6 +35,7 @@ public:
         k_param_servo_channels,
         k_param_rangefinder_port,
         k_param_gps_port,
+        k_param_msp_port,
     };
 
     AP_Int16 format_version;
@@ -76,6 +77,10 @@ public:
     AP_Int8 gps_port;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_MSP
+    AP_Int8 msp_port;
+#endif
+    
     AP_Int8 debug;
 
     AP_Int32 serial_number;

Tools/AP_Periph/msp.cpp

@@ -39,6 +39,9 @@ void AP_Periph_FW::send_msp_packet(uint16_t cmd, void *p, uint16_t size)
  */
 void AP_Periph_FW::msp_sensor_update(void)
 {
+    if (msp.port.uart == nullptr) {
+        return;
+    }
 #ifdef HAL_PERIPH_ENABLE_GPS
     send_msp_GPS();
 #endif