Mount: fix serial port used for 2nd serial driver

libraries/AP_Mount/AP_Mount.cpp

@@ -62,6 +62,9 @@ void AP_Mount::init()
     // primary is reset to the first instantiated mount
     bool primary_set = false;
 
+    // keep track of number of serial instances for initialisation
+    uint8_t serial_instance = 0;
+
     // create each instance
     for (uint8_t instance=0; instance<AP_MOUNT_MAX_INSTANCES; instance++) {
         switch (get_mount_type(instance)) {
@@ -98,8 +101,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_STORM32SERIAL_ENABLED
         // check for SToRM32 mounts using serial protocol
         case Type::SToRM32_serial:
-            _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_SToRM32_serial(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif
 
@@ -122,8 +126,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_SIYI_ENABLED
         // check for Siyi gimbal
         case Type::Siyi:
-            _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif // HAL_MOUNT_SIYI_ENABLED
 
@@ -146,8 +151,9 @@ void AP_Mount::init()
 #if HAL_MOUNT_VIEWPRO_ENABLED
         // check for Xacti gimbal
         case Type::Viewpro:
-            _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance);
+            _backends[instance] = new AP_Mount_Viewpro(*this, _params[instance], instance, serial_instance);
             _num_instances++;
+            serial_instance++;
             break;
 #endif // HAL_MOUNT_VIEWPRO_ENABLED
         }

libraries/AP_Mount/AP_Mount_Backend_Serial.cpp

@@ -0,0 +1,24 @@
+#include "AP_Mount_Backend_Serial.h"
+
+#if HAL_MOUNT_ENABLED
+#include <AP_SerialManager/AP_SerialManager.h>
+
+// Default init function for every mount
+void AP_Mount_Backend_Serial::init()
+{
+    const AP_SerialManager& serial_manager = AP::serialmanager();
+
+    // search for serial port.  hild classes should check that uart is not nullptr
+    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, _serial_instance);
+    if (_uart == nullptr) {
+        return;
+    }
+
+    // initialised successfully if uart is found
+    _initialised = true;
+
+    // call the parent class init
+    AP_Mount_Backend::init();
+}
+
+#endif // HAL_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_Backend_Serial.h

@@ -0,0 +1,48 @@
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  Mount driver backend class for serial drivers
+  Mounts using custom serial protocols should be derived from this class
+ */
+#pragma once
+
+#include "AP_Mount_config.h"
+
+#if HAL_MOUNT_ENABLED
+
+#include "AP_Mount_Backend.h"
+
+class AP_Mount_Backend_Serial : public AP_Mount_Backend
+{
+public:
+    // Constructor
+    AP_Mount_Backend_Serial(class AP_Mount &frontend, class AP_Mount_Params &params, uint8_t instance, uint8_t serial_instance) :
+        AP_Mount_Backend(frontend, params, instance),
+        _serial_instance(serial_instance)
+    {}
+
+    // perform any required initialisation for this instance
+    void init() override;
+
+protected:
+
+    // internal variables
+    AP_HAL::UARTDriver *_uart;      // uart connected to gimbal
+    uint8_t _serial_instance;       // this instance's serial instance number
+    bool _initialised;              // true if uart has been initialised
+};
+
+#endif // HAL_MOUNT_ENABLED

libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp

@@ -4,20 +4,6 @@
 #include <AP_HAL/AP_HAL.h>
 #include <GCS_MAVLink/GCS_MAVLink.h>
 #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
-#include <AP_SerialManager/AP_SerialManager.h>
-
-// init - performs any required initialisation for this instance
-void AP_Mount_SToRM32_serial::init()
-{
-    const AP_SerialManager& serial_manager = AP::serialmanager();
-
-    _port = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, 0);
-    if (_port) {
-        _initialised = true;
-    }
-    AP_Mount_Backend::init();
-
-}
 
 // update mount position - should be called periodically
 void AP_Mount_SToRM32_serial::update()
@@ -142,7 +128,7 @@ bool AP_Mount_SToRM32_serial::can_send(bool with_control) {
     if (with_control) {
         required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct);
     }
