libraries/AP_Mount/AP_Mount_Viewpro.h

   1 /*
   2   Viewpro gimbal driver using custom serial protocol
   3
   4   Packet format (courtesy of Viewpro's SDK document)
   5
   6   -------------------------------------------------------------------------------------------
   7   Field     Index   Bytes       Description
   8   -------------------------------------------------------------------------------------------
   9   Header    0~2     3           0x55 0xAA 0xDC
  10   Length    3       1           bit0~5: body length, n=all bytes from byte3 to checksum, min=4, max=63
  11                                 bits6~7: frame counter (increment by 1 compared to previous message sent)
  12   Frame Id  4       1
  13   Data      5~n+1   n           1st byte is command id (?)
  14   Checksum  n+2     1           XOR of byte3 to n+1 (inclusive)
  15  */
  16
  17 #pragma once
  18
  19 #include "AP_Mount_config.h"
  20
  21 #if HAL_MOUNT_VIEWPRO_ENABLED
  22
  23 #include "AP_Mount_Backend_Serial.h"
  24
  25 #include <AP_HAL/AP_HAL.h>
  26 #include <AP_Math/AP_Math.h>
  27 #include <AP_Common/AP_Common.h>
  28 #include <AP_HAL/utility/sparse-endian.h>
  29
  30 #define AP_MOUNT_VIEWPRO_PACKETLEN_MAX  63  // maximum number of bytes in a packet sent to or received from the gimbal
  31
  32 class AP_Mount_Viewpro : public AP_Mount_Backend_Serial
  33 {
  34
  35 public:
  36     // Constructor
  37     using AP_Mount_Backend_Serial::AP_Mount_Backend_Serial;
  38
  39     /* Do not allow copies */
  40     CLASS_NO_COPY(AP_Mount_Viewpro);
  41
  42     // update mount position - should be called periodically
  43     void update() override;
  44
  45     // return true if healthy
  46     bool healthy() const override;
  47
  48     // return true if this mount accepts roll targets
  49     bool has_roll_control() const override { return false; }
  50
  51     // has_pan_control - returns true if this mount can control its pan (required for multicopters)
  52     bool has_pan_control() const override { return yaw_range_valid(); };
  53
  54     //
  55     // camera controls
  56     //
  57
  58     // take a picture.  returns true on success
  59     bool take_picture() override;
  60
  61     // start or stop video recording
  62     // set start_recording = true to start record, false to stop recording
  63     bool record_video(bool start_recording) override;
  64
  65     // set zoom specified as a rate or percentage
  66     bool set_zoom(ZoomType zoom_type, float zoom_value) override;
  67
  68     // set focus specified as rate, percentage or auto
  69     // focus in = -1, focus hold = 0, focus out = 1
  70     SetFocusResult set_focus(FocusType focus_type, float focus_value) override;
  71
  72     // set tracking to none, point or rectangle (see TrackingType enum)
  73     // if POINT only p1 is used, if RECTANGLE then p1 is top-left, p2 is bottom-right
  74     // p1,p2 are in range 0 to 1.  0 is left or top, 1 is right or bottom
  75     bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) override;
  76
  77     // set camera lens as a value from 0 to 5
  78     bool set_lens(uint8_t lens) override;
  79
  80     // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
  81     // primary and secondary sources use the AP_Camera::CameraSource enum cast to uint8_t
  82     bool set_camera_source(uint8_t primary_source, uint8_t secondary_source) override;
  83
  84     // send camera information message to GCS
  85     void send_camera_information(mavlink_channel_t chan) const override;
  86
  87     // send camera settings message to GCS
  88     void send_camera_settings(mavlink_channel_t chan) const override;
  89
  90     //
  91     // rangefinder
  92     //
  93
  94     // get rangefinder distance.  Returns true on success
  95     bool get_rangefinder_distance(float& distance_m) const override;
  96
  97     // enable/disable rangefinder.  Returns true on success
  98     bool set_rangefinder_enable(bool enable) override;
  99
 100 protected:
 101
 102     // get attitude as a quaternion.  returns true on success
 103     bool get_attitude_quaternion(Quaternion& att_quat) override;
 104
 105 private:
 106
 107     // send text prefix string to reduce flash cost
 108     static const char* send_text_prefix;
 109
 110     // packet frame ids
 111     enum class FrameId : uint8_t {
 112         HANDSHAKE = 0x00,       // handshake sent to gimbal
 113         U = 0x01,               // communication configuration control (this packet is sent to gimbal)
 114         V = 0x02,               // communication configuration status (this is the reply to U)
 115         HEARTBEAT = 0x10,       // heartbeat received from gimbal
 116         A1 = 0x1A,              // target angles (sent)
 117         C1 = 0x1C,              // camera controls commonly used (sent)
 118         E1 = 0x1E,              // tracking controls commonly used (sent)
 119         C2 = 0x2C,              // camera controls infrequently used (sent)
 120         E2 = 0x2E,              // tracking controls infrequently used (sent)
 121         T1_F1_B1_D1 = 0x40,     // actual roll, pitch, yaw angles (received)
 122         M_AHRS = 0xB1,          // vehicle attitude and position (sent)
 123     };
 124
 125     // U communication configuration control commands
 126     enum class CommConfigCmd : uint8_t {
 127         QUERY_FIRMWARE_VER = 0xD0,
 128         QUERY_MODEL = 0xE4,
 129     };
 130
 131     // A1 servo status enum (used in A1, B1 packets)
 132     enum class ServoStatus : uint8_t {
 133         MANUAL_SPEED_MODE = 0x01,
 134         FOLLOW_YAW = 0x03,
 135         MANUAL_ABSOLUTE_ANGLE_MODE = 0x0B,
 136         FOLLOW_YAW_DISABLE = 0x0A,
 137     };
 138
 139     // C1 image sensor choice
 140     enum class ImageSensor : uint8_t {
 141         NO_ACTION = 0x00,   // no image sensor is affected
 142         EO1 = 0x01,         // electro-optical, aka rgb
 143         IR = 0x02,          // infrared, aka thermal
 144         EO1_IR_PIP = 0x03,  // rgb is main, IR is picture-in-picture
 145         IR_EO1_PIP = 0x04,  // thermal is main, rgb is picture-in-picture
 146         FUSION = 0x05,      // rgb and thermal are fused
 147         IR1_13MM = 0x06,    // only valid for 1352 module
 148         IR2_52MM = 0x07,    // only valid for 1352 module
 149     };
 150
 151     // C1 camera commands
 152     enum class CameraCommand : uint8_t {
 153         NO_ACTION = 0x00,
 154         STOP_FOCUS_AND_ZOOM = 0x01,
 155         ZOOM_OUT = 0x08,
 156         ZOOM_IN = 0x09,
 157         FOCUS_PLUS = 0x0A,
 158         FOCUS_MINUS = 0x0B,
 159         TAKE_PICTURE = 0x13,
 160         START_RECORD = 0x14,
 161         STOP_RECORD = 0x15,
 162         AUTO_FOCUS = 0x19,
 163         MANUAL_FOCUS = 0x1A,
 164         IR_ZOOM_OUT = 0x1B,
 165         IR_ZOOM_IN = 0x1C
 166     };
 167
 168     // C1 rangefinder commands
 169     enum class LRFCommand : uint8_t {
 170         NO_ACTION = 0x00,
 171         SINGLE_RANGING = 0x01,
 172         CONTINUOUS_RANGING_START = 0x02,
 173         LPCL_CONTINUOUS_RANGING_START = 0x03,
 174         STOP_RANGING = 0x05
 175     };
 176
 177     // C2 camera commands
 178     enum class CameraCommand2 : uint8_t {
 179         SET_EO_ZOOM = 0x53
 180     };
 181
 182     // D1 recording status (received from gimbal)
 183     enum class RecordingStatus : uint8_t {
 184         RECORDING_STOPPED = 0x00,
 185         RECORDING = 0x01,
 186         PICTURE_MODE = 0x02
 187     };
 188
 189     // E1 tracking commands
 190     enum class TrackingCommand : uint8_t {
 191         STOP = 0x01,
 192         START = 0x03,
 193         SET_POINT = 0x0A,
 194         SET_RECT_TOPLEFT = 0x0B,
 195         SET_RECT_BOTTOMRIGHT = 0x0C,
 196     };
 197
 198     // E1 tracking source (e.g. which camera)
 199     enum class TrackingSource : uint8_t {
 200         EO1 = 0x01,         // electro-optical, aka rgb
 201         IR = 0x02,          // infrared, aka thermal
 202         EO2 = 0x03,         // electro-optical, aka rgb
 203     };
 204
 205     // E2 tracking commands2
 206     enum class TrackingCommand2 : uint8_t {
 207         SET_POINT = 0x0A,
 208         SET_RECT_TOPLEFT = 0x0B,
 209         SET_RECT_BOTTOMRIGHT = 0x0C,
 210     };
 211
 212     // F1 tracking status (received from gimbal)
 213     enum class TrackingStatus : uint8_t {
 214         STOPPED = 0x00,     // not tracking
 215         SEARCHING = 0x01,   // searching
 216         TRACKING = 0x02,    // locked onto a target
 217         LOST = 0x03,        // lost target
 218     };
 219
 220     // parsing state
 221     enum class ParseState : uint8_t {
 222         WAITING_FOR_HEADER1,
 223         WAITING_FOR_HEADER2,
 224         WAITING_FOR_HEADER3,
 225         WAITING_FOR_LENGTH,
 226         WAITING_FOR_FRAMEID,
 227         WAITING_FOR_DATA,
 228         WAITING_FOR_CRC
 229     };
 230
 231     // packet formats
 232     union HandshakePacket {
 233         struct {
 234             FrameId frame_id;           // always 0x00
 235             uint8_t unused;             // always 0x00
 236         } content;
 237         uint8_t bytes[sizeof(content)];
 238     };
 239
 240     // U packed used to send communication configuration control commands
 241     // gimbal replies with V packet
 242     union UPacket {
 243         struct {
 244             FrameId frame_id;           // always 0x01
 245             CommConfigCmd control_cmd;  // see CommConfigCmd enum above
 246             uint8_t params[9];          // parameters (unused)
 247         } content;
 248         uint8_t bytes[sizeof(content)];
 249     };
 250
 251     // A1 used to send target angles and rates
 252     union A1Packet {
 253         struct {
 254             FrameId frame_id;           // always 0x1A
 255             ServoStatus servo_status;   // see ServoStatus enum above
 256             be16_t yaw_be;              // target yaw angle or rate msb
 257             be16_t pitch_be;            // target pitch angle or rate msb
 258             uint8_t unused[4];          // unused
 259         } content;
 260         uint8_t bytes[sizeof(content)];
 261     };
 262
 263     // C1 used to send camera commands (commonly used)
 264     union C1Packet {
 265         struct PACKED {
 266             FrameId frame_id;           // always 0x1C
 267             be16_t sensor_zoom_cmd_be;
 268         } content;
 269         uint8_t bytes[sizeof(content)];
 270     };
 271
 272     // C2 used to send camera commands (less commonly used)
 273     union C2Packet {
 274         struct {
 275             FrameId frame_id;           // always 0x2C
 276             CameraCommand2 cmd;         // see CameraCommand2 enum above
 277             be16_t value_be;            // value
 278         } content;
 279         uint8_t bytes[sizeof(content)];
 280     };
 281
 282     // E1 used to send tracking commands
 283     union E1Packet {
 284         struct {
 285             FrameId frame_id;           // always 0x1E
 286             TrackingSource source : 3;  // see TrackingSource enum above
 287             uint8_t unused : 5;         // param1 (unused)
 288             TrackingCommand cmd;        // see TrackingCommand enum above
 289             uint8_t param2;             // param2
 290         } content;
 291         uint8_t bytes[sizeof(content)];
 292     };
 293
 294     // E2 used to send tracking commands2
 295     union E2Packet {
 296         struct {
 297             FrameId frame_id;           // always 0x2E
 298             TrackingCommand2 cmd;       // see TrackingCommand2 enum above
 299             be16_t param1_be;
 300             be16_t param2_be;
 301         } content;
 302         uint8_t bytes[sizeof(content)];
 303     };
 304
 305     // M_AHRS used to send vehicle attitude and position to gimbal
 306     union M_AHRSPacket {
 307         struct PACKED {
 308             FrameId frame_id;           // always 0xB1
 309             uint8_t data_type;          // should be 0x07.  Bit0: Attitude, Bit1: GPS, Bit2 Gyro
 310             uint8_t unused2to8[7];      // unused
 311             be16_t roll_be;             // vehicle roll angle.  1bit=360deg/65536
 312             be16_t pitch_be;            // vehicle pitch angle.  1bit=360deg/65536
 313             be16_t yaw_be;              // vehicle yaw angle.  1bit=360deg/65536
 314             be16_t date_be;             // bit0~6:year, bit7~10:month, bit11~15:day
 315             uint8_t seconds_utc[3];     // seconds.  1bit = 0.01sec
 316             be16_t gps_yaw_be;          // GPS yaw
 317             uint8_t position_mark_bitmask;  // bit0:new position, bit1:clock fix calced, bit2:horiz calced, bit3:alt calced
 318             be32_t latitude_be;         // latitude.  1bit = 10e-7
 319             be32_t longitude_be;        // longitude.  1bit = 10e-7
 320             be32_t height_be;           // height.  1bit = 1mm
 321             be16_t ground_speed_N_be;   // ground speed in North direction. 1bit = 0.01m/s
 322             be16_t ground_speed_E_be;   // ground speed in East direction. 1bit = 0.01m/s
 323             be16_t vdop_be;             // GPS vdop. 1bit = 0.01
 324             be16_t ground_speed_D_be;   // speed downwards. 1bit = 0.01m/s
 325         } content;
 326         uint8_t bytes[sizeof(content)];
 327     };
 328
 329     // reading incoming packets from gimbal and confirm they are of the correct format
 330     // results are held in the _parsed_msg structure
 331     void read_incoming_packets();
 332
 333     // process successfully decoded packets held in the _parsed_msg structure
 334     void process_packet();
 335
 336     // calculate crc of the received message
 337     uint8_t calc_crc(const uint8_t *buf, uint8_t length) const;
 338
 339     // get the length and frame count byte (3rd byte of all messages)
 340     // length is all bytes after the header including CRC
 341     uint8_t get_length_and_frame_count_byte(uint8_t length);
 342
 343     // send packet to gimbal
 344     // returns true on success, false if outgoing serial buffer is full
 345     bool send_packet(const uint8_t* databuff, uint8_t databuff_len);
 346
 347     // send handshake, gimbal will respond with T1_F1_B1_D1 packet that includes current angles
 348     void send_handshake();
 349
 350     // set gimbal's lock vs follow mode
 351     // lock should be true if gimbal should maintain an earth-frame target
 352     // lock is false to follow / maintain a body-frame target
 353     bool set_lock(bool lock);
 354
 355     // send communication configuration command (aka U packet), gimbal will respond with a V packet
 356     bool send_comm_config_cmd(CommConfigCmd cmd);
 357
 358     // send target pitch and yaw rates to gimbal
 359     // yaw_is_ef should be true if yaw_rads target is an earth frame rate, false if body_frame
 360     bool send_target_rates(float pitch_rads, float yaw_rads, bool yaw_is_ef);
 361
 362     // send target pitch and yaw angles to gimbal
 363     // yaw_is_ef should be true if yaw_rad target is an earth frame angle, false if body_frame
 364     bool send_target_angles(float pitch_rad, float yaw_rad, bool yaw_is_ef);
 365
 366     // send camera command, affected image sensor and value (e.g. zoom speed)
 367     bool send_camera_command(ImageSensor img_sensor, CameraCommand cmd, uint8_t value, LRFCommand lrf_cmd = LRFCommand::NO_ACTION);
 368
 369     // send camera command2 and corresponding value (e.g. zoom as absolute value)
 370     bool send_camera_command2(CameraCommand2 cmd, uint16_t value);
 371
 372     // send tracking command and corresponding value (normally zero)
 373     bool send_tracking_command(TrackingCommand cmd, uint8_t value);
 374
 375     // send camera command2 and corresponding parameter values
 376     bool send_tracking_command2(TrackingCommand2 cmd, uint16_t param1, uint16_t param2);
 377
 378     // send vehicle attitude and position to gimbal
 379     bool send_m_ahrs();
 380
 381     // internal variables
 382     uint8_t _msg_buff[AP_MOUNT_VIEWPRO_PACKETLEN_MAX];  // buffer holding latest bytes from gimbal
 383     uint8_t _msg_buff_len;                          // number of bytes held in msg buff
 384     const uint8_t _msg_buff_data_start = 2;         // data starts at this byte of _msg_buff
 385
 386     // parser state and unpacked fields
 387     struct {
 388         uint8_t data_len;                           // expected number of data bytes
 389         uint8_t frame_id;                           // frame id (equivalent to command id)
 390         uint16_t data_bytes_received;               // number of data bytes received so far
 391         uint8_t crc;                                // latest message's crc
 392         ParseState state;                           // state of incoming message processing
 393     } _parsed_msg;
 394
 395     // variables for sending packets to gimbal
 396     uint8_t _last_frame_counter;                    // frame counter sent in last message
 397     uint32_t _last_update_ms;                       // system time (in milliseconds) that angle or rate targets were last sent
 398     Vector3f _current_angle_rad;                    // current angles in radians received from gimbal (x=roll, y=pitch, z=yaw)
 399     uint32_t _last_current_angle_rad_ms;            // system time _current_angle_rad was updated (used for health reporting)
 400     bool _recording;                                // recording status received from gimbal
 401     bool _last_lock;                                // last lock mode sent to gimbal
 402     TrackingStatus _last_tracking_status;           // last tracking status received from gimbal (used to notify users)
 403     ImageSensor _image_sensor;                      // user selected image sensor (aka camera lens)
 404     float _zoom_times;                              // zoom times received from gimbal
 405     uint32_t _firmware_version;                     // firmware version from gimbal
 406     bool _got_firmware_version;                     // true once we have received the firmware version
 407     uint8_t _model_name[11] {};                     // model name received from gimbal
 408     bool _got_model_name;                           // true once we have received model name
 409     float _rangefinder_dist_m;                      // latest rangefinder distance (in meters)
 410 };
 411
 412 #endif // HAL_MOUNT_VIEWPRO_ENABLED