From 7af731fbda37df7e07e0f410381a0cba1a4455a5 Mon Sep 17 00:00:00 2001
From: Tom Pittenger <tom.pittenger@krausaerospace.com>
Date: Tue, 22 Dec 2020 15:29:59 -0800
Subject: [PATCH] AP_Periph: change GPS and Rangefinder to use port stored in
 param

---
 Tools/AP_Periph/AP_Periph.cpp  | 11 +++++++----
 Tools/AP_Periph/Parameters.cpp | 26 +++++++++++++++++++++++---
 Tools/AP_Periph/Parameters.h   |  7 +++++++
 3 files changed, 37 insertions(+), 7 deletions(-)

diff --git a/Tools/AP_Periph/AP_Periph.cpp b/Tools/AP_Periph/AP_Periph.cpp
index 812af67b55..77e58ca2c3 100644
--- a/Tools/AP_Periph/AP_Periph.cpp
+++ b/Tools/AP_Periph/AP_Periph.cpp
@@ -107,6 +107,7 @@ void AP_Periph_FW::init()
 
 #ifdef HAL_PERIPH_ENABLE_GPS
     if (gps.get_type(0) != AP_GPS::GPS_Type::GPS_TYPE_NONE) {
+        serial_manager.set_protocol_and_baud(g.gps_port, AP_SerialManager::SerialProtocol_GPS, AP_SERIALMANAGER_GPS_BAUD);
         gps.init(serial_manager);
     }
 #endif
@@ -149,10 +150,12 @@ void AP_Periph_FW::init()
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     if (rangefinder.get_type(0) != RangeFinder::Type::NONE) {
-        const uint8_t sernum = 3; // uartB
-        hal.serial(3)->begin(g.rangefinder_baud);
-        serial_manager.set_protocol_and_baud(sernum, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
-        rangefinder.init(ROTATION_NONE);
+        auto *uart = hal.serial(g.rangefinder_port);
+        if (uart != nullptr) {
+            uart->begin(g.rangefinder_baud);
+            serial_manager.set_protocol_and_baud(g.rangefinder_port, AP_SerialManager::SerialProtocol_Rangefinder, g.rangefinder_baud);
+            rangefinder.init(ROTATION_NONE);
+        }
     }
 #endif
 
diff --git a/Tools/AP_Periph/Parameters.cpp b/Tools/AP_Periph/Parameters.cpp
index 3c6cb70b0d..2d4a9c749a 100644
--- a/Tools/AP_Periph/Parameters.cpp
+++ b/Tools/AP_Periph/Parameters.cpp
@@ -6,6 +6,18 @@ extern const AP_HAL::HAL &hal;
 #define AP_PERIPH_LED_BRIGHT_DEFAULT 100
 #endif
 
+#ifndef AP_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT
+#define AP_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT 115200
+#endif
+
+#ifndef AP_PERIPH_RANGEFINDER_PORT_DEFAULT
+#define AP_PERIPH_RANGEFINDER_PORT_DEFAULT 3
+#endif
+
+#ifndef HAL_PERIPH_GPS_PORT_DEFAULT
+#define HAL_PERIPH_GPS_PORT_DEFAULT 3
+#endif
+
 #ifndef HAL_PERIPH_ADSB_BAUD_DEFAULT
 #define HAL_PERIPH_ADSB_BAUD_DEFAULT 57600
 #endif
@@ -55,6 +67,15 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
     // @Group: GPS_
     // @Path: ../../libraries/AP_GPS/AP_GPS.cpp
     GOBJECT(gps, "GPS_", AP_GPS),
+
+    // @Param: GPS_PORT
+    // @DisplayName: GPS Serial Port
+    // @Description: This is the serial port number where SERIALx_PROTOCOL will be set to GPS.
+    // @Range: 0 10
+    // @Increment: 1
+    // @User: Advanced
+    // @RebootRequired: True
+    GSCALAR(gps_port, "GPS_PORT", HAL_PERIPH_GPS_PORT_DEFAULT),
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_BATTERY
@@ -89,10 +110,9 @@ const AP_Param::Info AP_Periph_FW::var_info[] = {
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
-    GSCALAR(rangefinder_baud, "RNGFND_BAUDRATE", 115200),
-#endif
+    GSCALAR(rangefinder_baud, "RNGFND_BAUDRATE", AP_PERIPH_RANGEFINDER_BAUDRATE_DEFAULT),
+    GSCALAR(rangefinder_port, "RNGFND_PORT", AP_PERIPH_RANGEFINDER_PORT_DEFAULT),
 
-#ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     // Rangefinder driver
     // @Group: RNGFND
     // @Path: ../../libraries/AP_RangeFinder/Rangefinder.cpp
diff --git a/Tools/AP_Periph/Parameters.h b/Tools/AP_Periph/Parameters.h
index 0fb5501cd1..b506a6b3a3 100644
--- a/Tools/AP_Periph/Parameters.h
+++ b/Tools/AP_Periph/Parameters.h
@@ -33,6 +33,8 @@ public:
         k_param_serial_number,
         k_param_adsb_port,
         k_param_servo_channels,
+        k_param_rangefinder_port,
+        k_param_gps_port,
     };
 
     AP_Int16 format_version;
@@ -53,6 +55,7 @@ public:
 
 #ifdef HAL_PERIPH_ENABLE_RANGEFINDER
     AP_Int32 rangefinder_baud;
+    AP_Int8 rangefinder_port;
 #endif
 
 #ifdef HAL_PERIPH_ENABLE_ADSB
@@ -69,6 +72,10 @@ public:
     AP_Int8 esc_number;
 #endif
 
+#ifdef HAL_PERIPH_ENABLE_GPS
+    AP_Int8 gps_port;
+#endif
+
     AP_Int8 debug;
 
     AP_Int32 serial_number;