-    return (_reply_type == ReplyType_UNKNOWN) && (_port->txspace() >= required_tx);
+    return (_reply_type == ReplyType_UNKNOWN) && (_uart->txspace() >= required_tx);
 }
 
 
@@ -167,7 +153,7 @@ void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target
         return;
     }
 
-    if ((size_t)_port->txspace() < sizeof(cmd_set_angles_data)) {
+    if ((size_t)_uart->txspace() < sizeof(cmd_set_angles_data)) {
         return;
     }
 
@@ -181,7 +167,7 @@ void AP_Mount_SToRM32_serial::send_target_angles(const MountTarget& angle_target
     cmd_set_angles_data.crc = crc_calculate(&buf[1], sizeof(cmd_set_angles_data)-3);
 
     for (uint8_t i = 0;  i != sizeof(cmd_set_angles_data) ; i++) {
-        _port->write(buf[i]);
+        _uart->write(buf[i]);
     }
 
     // store time of send
@@ -194,11 +180,11 @@ void AP_Mount_SToRM32_serial::get_angles() {
         return;
     }
 
-    if (_port->txspace() < 1) {
+    if (_uart->txspace() < 1) {
         return;
     }
 
-    _port->write('d');
+    _uart->write('d');
 };
 
 
@@ -220,14 +206,14 @@ void AP_Mount_SToRM32_serial::read_incoming() {
     uint8_t data;
     int16_t numc;
 
-    numc = _port->available();
+    numc = _uart->available();
 
     if (numc < 0 ) {
         return;
     }
 
     for (int16_t i = 0; i < numc; i++) {        // Process bytes received
-        data = _port->read();
+        data = _uart->read();
         if (_reply_type == ReplyType_UNKNOWN) {
             continue;
         }

libraries/AP_Mount/AP_Mount_SToRM32_serial.h

@@ -3,7 +3,7 @@
  */
 #pragma once
 
-#include "AP_Mount_Backend.h"
+#include "AP_Mount_Backend_Serial.h"
 
 #if HAL_MOUNT_STORM32SERIAL_ENABLED
 
@@ -13,15 +13,12 @@
 
 #define AP_MOUNT_STORM32_SERIAL_RESEND_MS   1000    // resend angle targets to gimbal once per second
 
-class AP_Mount_SToRM32_serial : public AP_Mount_Backend
+class AP_Mount_SToRM32_serial : public AP_Mount_Backend_Serial
 {
 
 public:
     // Constructor
-    using AP_Mount_Backend::AP_Mount_Backend;
-
-    // init - performs any required initialisation for this instance
-    void init() override;
+    using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
 
     // update mount position - should be called periodically
     void update() override;
@@ -127,11 +124,7 @@ class AP_Mount_SToRM32_serial : public AP_Mount_Backend
 
 
     // internal variables
-    AP_HAL::UARTDriver *_port;
-
-    bool _initialised;              // true once the driver has been initialised
     uint32_t _last_send;            // system time of last do_mount_control sent to gimbal
-
     uint8_t _reply_length;
     uint8_t _reply_counter;
     ReplyType _reply_type = ReplyType_UNKNOWN;

libraries/AP_Mount/AP_Mount_Siyi.cpp

@@ -5,7 +5,6 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
-#include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_RTC/AP_RTC.h>
 
 extern const AP_HAL::HAL& hal;
@@ -32,20 +31,6 @@ const AP_Mount_Siyi::HWInfo AP_Mount_Siyi::hardware_lookup_table[] {
         {{'7','A'}, "ZT30"},
 };
 
-// init - performs any required initialisation for this instance
-void AP_Mount_Siyi::init()
-{
-    const AP_SerialManager& serial_manager = AP::serialmanager();
-
-    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, 0);
-    if (_uart == nullptr) {
-        return;
-    }
-
-    _initialised = true;
-    AP_Mount_Backend::init();
-}
-
 // update mount position - should be called periodically
 void AP_Mount_Siyi::update()
 {

libraries/AP_Mount/AP_Mount_Siyi.h

@@ -19,7 +19,7 @@
 
 #pragma once
 
-#include "AP_Mount_Backend.h"
+#include "AP_Mount_Backend_Serial.h"
 
 #if HAL_MOUNT_SIYI_ENABLED
 
@@ -29,19 +29,16 @@
 
 #define AP_MOUNT_SIYI_PACKETLEN_MAX     38  // maximum number of bytes in a packet sent to or received from the gimbal
 
-class AP_Mount_Siyi : public AP_Mount_Backend
+class AP_Mount_Siyi : public AP_Mount_Backend_Serial
 {
 
 public:
     // Constructor
-    using AP_Mount_Backend::AP_Mount_Backend;
+    using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
 
     /* Do not allow copies */
     CLASS_NO_COPY(AP_Mount_Siyi);
 
-    // init - performs any required initialisation for this instance
-    void init() override;
-
     // update mount position - should be called periodically
     void update() override;
 
@@ -293,8 +290,6 @@ class AP_Mount_Siyi : public AP_Mount_Backend
     void check_firmware_version() const;
 
     // internal variables
-    AP_HAL::UARTDriver *_uart;                      // uart connected to gimbal
-    bool _initialised;                              // true once the driver has been initialised
     bool _got_hardware_id;                          // true once hardware id ha been received
 
     FirmwareVersion _fw_version;                    // firmware version (for reporting for GCS)

libraries/AP_Mount/AP_Mount_Viewpro.cpp

@@ -7,7 +7,6 @@
 #include <AP_RTC/AP_RTC.h>
 #include <GCS_MAVLink/GCS.h>
 #include <GCS_MAVLink/include/mavlink/v2.0/checksum.h>
-#include <AP_SerialManager/AP_SerialManager.h>
 
 extern const AP_HAL::HAL& hal;
 
@@ -28,20 +27,6 @@ extern const AP_HAL::HAL& hal;
 
 const char* AP_Mount_Viewpro::send_text_prefix = "Viewpro:";
 
-// init - performs any required initialisation for this instance
-void AP_Mount_Viewpro::init()
-{
-    const AP_SerialManager& serial_manager = AP::serialmanager();
-
-    _uart = serial_manager.find_serial(AP_SerialManager::SerialProtocol_Gimbal, 0);
-    if (_uart == nullptr) {
-        return;
-    }
-
-    _initialised = true;
-    AP_Mount_Backend::init();
-}
-
 // update mount position - should be called periodically
 void AP_Mount_Viewpro::update()
 {

libraries/AP_Mount/AP_Mount_Viewpro.h

@@ -16,7 +16,7 @@
 
 #pragma once
 
-#include "AP_Mount_Backend.h"
+#include "AP_Mount_Backend_Serial.h"
 
 #if HAL_MOUNT_VIEWPRO_ENABLED
 
@@ -27,19 +27,16 @@
 
 #define AP_MOUNT_VIEWPRO_PACKETLEN_MAX  63  // maximum number of bytes in a packet sent to or received from the gimbal
 
-class AP_Mount_Viewpro : public AP_Mount_Backend
+class AP_Mount_Viewpro : public AP_Mount_Backend_Serial
 {
 
 public:
     // Constructor
-    using AP_Mount_Backend::AP_Mount_Backend;
+    using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
 
     /* Do not allow copies */
     CLASS_NO_COPY(AP_Mount_Viewpro);
 
-    // init - performs any required initialisation for this instance
-    void init() override;
-
     // update mount position - should be called periodically
     void update() override;
 
@@ -378,8 +375,6 @@ class AP_Mount_Viewpro : public AP_Mount_Backend
     bool send_m_ahrs();
 
     // internal variables
-    AP_HAL::UARTDriver *_uart;                      // uart connected to gimbal
-    bool _initialised;                              // true once the driver has been initialised
     uint8_t _msg_buff[AP_MOUNT_VIEWPRO_PACKETLEN_MAX];  // buffer holding latest bytes from gimbal
     uint8_t _msg_buff_len;                          // number of bytes held in msg buff
     const uint8_t _msg_buff_data_start = 2;         // data starts at this byte of _msg_buff