mirror of
https://github.com/ArduPilot/ardupilot.git
synced 2026-10-06 19:00:27 +08:00
Andrew Tridgell
7232503ee6
Send changed targets immediately and refresh unchanged targets at a configurable rate. Keep vehicle attitude output configurable with a 50 Hz default, use absolute global ROI targets and explicit yaw frames, and require a separate link for each camera/gimbal unit. Resolve gimbal selectors as component IDs (7-255) or legacy mount slots (1-6), rejecting fractional, negative, NaN and out-of-range values instead of wrapping them into another mount. Zero still selects the primary mount. Advertise MAVLink gimbals by their component ID in GIMBAL_MANAGER information and status and in camera associations. Relay cached native GIMBAL_DEVICE_ATTITUDE_STATUS with the gimbal's own identity and attitude frame. Handle explicitly addressed ROI_LOCATION, ROI_NONE and ROI_SYSID mount selection, and pass camera_device_id through mount-backed camera information and settings.
388 lines
16 KiB
C++
388 lines
16 KiB
C++
/************************************************************
|
|
* AP_mount -- library to control a 2 or 3 axis mount. *
|
|
* *
|
|
* Author: Joe Holdsworth; *
|
|
* Ritchie Wilson; *
|
|
* Amilcar Lucas; *
|
|
* Gregory Fletcher; *
|
|
* heavily modified by Randy Mackay *
|
|
* *
|
|
* Purpose: Move a 2 or 3 axis mount attached to vehicle, *
|
|
* Used for mount to track targets or stabilise *
|
|
* camera plus other modes. *
|
|
* *
|
|
* Usage: Use in main code to control mounts attached to *
|
|
* vehicle. *
|
|
* *
|
|
* Comments: All angles in degrees, distances in meters *
|
|
* unless otherwise stated. *
|
|
************************************************************/
|
|
#pragma once
|
|
|
|
#include "AP_Mount_config.h"
|
|
|
|
#if HAL_MOUNT_ENABLED
|
|
|
|
#include <GCS_MAVLink/GCS_config.h>
|
|
#include <AP_HAL/AP_HAL_Boards.h>
|
|
#include <AP_Math/AP_Math.h>
|
|
#include <AP_Common/AP_Common.h>
|
|
#include <AP_Common/Location.h>
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
|
#include <AP_Camera/AP_Camera_shareddefs.h>
|
|
#include "AP_Mount_Params.h"
|
|
|
|
// maximum number of mounts
|
|
#define AP_MOUNT_MAX_INSTANCES 2
|
|
|
|
// declare backend classes
|
|
class AP_Mount_Backend;
|
|
class AP_Mount_Servo;
|
|
class AP_Mount_SoloGimbal;
|
|
class AP_Mount_Alexmos;
|
|
class AP_Mount_SToRM32;
|
|
class AP_Mount_SToRM32_serial;
|
|
class AP_Mount_MAVLink;
|
|
class AP_Mount_Siyi;
|
|
class AP_Mount_Scripting;
|
|
class AP_Mount_Xacti;
|
|
class AP_Mount_Viewpro;
|
|
class AP_Mount_Topotek;
|
|
class AP_Mount_CADDX;
|
|
class AP_Mount_XFRobot;
|
|
|
|
/*
|
|
This is a workaround to allow the MAVLink backend access to the
|
|
SmallEKF. It would be nice to find a neater solution to this
|
|
*/
|
|
|
|
class AP_Mount
|
|
{
|
|
// declare backends as friends
|
|
friend class AP_Mount_Backend;
|
|
friend class AP_Mount_Servo;
|
|
friend class AP_Mount_SoloGimbal;
|
|
friend class AP_Mount_Alexmos;
|
|
friend class AP_Mount_SToRM32;
|
|
friend class AP_Mount_SToRM32_serial;
|
|
friend class AP_Mount_MAVLink;
|
|
friend class AP_Mount_Siyi;
|
|
friend class AP_Mount_Scripting;
|
|
friend class AP_Mount_Xacti;
|
|
friend class AP_Mount_Viewpro;
|
|
friend class AP_Mount_Topotek;
|
|
friend class AP_Mount_CADDX;
|
|
|
|
public:
|
|
AP_Mount();
|
|
|
|
/* Do not allow copies */
|
|
CLASS_NO_COPY(AP_Mount);
|
|
|
|
// get singleton instance
|
|
static AP_Mount *get_singleton() {
|
|
return _singleton;
|
|
}
|
|
|
|
// Enums
|
|
enum class Type {
|
|
None = 0, /// no mount
|
|
#if HAL_MOUNT_SERVO_ENABLED
|
|
Servo = 1, /// servo controlled mount
|
|
#endif
|
|
#if HAL_SOLO_GIMBAL_ENABLED
|
|
SoloGimbal = 2, /// Solo's gimbal
|
|
#endif
|
|
#if HAL_MOUNT_ALEXMOS_ENABLED
|
|
Alexmos = 3, /// Alexmos mount
|
|
#endif
|
|
#if HAL_MOUNT_STORM32MAVLINK_ENABLED
|
|
SToRM32 = 4, /// SToRM32 mount using MAVLink protocol
|
|
#endif
|
|
#if HAL_MOUNT_STORM32SERIAL_ENABLED
|
|
SToRM32_serial = 5, /// SToRM32 mount using custom serial protocol
|
|
#endif
|
|
#if HAL_MOUNT_MAVLINK_ENABLED
|
|
MAVLink = 6, /// MAVLink v2 Gimbal protocol
|
|
#endif
|
|
#if HAL_MOUNT_SERVO_ENABLED
|
|
BrushlessPWM = 7, /// Brushless (stabilized) gimbal using PWM protocol
|
|
#endif
|
|
#if HAL_MOUNT_SIYI_ENABLED
|
|
Siyi = 8, /// Siyi gimbal using custom serial protocol
|
|
#endif
|
|
#if HAL_MOUNT_SCRIPTING_ENABLED
|
|
Scripting = 9, /// Scripting gimbal driver
|
|
#endif
|
|
#if HAL_MOUNT_XACTI_ENABLED
|
|
Xacti = 10, /// Xacti DroneCAN gimbal driver
|
|
#endif
|
|
#if HAL_MOUNT_VIEWPRO_ENABLED
|
|
Viewpro = 11, /// Viewpro gimbal using a custom serial protocol
|
|
#endif
|
|
#if HAL_MOUNT_TOPOTEK_ENABLED
|
|
Topotek = 12, /// Topotek gimbal using a custom serial protocol
|
|
#endif
|
|
#if HAL_MOUNT_CADDX_ENABLED
|
|
CADDX = 13, /// CADDX gimbal using a custom serial protocol
|
|
#endif
|
|
#if HAL_MOUNT_XFROBOT_ENABLED
|
|
XFRobot = 14, /// XFRobot gimbal using a custom serial protocol
|
|
#endif
|
|
};
|
|
|
|
// init - detect and initialise all mounts
|
|
__INITFUNC__ void init();
|
|
|
|
// update - give mount opportunity to update servos. should be called at 10hz or higher
|
|
void update();
|
|
|
|
// used for gimbals that need to read INS data at full rate
|
|
void update_fast();
|
|
|
|
// return primary instance ID
|
|
uint8_t get_primary_instance() const { return _primary; }
|
|
|
|
// MAVLink identity, with legacy numbered selectors accepted on input.
|
|
uint8_t get_device_id(uint8_t instance) const;
|
|
bool get_instance_from_device_id(float device_id, uint8_t &instance) const;
|
|
|
|
// get_mount_type - returns the type of mount
|
|
Type get_mount_type() const { return get_mount_type(_primary); }
|
|
Type get_mount_type(uint8_t instance) const;
|
|
|
|
// has_pan_control - returns true if the mount has yaw control (required for copters)
|
|
bool has_pan_control() const { return has_pan_control(_primary); }
|
|
bool has_pan_control(uint8_t instance) const;
|
|
|
|
// get_mode - returns current mode of mount (i.e. Retracted, Neutral, RC_Targeting, GPS Point)
|
|
enum MAV_MOUNT_MODE get_mode() const { return get_mode(_primary); }
|
|
enum MAV_MOUNT_MODE get_mode(uint8_t instance) const;
|
|
|
|
// set_mode - sets mount's mode
|
|
// returns true if mode is successfully set
|
|
void set_mode(enum MAV_MOUNT_MODE mode) { return set_mode(_primary, mode); }
|
|
void set_mode(uint8_t instance, enum MAV_MOUNT_MODE mode);
|
|
|
|
// set_mode_to_default - restores the mode to it's default mode held in the MNTx_DEFLT_MODE parameter
|
|
// this operation requires 60us on a Pixhawk/PX4
|
|
void set_mode_to_default() { set_mode_to_default(_primary); }
|
|
void set_mode_to_default(uint8_t instance);
|
|
|
|
// set pitch_lock used in RC_TARGETING mode. If true, the gimbal's pitch target is maintained in earth-frame (horizon does not move up and down) rather than body frame
|
|
void set_pitch_lock(bool pitch_lock) { set_pitch_lock(_primary, pitch_lock); }
|
|
void set_pitch_lock(uint8_t instance, bool pitch_lock);
|
|
|
|
// set roll_lock used in RC_TARGETING mode. If true, the gimbal's roll target is maintained in earth-frame (horizon does not rotate) rather than body frame
|
|
void set_roll_lock(bool roll_lock) { set_roll_lock(_primary, roll_lock); }
|
|
void set_roll_lock(uint8_t instance, bool roll_lock);
|
|
|
|
// set yaw_lock used in RC_TARGETING mode. If true, the gimbal's yaw target is maintained in earth-frame meaning it will lock onto an earth-frame heading (e.g. North)
|
|
// If false (aka "follow") the gimbal's yaw is maintained in body-frame meaning it will rotate with the vehicle
|
|
void set_yaw_lock(bool yaw_lock) { set_yaw_lock(_primary, yaw_lock); }
|
|
void set_yaw_lock(uint8_t instance, bool yaw_lock);
|
|
|
|
#if AP_MOUNT_POI_LOCK_ENABLED
|
|
// controls POI lock which locks gimbal to GPS point currently in view and switches to GPS Targeting mode, suspends tracking poi or clears it
|
|
void set_poi_lock() { set_poi_lock(_primary); }
|
|
void set_poi_lock(uint8_t instance);
|
|
void clear_poi_lock() { clear_poi_lock(_primary); }
|
|
void clear_poi_lock(uint8_t instance);
|
|
void suspend_poi_lock() { suspend_poi_lock(_primary); }
|
|
void suspend_poi_lock(uint8_t instance);
|
|
#endif
|
|
|
|
// set angle target in degrees
|
|
// roll and pitch are in earth-frame
|
|
// yaw_is_earth_frame (aka yaw_lock) should be true if yaw angle is earth-frame, false if body-frame
|
|
void set_angle_target(float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame) { set_angle_target(_primary, roll_deg, pitch_deg, yaw_deg, yaw_is_earth_frame); }
|
|
void set_angle_target(uint8_t instance, float roll_deg, float pitch_deg, float yaw_deg, bool yaw_is_earth_frame);
|
|
|
|
// sets rate target in deg/s
|
|
// yaw_lock should be true if the yaw rate is earth-frame, false if body-frame (e.g. rotates with body of vehicle)
|
|
void set_rate_target(float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock) { set_rate_target(_primary, roll_degs, pitch_degs, yaw_degs, yaw_lock); }
|
|
void set_rate_target(uint8_t instance, float roll_degs, float pitch_degs, float yaw_degs, bool yaw_lock);
|
|
|
|
// set_roi_target - sets target location that mount should attempt to point towards
|
|
void set_roi_target(const Location &target_loc) { set_roi_target(_primary,target_loc); }
|
|
void set_roi_target(uint8_t instance, const Location &target_loc);
|
|
|
|
// clear_roi_target - clears target location that mount should attempt to point towards
|
|
void clear_roi_target() { clear_roi_target(_primary); }
|
|
void clear_roi_target(uint8_t instance);
|
|
|
|
// point at system ID sysid
|
|
void set_target_sysid(uint8_t sysid) { set_target_sysid(_primary, sysid); }
|
|
void set_target_sysid(uint8_t instance, uint8_t sysid);
|
|
|
|
// Handle ROI_LOCATION/NONE addressed to a particular mount.
|
|
MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet, const Location &roi_loc);
|
|
|
|
// handling of set_roi_sysid message
|
|
MAV_RESULT handle_command_do_set_roi_sysid(const mavlink_command_int_t &packet);
|
|
|
|
#if AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
|
|
// point at next waypoint, with attitude offsets:
|
|
void set_roi_target_wpnext_offset(const Vector3f &rpy) { set_roi_target_wpnext_offset(_primary, rpy); }
|
|
void set_roi_target_wpnext_offset(uint8_t instance, const Vector3f &rpy);
|
|
#endif // AP_MOUNT_ROI_WPNEXT_OFFSET_ENABLED
|
|
|
|
// mavlink message handling:
|
|
MAV_RESULT handle_command(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
|
|
void handle_param_value(const mavlink_message_t &msg);
|
|
void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
|
|
|
|
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
|
|
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
|
|
|
|
// send a GIMBAL_MANAGER_INFORMATION message to GCS
|
|
void send_gimbal_manager_information(mavlink_channel_t chan);
|
|
|
|
// send a GIMBAL_MANAGER_STATUS message to GCS
|
|
void send_gimbal_manager_status(mavlink_channel_t chan);
|
|
|
|
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
|
|
// get poi information. Returns true on success and fills in gimbal attitude, location and poi location
|
|
bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
|
|
#endif
|
|
|
|
// get attitude as a quaternion. returns true on success.
|
|
// att_quat will be an earth-frame quaternion rotated such that
|
|
// yaw is in body-frame.
|
|
bool get_attitude_quaternion(uint8_t instance, Quaternion& att_quat);
|
|
|
|
// get mount's current attitude in euler angles in degrees. yaw angle is in body-frame
|
|
// returns true on success
|
|
bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
|
|
|
|
// run pre-arm check. returns false on failure and fills in failure_msg
|
|
// any failure_msg returned will not include a prefix
|
|
bool pre_arm_checks(char *failure_msg, uint8_t failure_msg_len);
|
|
|
|
#if AP_SCRIPTING_ENABLED
|
|
// get target rate in deg/sec. returns true on success
|
|
bool get_rate_target(uint8_t instance, float& roll_degs, float& pitch_degs, float& yaw_degs, bool& yaw_is_earth_frame);
|
|
|
|
// get target angle in deg. returns true on success
|
|
bool get_angle_target(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
|
|
|
|
// get mount target location. returns true on success
|
|
bool get_location_target(uint8_t instance, Location& target_loc);
|
|
#endif
|
|
|
|
// accessors for scripting backends and logging
|
|
void set_attitude_euler(uint8_t instance, float roll_deg, float pitch_deg, float yaw_bf_deg);
|
|
|
|
#if AP_SCRIPTING_ENABLED
|
|
// used by a script sending messages to a physical device to
|
|
// indicate what targets can be either natively sent to the device
|
|
// or internally handled by the script
|
|
void set_natively_supported_mount_target_types(uint8_t instance, uint8_t types_mask);
|
|
#endif // AP_SCRIPTING_ENABLED
|
|
|
|
// write mount log packet for all backends
|
|
void write_log();
|
|
|
|
// write mount log packet for a single backend (called by camera library)
|
|
void write_log(uint8_t instance, uint64_t timestamp_us);
|
|
|
|
//
|
|
// camera controls for gimbals that include a camera
|
|
//
|
|
|
|
// take a picture
|
|
bool take_picture(uint8_t instance);
|
|
|
|
// start or stop video recording
|
|
// set start_recording = true to start record, false to stop recording
|
|
bool record_video(uint8_t instance, bool start_recording);
|
|
|
|
// set zoom specified as a rate or percentage
|
|
bool set_zoom(uint8_t instance, ZoomType zoom_type, float zoom_value);
|
|
|
|
// set focus specified as rate, percentage or auto
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
|
SetFocusResult set_focus(uint8_t instance, FocusType focus_type, float focus_value);
|
|
|
|
// 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(uint8_t instance, TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2);
|
|
|
|
// set camera lens as a value from 0 to 5
|
|
bool set_lens(uint8_t instance, uint8_t lens);
|
|
|
|
#if HAL_MOUNT_SET_CAMERA_SOURCE_ENABLED
|
|
// 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 instance, uint8_t primary_source, uint8_t secondary_source);
|
|
#endif
|
|
|
|
// send camera information message to GCS
|
|
void send_camera_information(uint8_t instance, mavlink_channel_t chan, uint8_t camera_device_id) const;
|
|
|
|
// send camera settings message to GCS
|
|
void send_camera_settings(uint8_t instance, mavlink_channel_t chan, uint8_t camera_device_id) const;
|
|
|
|
// send camera capture status message to GCS
|
|
void send_camera_capture_status(uint8_t instance, mavlink_channel_t chan) const;
|
|
|
|
#if AP_MOUNT_SEND_THERMAL_RANGE_ENABLED
|
|
// send camera thermal range message to GCS
|
|
void send_camera_thermal_range(uint8_t instance, mavlink_channel_t chan, uint8_t camera_device_id) const;
|
|
#endif
|
|
|
|
// change camera settings not normally used by autopilot
|
|
// setting values from AP_Camera::Setting enum
|
|
bool change_setting(uint8_t instance, CameraSetting setting, float value);
|
|
|
|
//
|
|
// rangefinder
|
|
//
|
|
|
|
// get rangefinder distance. Returns true on success
|
|
bool get_rangefinder_distance(uint8_t instance, float& distance_m) const;
|
|
|
|
// enable/disable rangefinder. Returns true on success
|
|
bool set_rangefinder_enable(uint8_t instance, bool enable);
|
|
|
|
// parameter var table
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
protected:
|
|
|
|
static AP_Mount *_singleton;
|
|
|
|
// parameters for backends
|
|
AP_Mount_Params _params[AP_MOUNT_MAX_INSTANCES];
|
|
|
|
// front end members
|
|
uint8_t _num_instances; // number of mounts instantiated
|
|
uint8_t _primary; // primary mount
|
|
AP_Mount_Backend *_backends[AP_MOUNT_MAX_INSTANCES]; // pointers to instantiated mounts
|
|
|
|
private:
|
|
// Check if instance backend is ok
|
|
AP_Mount_Backend *get_primary() const;
|
|
AP_Mount_Backend *get_instance(uint8_t instance) const;
|
|
AP_Mount_Backend *mount_device_from_mavlink_gimbal_id(float gimbal_device_id) const;
|
|
|
|
void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg);
|
|
|
|
MAV_RESULT handle_command_do_mount_configure(const mavlink_command_int_t &packet);
|
|
MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
|
|
MAV_RESULT handle_command_do_gimbal_manager_pitchyaw(const mavlink_command_int_t &packet);
|
|
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
|
|
void handle_gimbal_manager_set_attitude(const mavlink_message_t &msg);
|
|
void handle_gimbal_manager_set_pitchyaw(const mavlink_message_t &msg);
|
|
void handle_global_position_int(const mavlink_message_t &msg);
|
|
void handle_gimbal_device_information(const mavlink_message_t &msg);
|
|
void handle_gimbal_device_attitude_status(const mavlink_message_t &msg);
|
|
};
|
|
|
|
namespace AP {
|
|
AP_Mount *mount();
|
|
};
|
|
|
|
#endif // HAL_MOUNT_ENABLED
|