From 5dfdca39d51ecb76c8586c7b0132c97e008bfe11 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 10:12:22 +0900 Subject: [PATCH 1/5] AP_Mount: add Backend_Serial --- .../AP_Mount/AP_Mount_Backend_Serial.cpp | 24 ++++++++++ libraries/AP_Mount/AP_Mount_Backend_Serial.h | 48 +++++++++++++++++++ 2 files changed, 72 insertions(+) create mode 100644 libraries/AP_Mount/AP_Mount_Backend_Serial.cpp create mode 100644 libraries/AP_Mount/AP_Mount_Backend_Serial.h diff --git a/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp new file mode 100644 index 0000000000000..ec27051057565 --- /dev/null +++ b/libraries/AP_Mount/AP_Mount_Backend_Serial.cpp @@ -0,0 +1,24 @@ +#include "AP_Mount_Backend_Serial.h" + +#if HAL_MOUNT_ENABLED +#include + +// 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 diff --git a/libraries/AP_Mount/AP_Mount_Backend_Serial.h b/libraries/AP_Mount/AP_Mount_Backend_Serial.h new file mode 100644 index 0000000000000..49fd2a50bdeb7 --- /dev/null +++ b/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 . + */ + +/* + 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 ¶ms, 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 From 38e8dda4f90b2bee7af2b7ae3ca16454847b37cb Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 10:13:27 +0900 Subject: [PATCH 2/5] AP_Mount: Siyi inherits from serial backend --- libraries/AP_Mount/AP_Mount_Siyi.cpp | 15 --------------- libraries/AP_Mount/AP_Mount_Siyi.h | 11 +++-------- 2 files changed, 3 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp index 3f00d1caa03e3..77704637dc99c 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.cpp +++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp @@ -5,7 +5,6 @@ #include #include #include -#include #include 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() { diff --git a/libraries/AP_Mount/AP_Mount_Siyi.h b/libraries/AP_Mount/AP_Mount_Siyi.h index 75a03efda3271..c6366e6b29d04 100644 --- a/libraries/AP_Mount/AP_Mount_Siyi.h +++ b/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) From f7b394f791db3350a6d0c9833f8da55ca45afc63 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 12:41:01 +0900 Subject: [PATCH 3/5] AP_Mount: Viewpro inherits from serial backend --- libraries/AP_Mount/AP_Mount_Viewpro.cpp | 15 --------------- libraries/AP_Mount/AP_Mount_Viewpro.h | 11 +++-------- 2 files changed, 3 insertions(+), 23 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.cpp b/libraries/AP_Mount/AP_Mount_Viewpro.cpp index 409d9cc27b4e8..e04510962ef07 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.cpp +++ b/libraries/AP_Mount/AP_Mount_Viewpro.cpp @@ -7,7 +7,6 @@ #include #include #include -#include 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() { diff --git a/libraries/AP_Mount/AP_Mount_Viewpro.h b/libraries/AP_Mount/AP_Mount_Viewpro.h index 1ddaa283212be..1e045c8fd1ae2 100644 --- a/libraries/AP_Mount/AP_Mount_Viewpro.h +++ b/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 From 20b2f8329bf1aae4c69042a7f594c5d288519dff Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 12:41:17 +0900 Subject: [PATCH 4/5] AP_Mount: SToRM32_serial inherits from serial backend --- .../AP_Mount/AP_Mount_SToRM32_serial.cpp | 28 +++++-------------- libraries/AP_Mount/AP_Mount_SToRM32_serial.h | 13 ++------- 2 files changed, 10 insertions(+), 31 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp index 928a40a297496..82387a0e93ec1 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp +++ b/libraries/AP_Mount/AP_Mount_SToRM32_serial.cpp @@ -4,20 +4,6 @@ #include #include #include -#include - -// 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; } diff --git a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h b/libraries/AP_Mount/AP_Mount_SToRM32_serial.h index e6632ca10eb8b..895143e80bb72 100644 --- a/libraries/AP_Mount/AP_Mount_SToRM32_serial.h +++ b/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; From 6548d4e745a168e4442785aaaf1e14fd5c465aa5 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 12:41:57 +0900 Subject: [PATCH 5/5] AP_Mount: serial backends gets instance --- libraries/AP_Mount/AP_Mount.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp index 70a136283d596..58837ca8792df 100644 --- a/libraries/AP_Mount/AP_Mount.cpp +++ b/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