From 37506c2f3acfa9257fe7cd522d737fe77b2a8547 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 27 Dec 2020 10:35:15 +1100
Subject: [PATCH] AP_Periph: added MSP_PORT parameter

allows MSP to be enabled/disabled
---
 Tools/AP_Periph/AP_Periph.cpp  | 8 +++++---
 Tools/AP_Periph/Parameters.cpp | 8 ++++++++
 Tools/AP_Periph/Parameters.h   | 5 +++++
 Tools/AP_Periph/msp.cpp        | 3 +++
 4 files changed, 21 insertions(+), 3 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 77e58ca2c3..31361a4a43 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/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();
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index f9fd408254..8993591422 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/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
 };
 
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index b506a6b3a3..b752b44e1b 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/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;
diff --git a/Tools/AP_Periph/msp.cpp b/Tools/AP_Periph/msp.cpp
index 02492ea540..5f5092a053 100644
--- a/Tools/AP_Periph/msp.cpp
+++ b/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