mirror of https://github.com/ArduPilot/ardupilot
267 lines
11 KiB
C
267 lines
11 KiB
C
|
/*
|
||
|
Topotek gimbal driver using custom serial protocol
|
||
|
|
||
|
Packet format (courtesy of Topotek's SDK document)
|
||
|
|
||
|
-------------------------------------------------------------------------------------------
|
||
|
Field Index Bytes Description
|
||
|
-------------------------------------------------------------------------------------------
|
||
|
Frame Header 0 3 type of command
|
||
|
Address Bit 3 2 the source address comes first, and the destination address comes last
|
||
|
Data_Len 5 1 data length
|
||
|
Control Bit 6 1 r -> query w -> setup and control
|
||
|
Identification Bit 7 3 identification function
|
||
|
Data 10 Data_Len
|
||
|
Check Bit 2 the frame header is converted to HEX before reaching the check bit,
|
||
|
the sum is done, and the result is converted to ASC-II. Two bytes, the high one first
|
||
|
*/
|
||
|
|
||
|
#pragma once
|
||
|
|
||
|
#include "AP_Mount_Backend_Serial.h"
|
||
|
|
||
|
#if HAL_MOUNT_TOPOTEK_ENABLED
|
||
|
|
||
|
#include <AP_HAL/AP_HAL.h>
|
||
|
#include <AP_Math/AP_Math.h>
|
||
|
#include <AP_Common/AP_Common.h>
|
||
|
|
||
|
#define AP_MOUNT_TOPOTEK_PACKETLEN_MAX 36 // maximum number of bytes in a packet sent to or received from the gimbal
|
||
|
#define AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM 6 // parse the number of gimbal command types
|
||
|
|
||
|
class AP_Mount_Topotek : public AP_Mount_Backend_Serial
|
||
|
{
|
||
|
|
||
|
public:
|
||
|
// Constructor
|
||
|
using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
|
||
|
|
||
|
// Do not allow copies
|
||
|
CLASS_NO_COPY(AP_Mount_Topotek);
|
||
|
|
||
|
// 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 for gimbals
|
||
|
//
|
||
|
|
||
|
// 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 zoom specified as a rate
|
||
|
bool set_zoom(ZoomType zoom_type, float zoom_value) override;
|
||
|
|
||
|
// set focus specified as rate or auto
|
||
|
// focus in = -1, focus hold = 0, focus out = 1
|
||
|
SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
|
||
|
|
||
|
// set tracking to none, point or rectangle (see TrackingType enum)
|
||
|
// if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
|
||
|
// p1,p2 are in range 0 to 1. 0 is left or top, 1 is right or bottom
|
||
|
bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override;
|
||
|
|
||
|
// send command to gimbal to cancel tracking (if necessary)
|
||
|
// returns true on success, false on failure to send message
|
||
|
bool cancel_tracking();
|
||
|
|
||
|
// set camera picture-in-picture mode
|
||
|
bool set_lens(uint8_t lens) override;
|
||
|
|
||
|
// set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
|
||
|
// primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
|
||
|
bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
|
||
|
|
||
|
// send camera information message to GCS
|
||
|
void send_camera_information(mavlink_channel_t chan) const override;
|
||
|
|
||
|
// send camera settings message to GCS
|
||
|
void send_camera_settings(mavlink_channel_t chan) const override;
|
||
|
|
||
|
//
|
||
|
// rangefinder
|
||
|
//
|
||
|
|
||
|
// get rangefinder distance. Returns true on success
|
||
|
bool get_rangefinder_distance(float& distance_m) const override;
|
||
|
|
||
|
// enable/disable rangefinder. Returns true on success
|
||
|
bool set_rangefinder_enable(bool enable) override;
|
||
|
|
||
|
protected:
|
||
|
|
||
|
// get attitude as a quaternion. returns true on success
|
||
|
bool get_attitude_quaternion(Quaternion& att_quat) override;
|
||
|
|
||
|
private:
|
||
|
|
||
|
// header type (fixed or variable length)
|
||
|
// first three bytes of packet determined by this value
|
||
|
enum class HeaderType : uint8_t {
|
||
|
FIXED_LEN = 0x00, // #TP will be sent
|
||
|
VARIABLE_LEN = 0x01, // #tp will be sent
|
||
|
};
|
||
|
|
||
|
// address (2nd and 3rd bytes of packet)
|
||
|
// first byte is always U followed by one of the other options
|
||
|
enum class AddressByte : uint8_t {
|
||
|
SYSTEM_AND_IMAGE = 68, // 'D'
|
||
|
AUXILIARY_EQUIPMENT = 69, // 'E'
|
||
|
GIMBAL = 71, // 'G'
|
||
|
LENS = 77, // 'M'
|
||
|
NETWORK = 80, // 'P'
|
||
|
UART = 85, // 'U'
|
||
|
};
|
||
|
|
||
|
// control byte (read or write)
|
||
|
// sent as 7th byte of packet
|
||
|
enum class ControlByte : uint8_t {
|
||
|
READ = 114, // 'r'
|
||
|
WRITE = 119, // 'w'
|
||
|
};
|
||
|
|
||
|
// parsing state
|
||
|
enum class ParseState : uint8_t {
|
||
|
WAITING_FOR_HEADER1 = 0,// #
|
||
|
WAITING_FOR_HEADER2, // T or t
|
||
|
WAITING_FOR_HEADER3, // P or p
|
||
|
WAITING_FOR_ADDR1, // normally U
|
||
|
WAITING_FOR_ADDR2, // M, D, E, P, G
|
||
|
WAITING_FOR_DATALEN,
|
||
|
WAITING_FOR_CONTROL, // r or w
|
||
|
WAITING_FOR_ID1, // e.g. 'G'
|
||
|
WAITING_FOR_ID2, // e.g. 'A'
|
||
|
WAITING_FOR_ID3, // e.g. 'C'
|
||
|
WAITING_FOR_DATA, // normally hex numbers in char form (e.g. '0A')
|
||
|
WAITING_FOR_CRC_LOW,
|
||
|
WAITING_FOR_CRC_HIGH,
|
||
|
};
|
||
|
|
||
|
// identifier bytes
|
||
|
typedef char Identifier[3];
|
||
|
|
||
|
// send text prefix string
|
||
|
static const char* send_message_prefix;
|
||
|
|
||
|
// reading incoming packets from gimbal and confirm they are of the correct format
|
||
|
void read_incoming_packets();
|
||
|
|
||
|
// request gimbal attitude
|
||
|
void request_gimbal_attitude();
|
||
|
|
||
|
// request gimbal memory card information
|
||
|
void request_gimbal_sdcard_info();
|
||
|
|
||
|
// request gimbal tracking status
|
||
|
void request_track_status();
|
||
|
|
||
|
// request gimbal version
|
||
|
void request_gimbal_version();
|
||
|
|
||
|
// send angle target in radians to gimbal
|
||
|
void send_angle_target(const MountTarget& angle_rad);
|
||
|
|
||
|
// send rate target in rad/s to gimbal
|
||
|
void send_rate_target(const MountTarget& rate_rads);
|
||
|
|
||
|
// send time and date to gimbal
|
||
|
bool send_time_to_gimbal();
|
||
|
|
||
|
// send GPS-related information to the gimbal
|
||
|
bool send_location_info();
|
||
|
|
||
|
// attitude information analysis of gimbal
|
||
|
void gimbal_angle_analyse();
|
||
|
|
||
|
// gimbal video information analysis
|
||
|
void gimbal_record_analyse();
|
||
|
|
||
|
// information analysis of gimbal storage card
|
||
|
void gimbal_sdcard_analyse();
|
||
|
|
||
|
// gimbal tracking information analysis
|
||
|
void gimbal_track_analyse();
|
||
|
|
||
|
// gimbal basic information analysis
|
||
|
void gimbal_version_analyse();
|
||
|
|
||
|
// gimbal distance information analysis
|
||
|
void gimbal_dist_info_analyse();
|
||
|
|
||
|
// calculate checksum
|
||
|
uint8_t calculate_crc(const uint8_t *cmd, uint8_t len) const;
|
||
|
|
||
|
// hexadecimal to character conversion
|
||
|
uint8_t hex2char(uint8_t data) const;
|
||
|
|
||
|
// convert a 4 character hex number to an integer
|
||
|
// the characters are in the format "1234" where the most significant digit is first
|
||
|
int16_t hexchar4_to_int16(char high, char mid_high, char mid_low, char low) const;
|
||
|
|
||
|
// send a fixed length packet to gimbal
|
||
|
// returns true on success, false if serial port initialization failed
|
||
|
bool send_fixedlen_packet(AddressByte address, const Identifier id, bool write, uint8_t value);
|
||
|
|
||
|
// send a variable length packet to gimbal
|
||
|
// returns true on success, false if serial port initialization failed
|
||
|
bool send_variablelen_packet(HeaderType header, AddressByte address, const Identifier id, bool write, const uint8_t* databuff, uint8_t databuff_len);
|
||
|
|
||
|
// 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
|
||
|
bool set_gimbal_lock(bool lock);
|
||
|
|
||
|
// members
|
||
|
bool _recording; // recording status (received from gimbal)
|
||
|
bool _is_tracking; // whether to enable the tracking state
|
||
|
uint8_t _last_tracking_state; // last tracking state received from gimbal
|
||
|
uint8_t _last_mode; // mode during latest update, used to detect mode changes and cancel tracking
|
||
|
bool _sdcard_status; // memory card status (received from gimbal)
|
||
|
bool _last_lock; // last lock mode sent to gimbal
|
||
|
bool _got_gimbal_version; // true if gimbal's version has been received
|
||
|
bool _last_zoom_stop; // true if zoom has been stopped (used to re-send in order to handle lost packets)
|
||
|
bool _last_focus_stop; // true if focus has been stopped (used to re-sent in order to handle lost packets)
|
||
|
uint8_t _sent_time_count; // count of current time messages sent to gimbal
|
||
|
uint32_t _firmware_ver; // firmware version
|
||
|
Vector3f _current_angle_rad; // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
|
||
|
uint32_t _last_current_angle_ms; // system time (in milliseconds) that angle information received from the gimbal
|
||
|
uint32_t _last_req_current_info_ms; // system time that this driver last requested current gimbal infomation
|
||
|
uint8_t _last_req_step; // 10hz request loop step (different requests are sent at various steps)
|
||
|
uint8_t _stop_order_count; // number of stop commands sent since target rates became zero
|
||
|
float _measure_dist_m = -1.0f; // latest rangefinder distance (in meters)
|
||
|
uint8_t _msg_buff[AP_MOUNT_TOPOTEK_PACKETLEN_MAX]; // buffer holding bytes from latest packet received. only used to calculate crc
|
||
|
uint8_t _msg_buff_len; // number of bytes in the msg buffer
|
||
|
struct {
|
||
|
ParseState state; // parser state
|
||
|
uint8_t data_len; // expected number of data bytes
|
||
|
} _parser;
|
||
|
|
||
|
// mapping from received message key to member function pointer to consume the message
|
||
|
typedef struct {
|
||
|
uint8_t uart_cmd_key[4]; // gimbal message key;
|
||
|
void (AP_Mount_Topotek::*func)(void); // member function to consume messager
|
||
|
} UartCmdFunctionHandler;
|
||
|
|
||
|
// stores command ID and corresponding member functions that are compared with the command received by the gimbal
|
||
|
UartCmdFunctionHandler uart_recv_cmd_compare_list[AP_MOUNT_RECV_GIMBAL_CMD_CATEGORIES_NUM] = {
|
||
|
{{"GAC"}, &AP_Mount_Topotek::gimbal_angle_analyse},
|
||
|
{{"REC"}, &AP_Mount_Topotek::gimbal_record_analyse},
|
||
|
{{"SDC"}, &AP_Mount_Topotek::gimbal_sdcard_analyse},
|
||
|
{{"LRF"}, &AP_Mount_Topotek::gimbal_dist_info_analyse},
|
||
|
{{"TRC"}, &AP_Mount_Topotek::gimbal_track_analyse},
|
||
|
{{"VSN"}, &AP_Mount_Topotek::gimbal_version_analyse},
|
||
|
};
|
||
|
};
|
||
|
|
||
|
#endif // HAL_MOUNT_TOPOTEK_ENABLED
|