mirror of https://github.com/ArduPilot/ardupilot
AP_Mount: SToRM32_serial inherits from serial backend
This commit is contained in:
parent
7fc00efe7e
commit
99626b73ff
|
@ -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;
|
||||||
}
|
}
|
||||||
|
|
|
@ -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;
|
||||||
|
|
Loading…
Reference in New Issue