mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_Mount: add Siyi gimbal driver
This commit is contained in:
Randy Mackay
Andrew Tridgell
parent
f6056a4734
commit
80da0c541b
@ -11,6 +11,7 @@
|
||||
#include "AP_Mount_SToRM32.h"
|
||||
#include "AP_Mount_SToRM32_serial.h"
|
||||
#include "AP_Mount_Gremsy.h"
|
||||
#include "AP_Mount_Siyi.h"
|
||||
#include <stdio.h>
|
||||
#include <AP_Math/location.h>
|
||||
#include <SRV_Channel/SRV_Channel.h>
|
||||
@ -109,6 +110,14 @@ void AP_Mount::init()
|
||||
_backends[instance] = new AP_Mount_Servo(*this, _params[instance], false, instance);
|
||||
_num_instances++;
|
||||
#endif
|
||||
|
||||
#if HAL_MOUNT_SIYI_ENABLED
|
||||
// check for Siyi gimbal
|
||||
} else if (mount_type == Mount_Type_Siyi) {
|
||||
_backends[instance] = new AP_Mount_Siyi(*this, _params[instance], instance);
|
||||
_num_instances++;
|
||||
#endif // HAL_MOUNT_SIYI_ENABLED
|
||||
|
||||
}
|
||||
|
||||
// init new instance
|
||||
|
||||
@ -48,6 +48,7 @@ class AP_Mount_Alexmos;
|
||||
class AP_Mount_SToRM32;
|
||||
class AP_Mount_SToRM32_serial;
|
||||
class AP_Mount_Gremsy;
|
||||
class AP_Mount_Siyi;
|
||||
|
||||
/*
|
||||
This is a workaround to allow the MAVLink backend access to the
|
||||
@ -64,6 +65,7 @@ class AP_Mount
|
||||
friend class AP_Mount_SToRM32;
|
||||
friend class AP_Mount_SToRM32_serial;
|
||||
friend class AP_Mount_Gremsy;
|
||||
friend class AP_Mount_Siyi;
|
||||
|
||||
public:
|
||||
AP_Mount();
|
||||
@ -86,7 +88,8 @@ public:
|
||||
Mount_Type_SToRM32 = 4, /// SToRM32 mount using MAVLink protocol
|
||||
Mount_Type_SToRM32_serial = 5, /// SToRM32 mount using custom serial protocol
|
||||
Mount_Type_Gremsy = 6, /// Gremsy gimbal using MAVLink v2 Gimbal protocol
|
||||
Mount_Type_BrushlessPWM = 7 /// Brushless (stabilized) gimbal using PWM protocol
|
||||
Mount_Type_BrushlessPWM = 7, /// Brushless (stabilized) gimbal using PWM protocol
|
||||
Mount_Type_Siyi = 8, /// Siyi gimbal using custom serial protocol
|
||||
};
|
||||
|
||||
// init - detect and initialise all mounts
|
||||
|
||||
@ -9,7 +9,7 @@ const AP_Param::GroupInfo AP_Mount_Params::var_info[] = {
|
||||
// @Param: _TYPE
|
||||
// @DisplayName: Mount Type
|
||||
// @Description: Mount Type
|
||||
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM
|
||||
// @Values: 0:None, 1:Servo, 2:3DR Solo, 3:Alexmos Serial, 4:SToRM32 MAVLink, 5:SToRM32 Serial, 6:Gremsy, 7:BrushlessPWM, 8:Siyi
|
||||
// @RebootRequired: True
|
||||
// @User: Standard
|
||||
AP_GROUPINFO_FLAGS("_TYPE", 1, AP_Mount_Params, type, 0, AP_PARAM_FLAG_ENABLE),
|
||||
|
||||
627
libraries/AP_Mount/AP_Mount_Siyi.cpp
Normal file
627
libraries/AP_Mount/AP_Mount_Siyi.cpp
Normal file
@ -0,0 +1,627 @@
|
||||
#include "AP_Mount_Siyi.h"
|
||||
|
||||
#if HAL_MOUNT_SIYI_ENABLED
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#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>
|
||||
|
||||
extern const AP_HAL::HAL& hal;
|
||||
|
||||
#define AP_MOUNT_SIYI_HEADER1 0x55 // first header byte
|
||||
#define AP_MOUNT_SIYI_HEADER2 0x66 // second header byte
|
||||
#define AP_MOUNT_SIYI_PACKETLEN_MIN 10 // minimum number of bytes in a packet. this is a packet with no data bytes
|
||||
#define AP_MOUNT_SIYI_DATALEN_MAX (AP_MOUNT_SIYI_PACKETLEN_MAX-AP_MOUNT_SIYI_PACKETLEN_MIN) // max bytes for data portion of packet
|
||||
#define AP_MOUNT_SIYI_SERIAL_RESEND_MS 1000 // resend angle targets to gimbal once per second
|
||||
#define AP_MOUNT_SIYI_MSG_BUF_DATA_START 8 // data starts at this byte in _msg_buf
|
||||
#define AP_MOUNT_SIYI_RATE_MAX_RADS radians(90) // maximum physical rotation rate of gimbal in radans/sec
|
||||
#define AP_MOUNT_SIYI_PITCH_P 1.50 // pitch controller P gain (converts pitch angle error to target rate)
|
||||
#define AP_MOUNT_SIYI_YAW_P 1.50 // yaw controller P gain (converts yaw angle error to target rate)
|
||||
#define AP_MOUNT_SIYI_LOCK_RESEND_COUNT 5 // lock value is resent to gimbal every 5 iterations
|
||||
|
||||
#define AP_MOUNT_SIYI_DEBUG 0
|
||||
#define debug(fmt, args ...) do { if (AP_MOUNT_SIYI_DEBUG) { GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Siyi: " fmt, ## args); } } while (0)
|
||||
|
||||
// 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_SToRM32, 0);
|
||||
if (_uart != nullptr) {
|
||||
_initialised = true;
|
||||
set_mode((enum MAV_MOUNT_MODE)_params.default_mode.get());
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// update mount position - should be called periodically
|
||||
void AP_Mount_Siyi::update()
|
||||
{
|
||||
// exit immediately if not initialised
|
||||
if (!_initialised) {
|
||||
return;
|
||||
}
|
||||
|
||||
// reading incoming packets from gimbal
|
||||
read_incoming_packets();
|
||||
|
||||
// request firmware version at 1hz
|
||||
uint32_t now_ms = AP_HAL::millis();
|
||||
if (!_got_firmware_version) {
|
||||
if ((now_ms - _last_send_ms) >= 1000) {
|
||||
request_firmware_version();
|
||||
_last_send_ms = now_ms;
|
||||
}
|
||||
return;
|
||||
}
|
||||
|
||||
// request attitude at regular intervals
|
||||
if ((now_ms - _last_req_current_angle_rad_ms) >= 50) {
|
||||
request_gimbal_attitude();
|
||||
_last_req_current_angle_rad_ms = now_ms;
|
||||
}
|
||||
|
||||
// update based on mount mode
|
||||
switch (get_mode()) {
|
||||
// move mount to a "retracted" position. To-Do: remove support and replace with a relaxed mode?
|
||||
case MAV_MOUNT_MODE_RETRACT: {
|
||||
const Vector3f &angle_bf_target = _params.retract_angles.get();
|
||||
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
|
||||
break;
|
||||
}
|
||||
|
||||
// move mount to a neutral position, typically pointing forward
|
||||
case MAV_MOUNT_MODE_NEUTRAL: {
|
||||
const Vector3f &angle_bf_target = _params.neutral_angles.get();
|
||||
send_target_angles(ToRad(angle_bf_target.y), ToRad(angle_bf_target.z), false);
|
||||
break;
|
||||
}
|
||||
|
||||
// point to the angles given by a mavlink message
|
||||
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
|
||||
switch (mavt_target.target_type) {
|
||||
case MountTargetType::ANGLE:
|
||||
send_target_angles(mavt_target.angle_rad.pitch, mavt_target.angle_rad.yaw, mavt_target.angle_rad.yaw_is_ef);
|
||||
break;
|
||||
case MountTargetType::RATE:
|
||||
send_target_rates(mavt_target.rate_rads.pitch, mavt_target.rate_rads.yaw, mavt_target.rate_rads.yaw_is_ef);
|
||||
break;
|
||||
}
|
||||
break;
|
||||
|
||||
// RC radio manual angle control, but with stabilization from the AHRS
|
||||
case MAV_MOUNT_MODE_RC_TARGETING: {
|
||||
// update targets using pilot's rc inputs
|
||||
MountTarget rc_target {};
|
||||
if (get_rc_rate_target(rc_target)) {
|
||||
send_target_rates(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
|
||||
} else if (get_rc_angle_target(rc_target)) {
|
||||
send_target_angles(rc_target.pitch, rc_target.yaw, rc_target.yaw_is_ef);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
// point mount to a GPS point given by the mission planner
|
||||
case MAV_MOUNT_MODE_GPS_POINT: {
|
||||
MountTarget angle_target_rad {};
|
||||
if (get_angle_target_to_roi(angle_target_rad)) {
|
||||
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MOUNT_MODE_HOME_LOCATION: {
|
||||
MountTarget angle_target_rad {};
|
||||
if (get_angle_target_to_home(angle_target_rad)) {
|
||||
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case MAV_MOUNT_MODE_SYSID_TARGET:{
|
||||
MountTarget angle_target_rad {};
|
||||
if (get_angle_target_to_sysid(angle_target_rad)) {
|
||||
send_target_angles(angle_target_rad.pitch, angle_target_rad.yaw, angle_target_rad.yaw_is_ef);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
// we do not know this mode so raise internal error
|
||||
INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
// return true if healthy
|
||||
bool AP_Mount_Siyi::healthy() const
|
||||
{
|
||||
// unhealthy until gimbal has been found and replied with firmware version info
|
||||
if (!_initialised || !_got_firmware_version) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// unhealthy if attitude information NOT received recently
|
||||
if (AP_HAL::millis() - _last_current_angle_rad_ms > 1000) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// if we get this far return healthy
|
||||
return true;
|
||||
}
|
||||
|
||||
// get attitude as a quaternion. returns true on success
|
||||
bool AP_Mount_Siyi::get_attitude_quaternion(Quaternion& att_quat)
|
||||
{
|
||||
att_quat.from_euler(_current_angle_rad.x, _current_angle_rad.y, _current_angle_rad.z);
|
||||
return true;
|
||||
}
|
||||
|
||||
// reading incoming packets from gimbal and confirm they are of the correct format
|
||||
// results are held in the _parsed_msg structure
|
||||
void AP_Mount_Siyi::read_incoming_packets()
|
||||
{
|
||||
// check for bytes on the serial port
|
||||
int16_t nbytes = MIN(_uart->available(), 1024U);
|
||||
if (nbytes <= 0 ) {
|
||||
return;
|
||||
}
|
||||
|
||||
// flag to allow cases below to reset parser state
|
||||
bool reset_parser = false;
|
||||
|
||||
// process bytes received
|
||||
for (int16_t i = 0; i < nbytes; i++) {
|
||||
const int16_t b = _uart->read();
|
||||
|
||||
// sanity check byte
|
||||
if ((b < 0) || (b > 0xFF)) {
|
||||
continue;
|
||||
}
|
||||
|
||||
_msg_buff[_msg_buff_len++] = b;
|
||||
|
||||
// protect against overly long messages
|
||||
if (_msg_buff_len >= AP_MOUNT_SIYI_PACKETLEN_MAX) {
|
||||
reset_parser = true;
|
||||
}
|
||||
|
||||
// process byte depending upon current state
|
||||
switch (_parsed_msg.state) {
|
||||
|
||||
case ParseState::WAITING_FOR_HEADER_LOW:
|
||||
if (b == AP_MOUNT_SIYI_HEADER1) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_HEADER_HIGH;
|
||||
} else {
|
||||
reset_parser = true;
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_HEADER_HIGH:
|
||||
if (b == AP_MOUNT_SIYI_HEADER2) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_CTRL;
|
||||
} else {
|
||||
reset_parser = true;
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_CTRL:
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_DATALEN_LOW;
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_DATALEN_LOW:
|
||||
_parsed_msg.data_len = b;
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_DATALEN_HIGH;
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_DATALEN_HIGH:
|
||||
_parsed_msg.data_len |= ((uint16_t)b << 8);
|
||||
// sanity check data length
|
||||
if (_parsed_msg.data_len <= AP_MOUNT_SIYI_DATALEN_MAX) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_SEQ_LOW;
|
||||
} else {
|
||||
reset_parser = true;
|
||||
debug("data len too large:%u (>%u)", (unsigned)_parsed_msg.data_len, (unsigned)AP_MOUNT_SIYI_DATALEN_MAX);
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_SEQ_LOW:
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_SEQ_HIGH;
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_SEQ_HIGH:
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_CMDID;
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_CMDID:
|
||||
_parsed_msg.command_id = b;
|
||||
_parsed_msg.data_bytes_received = 0;
|
||||
if (_parsed_msg.data_len > 0) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_DATA;
|
||||
} else {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_DATA:
|
||||
_parsed_msg.data_bytes_received++;
|
||||
if (_parsed_msg.data_bytes_received >= _parsed_msg.data_len) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_CRC_LOW;
|
||||
}
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_CRC_LOW:
|
||||
_parsed_msg.crc16 = b;
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_CRC_HIGH;
|
||||
break;
|
||||
|
||||
case ParseState::WAITING_FOR_CRC_HIGH:
|
||||
_parsed_msg.crc16 |= ((uint16_t)b << 8);
|
||||
|
||||
// check crc
|
||||
const uint16_t expected_crc = crc16_ccitt(_msg_buff, _msg_buff_len-2, 0);
|
||||
if (expected_crc == _parsed_msg.crc16) {
|
||||
// successfully received a message, do something with it
|
||||
process_packet();
|
||||
} else {
|
||||
debug("crc expected:%x got:%x", (unsigned)expected_crc, (unsigned)_parsed_msg.crc16);
|
||||
}
|
||||
reset_parser = true;
|
||||
break;
|
||||
}
|
||||
|
||||
// handle reset of parser
|
||||
if (reset_parser) {
|
||||
_parsed_msg.state = ParseState::WAITING_FOR_HEADER_LOW;
|
||||
_msg_buff_len = 0;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// process successfully decoded packets held in the _parsed_msg structure
|
||||
void AP_Mount_Siyi::process_packet()
|
||||
{
|
||||
// flag to warn of unexpected data buffer length
|
||||
bool unexpected_len = false;
|
||||
|
||||
// process packet depending upon command id
|
||||
switch ((SiyiCommandId)_parsed_msg.command_id) {
|
||||
|
||||
case SiyiCommandId::ACQUIRE_FIRMWARE_VERSION: {
|
||||
if (_parsed_msg.data_bytes_received != 12) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
_got_firmware_version = true;
|
||||
|
||||
// display camera firmware version
|
||||
debug("Mount: SiyiCam fw:%u.%u.%u",
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+1], // firmware major version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+2], // firmware minor version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+3]); // firmware revision
|
||||
|
||||
// display gimbal info to user
|
||||
gcs().send_text(MAV_SEVERITY_INFO, "Mount: Siyi fw:%u.%u.%u",
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+5], // firmware major version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+6], // firmware minor version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+7]); // firmware revision
|
||||
|
||||
// display zoom firmware version
|
||||
debug("Mount: SiyiZoom fw:%u.%u.%u",
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+9], // firmware major version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+10], // firmware minor version
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+11]); // firmware revision
|
||||
break;
|
||||
}
|
||||
|
||||
case SiyiCommandId::HARDWARE_ID:
|
||||
// unsupported
|
||||
break;
|
||||
|
||||
case SiyiCommandId::AUTO_FOCUS:
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
if (_parsed_msg.data_bytes_received != 1) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
debug("AutoFocus:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS: {
|
||||
if (_parsed_msg.data_bytes_received != 2) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
const float zoom_mult = UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1;
|
||||
debug("ZoomMult:%4.1f", (double)zoom_mult);
|
||||
break;
|
||||
}
|
||||
|
||||
case SiyiCommandId::MANUAL_FOCUS:
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
if (_parsed_msg.data_bytes_received != 1) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
debug("ManualFocus:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case SiyiCommandId::GIMBAL_ROTATION:
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
if (_parsed_msg.data_bytes_received != 1) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
debug("GimbRot:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case SiyiCommandId::CENTER:
|
||||
#if AP_MOUNT_SIYI_DEBUG
|
||||
if (_parsed_msg.data_bytes_received != 1) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
debug("Center:%u", (unsigned)_msg_buff[_msg_buff_data_start]);
|
||||
#endif
|
||||
break;
|
||||
|
||||
case SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO: {
|
||||
if (_parsed_msg.data_bytes_received != 5) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
// update recording state and warn user of mismatch
|
||||
const bool recording = _msg_buff[_msg_buff_data_start+3] > 0;
|
||||
if (recording != _last_record_video) {
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "Siyi: recording %s", recording ? "ON" : "OFF");
|
||||
}
|
||||
_last_record_video = recording;
|
||||
debug("GimConf hdr:%u rec:%u foll:%u", (unsigned)_msg_buff[_msg_buff_data_start+1],
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+3],
|
||||
(unsigned)_msg_buff[_msg_buff_data_start+4]);
|
||||
break;
|
||||
}
|
||||
|
||||
case SiyiCommandId::FUNCTION_FEEDBACK_INFO: {
|
||||
if (_parsed_msg.data_bytes_received != 1) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
const uint8_t func_feedback_info = _msg_buff[_msg_buff_data_start];
|
||||
const char* err_prefix = "Mount: Siyi";
|
||||
switch ((FunctionFeedbackInfo)func_feedback_info) {
|
||||
case FunctionFeedbackInfo::SUCCESS:
|
||||
debug("FnFeedB success");
|
||||
break;
|
||||
case FunctionFeedbackInfo::FAILED_TO_TAKE_PHOTO:
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "%s failed to take picture", err_prefix);
|
||||
break;
|
||||
case FunctionFeedbackInfo::HDR_ON:
|
||||
debug("HDR on");
|
||||
break;
|
||||
case FunctionFeedbackInfo::HDR_OFF:
|
||||
debug("HDR off");
|
||||
break;
|
||||
case FunctionFeedbackInfo::FAILED_TO_RECORD_VIDEO:
|
||||
gcs().send_text(MAV_SEVERITY_ERROR, "%s failed to record video", err_prefix);
|
||||
break;
|
||||
default:
|
||||
debug("FnFeedB unexpected val:%u", (unsigned)func_feedback_info);
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
case SiyiCommandId::PHOTO:
|
||||
// no ack should ever be sent by the gimbal
|
||||
break;
|
||||
|
||||
case SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE: {
|
||||
if (_parsed_msg.data_bytes_received != 12) {
|
||||
unexpected_len = true;
|
||||
break;
|
||||
}
|
||||
_last_current_angle_rad_ms = AP_HAL::millis();
|
||||
_current_angle_rad.z = -radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+1], _msg_buff[_msg_buff_data_start]) * 0.1); // yaw angle
|
||||
_current_angle_rad.y = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+3], _msg_buff[_msg_buff_data_start+2]) * 0.1); // pitch angle
|
||||
_current_angle_rad.x = radians((int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+5], _msg_buff[_msg_buff_data_start+4]) * 0.1); // roll angle
|
||||
//const float yaw_rate_degs = -(int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+7], _msg_buff[_msg_buff_data_start+6]) * 0.1; // yaw rate
|
||||
//const float pitch_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+9], _msg_buff[_msg_buff_data_start+8]) * 0.1; // pitch rate
|
||||
//const float roll_rate_deg = (int16_t)UINT16_VALUE(_msg_buff[_msg_buff_data_start+11], _msg_buff[_msg_buff_data_start+10]) * 0.1; // roll rate
|
||||
break;
|
||||
}
|
||||
|
||||
default:
|
||||
debug("Unhandled CmdId:%u", (unsigned)_parsed_msg.command_id);
|
||||
break;
|
||||
}
|
||||
|
||||
// handle unexpected data buffer lenth
|
||||
if (unexpected_len) {
|
||||
debug("CmdId:%u unexpected len:%u", (unsigned)_parsed_msg.command_id, (unsigned)_parsed_msg.data_bytes_received);
|
||||
}
|
||||
}
|
||||
|
||||
// methods to send commands to gimbal
|
||||
// returns true on success, false if outgoing serial buffer is full
|
||||
bool AP_Mount_Siyi::send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len)
|
||||
{
|
||||
// calculate and sanity check packet size
|
||||
const uint16_t packet_size = AP_MOUNT_SIYI_PACKETLEN_MIN + databuff_len;
|
||||
if (packet_size > AP_MOUNT_SIYI_PACKETLEN_MAX) {
|
||||
debug("send_packet data buff too large");
|
||||
return false;
|
||||
}
|
||||
|
||||
// check for sufficient space in outgoing buffer
|
||||
if (_uart->txspace() < packet_size) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// buffer for holding outgoing packet
|
||||
uint8_t send_buff[packet_size];
|
||||
uint8_t send_buff_ofs = 0;
|
||||
|
||||
// packet header
|
||||
send_buff[send_buff_ofs++] = AP_MOUNT_SIYI_HEADER1;
|
||||
send_buff[send_buff_ofs++] = AP_MOUNT_SIYI_HEADER2;
|
||||
|
||||
// CTRL. Always request ACK
|
||||
send_buff[send_buff_ofs++] = 1;
|
||||
|
||||
// Data_len. protocol supports uint16_t but messages are never longer than 22 bytes
|
||||
send_buff[send_buff_ofs++] = databuff_len;
|
||||
send_buff[send_buff_ofs++] = 0;
|
||||
|
||||
// SEQ (sequence)
|
||||
send_buff[send_buff_ofs++] = LOWBYTE(_last_seq);
|
||||
send_buff[send_buff_ofs++] = HIGHBYTE(_last_seq++);
|
||||
|
||||
// CMD_ID
|
||||
send_buff[send_buff_ofs++] = (uint8_t)cmd_id;
|
||||
|
||||
// DATA
|
||||
if (databuff_len != 0) {
|
||||
memcpy(&send_buff[send_buff_ofs], databuff, databuff_len);
|
||||
send_buff_ofs += databuff_len;
|
||||
}
|
||||
|
||||
// CRC16
|
||||
const uint16_t crc = crc16_ccitt(send_buff, send_buff_ofs, 0);
|
||||
send_buff[send_buff_ofs++] = LOWBYTE(crc);
|
||||
send_buff[send_buff_ofs++] = HIGHBYTE(crc);
|
||||
|
||||
// send packet
|
||||
_uart->write(send_buff, send_buff_ofs);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// send a packet with a single data byte to gimbal
|
||||
// returns true on success, false if outgoing serial buffer is full
|
||||
bool AP_Mount_Siyi::send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte)
|
||||
{
|
||||
return send_packet(cmd_id, &data_byte, 1);
|
||||
}
|
||||
|
||||
// rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
|
||||
// yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
|
||||
void AP_Mount_Siyi::rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef)
|
||||
{
|
||||
// send lock/follow value if it has changed
|
||||
if ((yaw_is_ef != _last_lock) || (_lock_send_counter >= AP_MOUNT_SIYI_LOCK_RESEND_COUNT)) {
|
||||
set_lock(yaw_is_ef);
|
||||
_lock_send_counter = 0;
|
||||
_last_lock = yaw_is_ef;
|
||||
} else {
|
||||
_lock_send_counter++;
|
||||
}
|
||||
|
||||
const uint8_t yaw_and_pitch_rates[] {(uint8_t)yaw_scalar, (uint8_t)pitch_scalar};
|
||||
send_packet(SiyiCommandId::GIMBAL_ROTATION, yaw_and_pitch_rates, ARRAY_SIZE(yaw_and_pitch_rates));
|
||||
}
|
||||
|
||||
// center gimbal
|
||||
void AP_Mount_Siyi::center_gimbal()
|
||||
{
|
||||
send_1byte_packet(SiyiCommandId::CENTER, 1);
|
||||
}
|
||||
|
||||
// set gimbal's lock vs follow mode
|
||||
// lock should be true if gimbal should maintain an earth-frame target
|
||||
// lock is false to follow / maintain a body-frame target
|
||||
void AP_Mount_Siyi::set_lock(bool lock)
|
||||
{
|
||||
send_1byte_packet(SiyiCommandId::PHOTO, lock ? (uint8_t)PhotoFunction::LOCK_MODE : (uint8_t)PhotoFunction::FOLLOW_MODE);
|
||||
}
|
||||
|
||||
// send target pitch and yaw rates to gimbal
|
||||
// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
|
||||
void AP_Mount_Siyi::send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef)
|
||||
{
|
||||
const float pitch_rate_scalar = constrain_float(100.0 * pitch_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
|
||||
const float yaw_rate_scalar = constrain_float(100.0 * yaw_rads / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
|
||||
rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
|
||||
}
|
||||
|
||||
// send target pitch and yaw angles to gimbal
|
||||
// yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame
|
||||
void AP_Mount_Siyi::send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef)
|
||||
{
|
||||
// stop gimbal if no recent actual angles
|
||||
uint32_t now_ms = AP_HAL::millis();
|
||||
if (now_ms - _last_current_angle_rad_ms >= 200) {
|
||||
rotate_gimbal(0, 0, false);
|
||||
return;
|
||||
}
|
||||
|
||||
// use simple P controller to convert pitch angle error (in radians) to a target rate scalar (-100 to +100)
|
||||
const float pitch_err_rad = (pitch_rad - _current_angle_rad.y);
|
||||
const float pitch_rate_scalar = constrain_float(100.0 * pitch_err_rad * AP_MOUNT_SIYI_PITCH_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
|
||||
|
||||
// convert yaw angle to body-frame the use simple P controller to convert yaw angle error to a target rate scalar (-100 to +100)
|
||||
const float yaw_bf_rad = yaw_is_ef ? wrap_PI(yaw_rad - AP::ahrs().yaw) : yaw_rad;
|
||||
const float yaw_err_rad = (yaw_bf_rad - _current_angle_rad.z);
|
||||
const float yaw_rate_scalar = constrain_float(100.0 * yaw_err_rad * AP_MOUNT_SIYI_YAW_P / AP_MOUNT_SIYI_RATE_MAX_RADS, -100, 100);
|
||||
|
||||
// rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
|
||||
rotate_gimbal(pitch_rate_scalar, yaw_rate_scalar, yaw_is_ef);
|
||||
}
|
||||
|
||||
// take a picture. returns true on success
|
||||
bool AP_Mount_Siyi::take_picture()
|
||||
{
|
||||
return send_1byte_packet(SiyiCommandId::PHOTO, (uint8_t)PhotoFunction::TAKE_PICTURE);
|
||||
}
|
||||
|
||||
// start or stop video recording. returns true on success
|
||||
// set start_recording = true to start record, false to stop recording
|
||||
bool AP_Mount_Siyi::record_video(bool start_recording)
|
||||
{
|
||||
// exit immediately if not initialised to reduce mismatch
|
||||
// between internal and actual state of recording
|
||||
if (!_initialised) {
|
||||
return false;
|
||||
}
|
||||
|
||||
// check desired recording state has changed
|
||||
bool ret = true;
|
||||
if (_last_record_video != start_recording) {
|
||||
_last_record_video = start_recording;
|
||||
|
||||
// request recording start or stop (sadly the same message is used)
|
||||
const uint8_t func_type = (uint8_t)PhotoFunction::RECORD_VIDEO_TOGGLE;
|
||||
ret = send_packet(SiyiCommandId::PHOTO, &func_type, 1);
|
||||
}
|
||||
|
||||
// request recording state update from gimbal
|
||||
request_configuration();
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
// set camera zoom step. returns true on success
|
||||
// zoom out = -1, hold = 0, zoom in = 1
|
||||
bool AP_Mount_Siyi::set_zoom_step(int8_t zoom_step)
|
||||
{
|
||||
return send_1byte_packet(SiyiCommandId::MANUAL_ZOOM_AND_AUTO_FOCUS, (uint8_t)zoom_step);
|
||||
}
|
||||
|
||||
// set focus in, out or hold. returns true on success
|
||||
// focus in = -1, focus hold = 0, focus out = 1
|
||||
bool AP_Mount_Siyi::set_manual_focus_step(int8_t focus_step)
|
||||
{
|
||||
return send_1byte_packet(SiyiCommandId::MANUAL_FOCUS, (uint8_t)focus_step);
|
||||
}
|
||||
|
||||
// auto focus. returns true on success
|
||||
bool AP_Mount_Siyi::set_auto_focus()
|
||||
{
|
||||
return send_1byte_packet(SiyiCommandId::AUTO_FOCUS, 1);
|
||||
}
|
||||
|
||||
#endif // HAL_MOUNT_SIYI_ENABLED
|
||||
209
libraries/AP_Mount/AP_Mount_Siyi.h
Normal file
209
libraries/AP_Mount/AP_Mount_Siyi.h
Normal file
@ -0,0 +1,209 @@
|
||||
/*
|
||||
Siyi gimbal driver using custom serial protocol
|
||||
|
||||
Packet format (courtesy of Siyi's SDK document)
|
||||
|
||||
-------------------------------------------------------------------------------------------
|
||||
Field Index Bytes Description
|
||||
-------------------------------------------------------------------------------------------
|
||||
STX 0 2 0x5566: starting mark
|
||||
CTRL 2 1 bit 0: need_ack. set if the current data packet needs ack
|
||||
bit 1: ack_pack. set if the current data packate IS an ack
|
||||
bit 2-7: reserved
|
||||
Data_len 3 2 Data field byte length. Low byte in the front
|
||||
SEQ 5 2 Frame sequence (0 ~ 65535). Low byte in the front. May be used to detect packet loss
|
||||
CMD_ID 7 1 Command ID
|
||||
DATA 8 Data_len Data
|
||||
CRC16 2 CRC16 check the complete data package. Low byte in the front
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "AP_Mount_Backend.h"
|
||||
|
||||
#ifndef HAL_MOUNT_SIYI_ENABLED
|
||||
#define HAL_MOUNT_SIYI_ENABLED HAL_MOUNT_ENABLED
|
||||
#endif
|
||||
|
||||
#if HAL_MOUNT_SIYI_ENABLED
|
||||
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include <AP_Common/AP_Common.h>
|
||||
|
||||
#define AP_MOUNT_SIYI_PACKETLEN_MAX 22 // maximum number of bytes in a packet sent to or received from the gimbal
|
||||
|
||||
class AP_Mount_Siyi : public AP_Mount_Backend
|
||||
{
|
||||
|
||||
public:
|
||||
// Constructor
|
||||
using AP_Mount_Backend::AP_Mount_Backend;
|
||||
|
||||
/* 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;
|
||||
|
||||
// return true if healthy
|
||||
bool healthy() const override;
|
||||
|
||||
// has_pan_control - returns true if this mount can control its pan (required for multicopters)
|
||||
bool has_pan_control() const override { return yaw_range_valid(); };
|
||||
|
||||
//
|
||||
// camera controls
|
||||
//
|
||||
|
||||
// take a picture. returns true on success
|
||||
bool take_picture() override;
|
||||
|
||||
// start or stop video recording
|
||||
// set start_recording = true to start record, false to stop recording
|
||||
bool record_video(bool start_recording) override;
|
||||
|
||||
// set camera zoom step. returns true on success
|
||||
// zoom out = -1, hold = 0, zoom in = 1
|
||||
bool set_zoom_step(int8_t zoom_step) override;
|
||||
|
||||
// set focus in, out or hold. returns true on success
|
||||
// focus in = -1, focus hold = 0, focus out = 1
|
||||
bool set_manual_focus_step(int8_t focus_step) override;
|
||||
|
||||
// auto focus. returns true on success
|
||||
bool set_auto_focus() override;
|
||||
|
||||
protected:
|
||||
|
||||
// get attitude as a quaternion. returns true on success
|
||||
bool get_attitude_quaternion(Quaternion& att_quat) override;
|
||||
|
||||
private:
|
||||
|
||||
// serial protocol command ids
|
||||
enum class SiyiCommandId {
|
||||
ACQUIRE_FIRMWARE_VERSION = 0x01,
|
||||
HARDWARE_ID = 0x02,
|
||||
AUTO_FOCUS = 0x04,
|
||||
MANUAL_ZOOM_AND_AUTO_FOCUS = 0x05,
|
||||
MANUAL_FOCUS = 0x06,
|
||||
GIMBAL_ROTATION = 0x07,
|
||||
CENTER = 0x08,
|
||||
ACQUIRE_GIMBAL_CONFIG_INFO = 0x0A,
|
||||
FUNCTION_FEEDBACK_INFO = 0x0B,
|
||||
PHOTO = 0x0C,
|
||||
ACQUIRE_GIMBAL_ATTITUDE = 0x0D
|
||||
};
|
||||
|
||||
// Function Feedback Info packet info_type values
|
||||
enum class FunctionFeedbackInfo : uint8_t {
|
||||
SUCCESS = 0,
|
||||
FAILED_TO_TAKE_PHOTO = 1,
|
||||
HDR_ON = 2,
|
||||
HDR_OFF = 3,
|
||||
FAILED_TO_RECORD_VIDEO = 4
|
||||
};
|
||||
|
||||
// Photo Function packet func_type values
|
||||
enum class PhotoFunction : uint8_t {
|
||||
TAKE_PICTURE = 0,
|
||||
HDR_TOGGLE = 1,
|
||||
RECORD_VIDEO_TOGGLE = 2,
|
||||
LOCK_MODE = 3,
|
||||
FOLLOW_MODE = 4,
|
||||
FPV_MODE = 5
|
||||
};
|
||||
|
||||
// parsing state
|
||||
enum class ParseState : uint8_t {
|
||||
WAITING_FOR_HEADER_LOW,
|
||||
WAITING_FOR_HEADER_HIGH,
|
||||
WAITING_FOR_CTRL,
|
||||
WAITING_FOR_DATALEN_LOW,
|
||||
WAITING_FOR_DATALEN_HIGH,
|
||||
WAITING_FOR_SEQ_LOW,
|
||||
WAITING_FOR_SEQ_HIGH,
|
||||
WAITING_FOR_CMDID,
|
||||
WAITING_FOR_DATA,
|
||||
WAITING_FOR_CRC_LOW,
|
||||
WAITING_FOR_CRC_HIGH,
|
||||
};
|
||||
|
||||
// reading incoming packets from gimbal and confirm they are of the correct format
|
||||
// results are held in the _parsed_msg structure
|
||||
void read_incoming_packets();
|
||||
|
||||
// process successfully decoded packets held in the _parsed_msg structure
|
||||
void process_packet();
|
||||
|
||||
// send packet to gimbal
|
||||
// returns true on success, false if outgoing serial buffer is full
|
||||
bool send_packet(SiyiCommandId cmd_id, const uint8_t* databuff, uint8_t databuff_len);
|
||||
bool send_1byte_packet(SiyiCommandId cmd_id, uint8_t data_byte);
|
||||
|
||||
// request info from gimbal
|
||||
void request_firmware_version() { send_packet(SiyiCommandId::ACQUIRE_FIRMWARE_VERSION, nullptr, 0); }
|
||||
void request_hardware_id() { send_packet(SiyiCommandId::HARDWARE_ID, nullptr, 0); }
|
||||
void request_configuration() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_CONFIG_INFO, nullptr, 0); }
|
||||
void request_function_feedback_info() { send_packet(SiyiCommandId::FUNCTION_FEEDBACK_INFO, nullptr, 0); }
|
||||
void request_gimbal_attitude() { send_packet(SiyiCommandId::ACQUIRE_GIMBAL_ATTITUDE, nullptr, 0); }
|
||||
|
||||
// rotate gimbal. pitch_rate and yaw_rate are scalars in the range -100 ~ +100
|
||||
// yaw_is_ef should be true if gimbal should maintain an earth-frame target (aka lock)
|
||||
void rotate_gimbal(int8_t pitch_scalar, int8_t yaw_scalar, bool yaw_is_ef);
|
||||
|
||||
// center gimbal
|
||||
void center_gimbal();
|
||||
|
||||
// set gimbal's lock vs follow mode
|
||||
// lock should be true if gimbal should maintain an earth-frame target
|
||||
// lock is false to follow / maintain a body-frame target
|
||||
void set_lock(bool lock);
|
||||
|
||||
// send target pitch and yaw rates to gimbal
|
||||
// yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
|
||||
void send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);
|
||||
|
||||
// send target pitch and yaw angles to gimbal
|
||||
// yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame
|
||||
void send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);
|
||||
|
||||
// internal variables
|
||||
AP_HAL::UARTDriver *_uart; // uart connected to gimbal
|
||||
bool _initialised; // true once the driver has been initialised
|
||||
bool _got_firmware_version; // true once gimbal firmware version has been received
|
||||
|
||||
// buffer holding bytes from latest packet. This is only used to calculate the crc
|
||||
uint8_t _msg_buff[AP_MOUNT_SIYI_PACKETLEN_MAX];
|
||||
uint8_t _msg_buff_len;
|
||||
const uint8_t _msg_buff_data_start = 8; // data starts at this byte of _msg_buff
|
||||
|
||||
// parser state and unpacked fields
|
||||
struct PACKED {
|
||||
uint16_t data_len; // expected number of data bytes
|
||||
uint8_t command_id; // command id
|
||||
uint16_t data_bytes_received; // number of data bytes received so far
|
||||
uint16_t crc16; // latest message's crc
|
||||
ParseState state; // state of incoming message processing
|
||||
} _parsed_msg;
|
||||
|
||||
// variables for sending packets to gimbal
|
||||
uint32_t _last_send_ms; // system time (in milliseconds) of last packet sent to gimbal
|
||||
uint16_t _last_seq; // last sequence number used (should be increment for each send)
|
||||
bool _last_lock; // last lock value sent to gimbal
|
||||
uint8_t _lock_send_counter; // counter used to resend lock status to gimbal at regular intervals
|
||||
|
||||
// actual attitude received from gimbal
|
||||
Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
|
||||
uint32_t _last_current_angle_rad_ms; // system time _current_angle_rad was updated
|
||||
uint32_t _last_req_current_angle_rad_ms; // system time that this driver last requested current angle
|
||||
|
||||
// variables for camera state
|
||||
bool _last_record_video; // last record_video state sent to gimbal
|
||||
};
|
||||
|
||||
#endif // HAL_MOUNT_SIYISERIAL_ENABLED
|
||||
Loading…
Reference in New Issue
Block a user