From 99626b73ff0867d05e5bc246ed52e7ae2df45280 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 22 May 2024 12:41:17 +0900 Subject: [PATCH] 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 928a40a297..82387a0e93 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 e6632ca10e..895143e80b 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 @@ private: // 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;