AP_Mount: SToRM32_serial inherits from serial backend

This commit is contained in:
Randy Mackay 2024-05-22 12:41:17 +09:00 committed by Andrew Tridgell
parent 7fc00efe7e
commit 99626b73ff
2 changed files with 10 additions and 31 deletions

View File

@ -4,20 +4,6 @@
#include <AP_HAL/AP_HAL.h> #include <AP_HAL/AP_HAL.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <GCS_MAVLink/include/mavlink/v2.0/checksum.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 // update mount position - should be called periodically
void AP_Mount_SToRM32_serial::update() void AP_Mount_SToRM32_serial::update()
@ -142,7 +128,7 @@ bool AP_Mount_SToRM32_serial::can_send(bool with_control) {
if (with_control) { if (with_control) {
required_tx += sizeof(AP_Mount_SToRM32_serial::cmd_set_angles_struct); 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; return;
} }
if ((size_t)_port->txspace() < sizeof(cmd_set_angles_data)) { if ((size_t)_uart->txspace() < sizeof(cmd_set_angles_data)) {
return; 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); 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++) { for (uint8_t i = 0; i != sizeof(cmd_set_angles_data) ; i++) {
_port->write(buf[i]); _uart->write(buf[i]);
} }
// store time of send // store time of send
@ -194,11 +180,11 @@ void AP_Mount_SToRM32_serial::get_angles() {
return; return;
} }
if (_port->txspace() < 1) { if (_uart->txspace() < 1) {
return; return;
} }
_port->write('d'); _uart->write('d');
}; };
@ -220,14 +206,14 @@ void AP_Mount_SToRM32_serial::read_incoming() {
uint8_t data; uint8_t data;
int16_t numc; int16_t numc;
numc = _port->available(); numc = _uart->available();
if (numc < 0 ) { if (numc < 0 ) {
return; return;
} }
for (int16_t i = 0; i < numc; i++) { // Process bytes received for (int16_t i = 0; i < numc; i++) { // Process bytes received
data = _port->read(); data = _uart->read();
if (_reply_type == ReplyType_UNKNOWN) { if (_reply_type == ReplyType_UNKNOWN) {
continue; continue;
} }

View File

@ -3,7 +3,7 @@
*/ */
#pragma once #pragma once
#include "AP_Mount_Backend.h" #include "AP_Mount_Backend_Serial.h"
#if HAL_MOUNT_STORM32SERIAL_ENABLED #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 #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: public:
// Constructor // Constructor
using AP_Mount_Backend::AP_Mount_Backend; using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
// init - performs any required initialisation for this instance
void init() override;
// update mount position - should be called periodically // update mount position - should be called periodically
void update() override; void update() override;
@ -127,11 +124,7 @@ private:
// internal variables // 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 uint32_t _last_send; // system time of last do_mount_control sent to gimbal
uint8_t _reply_length; uint8_t _reply_length;
uint8_t _reply_counter; uint8_t _reply_counter;
ReplyType _reply_type = ReplyType_UNKNOWN; ReplyType _reply_type = ReplyType_UNKNOWN;