mirror of https://github.com/ArduPilot/ardupilot
129 lines
5.3 KiB
C++
129 lines
5.3 KiB
C++
/*
|
|
DroneCAN gimbal driver
|
|
|
|
Implements gimbal control and attitude feedback using the DroneCAN / DSDL / com / xacti messages
|
|
see https://github.com/dronecan/DSDL/tree/master/com/xacti
|
|
|
|
*/
|
|
|
|
#pragma once
|
|
|
|
#include "AP_Mount_Backend.h"
|
|
|
|
#if HAL_MOUNT_XACTI_ENABLED
|
|
|
|
#include <AP_HAL/AP_HAL.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_DroneCAN/AP_DroneCAN.h>
|
|
#include "AP_Mount.h"
|
|
|
|
class AP_Mount_Xacti : public AP_Mount_Backend
|
|
{
|
|
|
|
public:
|
|
// Constructor
|
|
AP_Mount_Xacti(class AP_Mount &frontend, class AP_Mount_Params ¶ms, uint8_t instance);
|
|
|
|
/* Do not allow copies */
|
|
CLASS_NO_COPY(AP_Mount_Xacti);
|
|
|
|
// 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 focus specified as rate, percentage or auto
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
|
SetFocusResult set_focus(FocusType focus_type, float focus_value) 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;
|
|
|
|
// subscribe to Xacti DroneCAN messages
|
|
static void subscribe_msgs(AP_DroneCAN* ap_dronecan);
|
|
|
|
// xacti specific message handlers
|
|
static void handle_gimbal_attitude_status(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GimbalAttitudeStatus &msg);
|
|
static void handle_gnss_status_req(AP_DroneCAN* ap_dronecan, const CanardRxTransfer& transfer, const com_xacti_GnssStatusReq &msg);
|
|
|
|
protected:
|
|
|
|
// get attitude as a quaternion. returns true on success
|
|
bool get_attitude_quaternion(Quaternion& att_quat) override;
|
|
|
|
private:
|
|
|
|
// 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);
|
|
|
|
// register backend in detected modules array used to map DroneCAN port and node id to backend
|
|
void register_backend();
|
|
|
|
// find backend associated with the given dronecan port and node_id. also associates backends with zero node ids
|
|
// returns pointer to backend on success, nullptr on failure
|
|
static AP_Mount_Xacti* get_dronecan_backend(AP_DroneCAN* ap_dronecan, uint8_t node_id);
|
|
|
|
// DroneCAN parameter handling methods
|
|
FUNCTOR_DECLARE(param_int_cb, bool, AP_DroneCAN*, const uint8_t, const char*, int32_t &);
|
|
FUNCTOR_DECLARE(param_save_cb, void, AP_DroneCAN*, const uint8_t, bool);
|
|
bool handle_param_get_set_response_int(AP_DroneCAN* ap_dronecan, const uint8_t node_id, const char* name, int32_t &value);
|
|
void handle_param_save_response(AP_DroneCAN* ap_dronecan, const uint8_t node_id, bool success);
|
|
|
|
// send gimbal control message via DroneCAN
|
|
// mode is 2:angle control or 3:rate control
|
|
// pitch_cd is pitch angle in centi-degrees or pitch rate in cds
|
|
// yaw_cd is angle in centi-degrees or yaw rate in cds
|
|
void send_gimbal_control(uint8_t mode, int16_t pitch_cd, int16_t yaw_cd);
|
|
|
|
// send vehicle attitude to gimbal via DroneCAN
|
|
void send_copter_att_status();
|
|
|
|
// internal variables
|
|
bool _initialised; // true once the driver has been initialised
|
|
|
|
// attitude received from gimbal
|
|
Quaternion _current_attitude_quat; // current attitude as a quaternion
|
|
uint32_t _last_current_attitude_quat_ms; // system time _current_angle_rad was updated
|
|
bool _recording_video; // true if recording video
|
|
|
|
// DroneCAN related variables
|
|
static bool _subscribed; // true once subscribed to receive DroneCAN messages
|
|
static struct DetectedModules {
|
|
AP_Mount_Xacti *driver; // pointer to Xacti backends
|
|
AP_DroneCAN* ap_dronecan; // DroneCAN interface used by this backend
|
|
uint8_t node_id; // DroneCAN node id associated by this backend
|
|
} _detected_modules[AP_MOUNT_MAX_INSTANCES];
|
|
static HAL_Semaphore _sem_registry; // semaphore protecting access to _detected_modules table
|
|
uint32_t last_send_gimbal_control_ms; // system time that send_gimbal_control was last called (used to slow down sends to 5hz)
|
|
uint32_t last_send_copter_att_status_ms; // system time that send_copter_att_status was last called (used to slow down sends to 10hz)
|
|
};
|
|
|
|
#endif // HAL_MOUNT_XACTI_ENABLED
|