2015-01-08 16:11:00 -04:00
|
|
|
/*
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
|
|
it under the terms of the GNU General Public License as published by
|
|
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
|
|
(at your option) any later version.
|
|
|
|
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
|
|
GNU General Public License for more details.
|
|
|
|
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
|
|
*/
|
|
|
|
|
|
|
|
/*
|
|
|
|
Mount driver backend class. Each supported mount type
|
|
|
|
needs to have an object derived from this class.
|
|
|
|
*/
|
2016-02-17 21:25:38 -04:00
|
|
|
#pragma once
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2023-05-26 01:16:12 -03:00
|
|
|
#include "AP_Mount_config.h"
|
|
|
|
|
2020-07-24 14:01:21 -03:00
|
|
|
#if HAL_MOUNT_ENABLED
|
2023-05-26 01:16:12 -03:00
|
|
|
|
|
|
|
#include <GCS_MAVLink/GCS_MAVLink.h>
|
2022-06-14 01:57:26 -03:00
|
|
|
#include <AP_Common/AP_Common.h>
|
2023-05-26 01:16:12 -03:00
|
|
|
#include <AP_Common/Location.h>
|
2019-04-04 07:50:00 -03:00
|
|
|
#include <RC_Channel/RC_Channel.h>
|
2023-05-26 01:16:12 -03:00
|
|
|
#include <AP_Camera/AP_Camera_shareddefs.h>
|
2023-07-21 03:35:42 -03:00
|
|
|
#include "AP_Mount.h"
|
2015-01-08 16:11:00 -04:00
|
|
|
|
|
|
|
class AP_Mount_Backend
|
|
|
|
{
|
|
|
|
public:
|
|
|
|
// Constructor
|
2023-05-26 01:16:12 -03:00
|
|
|
AP_Mount_Backend(class AP_Mount &frontend, class AP_Mount_Params ¶ms, uint8_t instance) :
|
2015-01-08 16:11:00 -04:00
|
|
|
_frontend(frontend),
|
2022-08-30 08:15:10 -03:00
|
|
|
_params(params),
|
2015-01-08 16:11:00 -04:00
|
|
|
_instance(instance)
|
|
|
|
{}
|
|
|
|
|
|
|
|
// init - performs any required initialisation for this instance
|
2023-04-20 05:22:59 -03:00
|
|
|
virtual void init();
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2023-06-21 04:24:06 -03:00
|
|
|
// set device id of this instance, for MNTx_DEVID parameter
|
|
|
|
void set_dev_id(uint32_t id);
|
|
|
|
|
2015-01-08 16:11:00 -04:00
|
|
|
// update mount position - should be called periodically
|
|
|
|
virtual void update() = 0;
|
|
|
|
|
2015-12-30 22:19:52 -04:00
|
|
|
// used for gimbals that need to read INS data at full rate
|
|
|
|
virtual void update_fast() {}
|
|
|
|
|
2022-06-16 05:13:08 -03:00
|
|
|
// return true if healthy
|
|
|
|
virtual bool healthy() const { return true; }
|
|
|
|
|
2023-04-24 01:58:58 -03:00
|
|
|
// return true if this mount accepts roll or pitch targets
|
|
|
|
virtual bool has_roll_control() const;
|
|
|
|
virtual bool has_pitch_control() const;
|
|
|
|
|
2022-08-26 00:42:17 -03:00
|
|
|
// returns true if this mount can control its pan (required for multicopters)
|
2015-01-08 16:11:00 -04:00
|
|
|
virtual bool has_pan_control() const = 0;
|
|
|
|
|
2023-02-13 02:28:55 -04:00
|
|
|
// 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.
|
2023-02-15 02:50:34 -04:00
|
|
|
virtual bool get_attitude_quaternion(Quaternion& att_quat) = 0;
|
2022-09-20 02:39:35 -03:00
|
|
|
|
2022-06-20 08:24:26 -03:00
|
|
|
// get mount's mode
|
|
|
|
enum MAV_MOUNT_MODE get_mode() const { return _mode; }
|
|
|
|
|
|
|
|
// set mount's mode
|
2022-09-16 21:28:12 -03:00
|
|
|
void set_mode(enum MAV_MOUNT_MODE mode) { _mode = mode; }
|
2022-06-20 08:24:26 -03:00
|
|
|
|
|
|
|
// set yaw_lock. 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) { _yaw_lock = yaw_lock; }
|
2015-01-12 07:41:28 -04:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// set angle target in degrees
|
|
|
|
// 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);
|
|
|
|
|
|
|
|
// 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_is_earth_frame);
|
2015-03-21 08:58:57 -03:00
|
|
|
|
2015-01-08 16:11:00 -04:00
|
|
|
// set_roi_target - sets target location that mount should attempt to point towards
|
2022-06-20 07:59:02 -03:00
|
|
|
void set_roi_target(const Location &target_loc);
|
2023-02-15 00:52:56 -04:00
|
|
|
// clear_roi_target - clears target location that mount should attempt to point towards
|
|
|
|
void clear_roi_target();
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2019-03-16 04:06:02 -03:00
|
|
|
// set_sys_target - sets system that mount should attempt to point towards
|
|
|
|
void set_target_sysid(uint8_t sysid);
|
|
|
|
|
2022-08-23 07:58:15 -03:00
|
|
|
// handle do_mount_control command. Returns MAV_RESULT_ACCEPTED on success
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT handle_command_do_mount_control(const mavlink_command_int_t &packet);
|
2023-05-07 20:07:14 -03:00
|
|
|
|
|
|
|
// handle do_gimbal_manager_configure. Returns MAV_RESULT_ACCEPTED on success
|
|
|
|
// requires original message in order to extract caller's sysid and compid
|
2023-08-17 04:19:03 -03:00
|
|
|
MAV_RESULT handle_command_do_gimbal_manager_configure(const mavlink_command_int_t &packet, const mavlink_message_t &msg);
|
2023-08-23 21:10:29 -03:00
|
|
|
|
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONFIGURE_ENABLED
|
2021-08-25 03:04:18 -03:00
|
|
|
// process MOUNT_CONFIGURE messages received from GCS. deprecated.
|
2018-10-11 06:30:03 -03:00
|
|
|
void handle_mount_configure(const mavlink_mount_configure_t &msg);
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2023-08-23 21:10:29 -03:00
|
|
|
#if AP_MAVLINK_MSG_MOUNT_CONTROL_ENABLED
|
2021-08-25 03:04:18 -03:00
|
|
|
// process MOUNT_CONTROL messages received from GCS. deprecated.
|
2018-10-11 06:30:03 -03:00
|
|
|
void handle_mount_control(const mavlink_mount_control_t &packet);
|
2023-08-23 21:10:29 -03:00
|
|
|
#endif
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2022-07-11 05:07:22 -03:00
|
|
|
// send a GIMBAL_DEVICE_ATTITUDE_STATUS message to GCS
|
|
|
|
void send_gimbal_device_attitude_status(mavlink_channel_t chan);
|
2015-01-08 16:11:00 -04:00
|
|
|
|
2023-04-24 01:58:58 -03:00
|
|
|
// return gimbal capabilities sent to GCS in the GIMBAL_MANAGER_INFORMATION
|
|
|
|
virtual uint32_t get_gimbal_manager_capability_flags() const;
|
|
|
|
|
|
|
|
// send a GIMBAL_MANAGER_INFORMATION message to GCS
|
|
|
|
void send_gimbal_manager_information(mavlink_channel_t chan);
|
|
|
|
|
2023-05-07 20:07:14 -03:00
|
|
|
// send a GIMBAL_MANAGER_STATUS message to GCS
|
|
|
|
void send_gimbal_manager_status(mavlink_channel_t chan);
|
|
|
|
|
2015-01-29 05:01:55 -04:00
|
|
|
// handle a GIMBAL_REPORT message
|
2019-04-30 07:22:48 -03:00
|
|
|
virtual void handle_gimbal_report(mavlink_channel_t chan, const mavlink_message_t &msg) {}
|
2015-01-29 05:01:55 -04:00
|
|
|
|
2015-12-30 22:19:52 -04:00
|
|
|
// handle a PARAM_VALUE message
|
2019-04-30 07:22:48 -03:00
|
|
|
virtual void handle_param_value(const mavlink_message_t &msg) {}
|
2015-12-30 22:19:52 -04:00
|
|
|
|
2021-08-25 03:04:18 -03:00
|
|
|
// handle a GLOBAL_POSITION_INT message
|
|
|
|
bool handle_global_position_int(uint8_t msg_sysid, const mavlink_global_position_int_t &packet);
|
|
|
|
|
2022-05-26 23:57:55 -03:00
|
|
|
// handle GIMBAL_DEVICE_INFORMATION message
|
|
|
|
virtual void handle_gimbal_device_information(const mavlink_message_t &msg) {}
|
|
|
|
|
2022-06-03 05:27:08 -03:00
|
|
|
// handle GIMBAL_DEVICE_ATTITUDE_STATUS message
|
|
|
|
virtual void handle_gimbal_device_attitude_status(const mavlink_message_t &msg) {}
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// get target rate in deg/sec. returns true on success
|
|
|
|
bool get_rate_target(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(float& roll_deg, float& pitch_deg, float& yaw_deg, bool& yaw_is_earth_frame);
|
|
|
|
|
2022-12-27 21:21:46 -04:00
|
|
|
// accessors for scripting backends
|
|
|
|
virtual bool get_location_target(Location &target_loc) { return false; }
|
|
|
|
virtual void set_attitude_euler(float roll_deg, float pitch_deg, float yaw_bf_deg) {};
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// write mount log packet
|
|
|
|
void write_log(uint64_t timestamp_us);
|
|
|
|
|
2022-09-27 22:28:56 -03:00
|
|
|
//
|
|
|
|
// camera controls for gimbals that include a camera
|
|
|
|
//
|
|
|
|
|
|
|
|
// take a picture. returns true on success
|
|
|
|
virtual bool take_picture() { return false; }
|
|
|
|
|
|
|
|
// start or stop video recording. returns true on success
|
|
|
|
// set start_recording = true to start record, false to stop recording
|
|
|
|
virtual bool record_video(bool start_recording) { return false; }
|
|
|
|
|
2023-04-12 09:35:43 -03:00
|
|
|
// set zoom specified as a rate or percentage
|
2023-04-18 22:06:46 -03:00
|
|
|
virtual bool set_zoom(ZoomType zoom_type, float zoom_value) { return false; }
|
2022-09-27 22:28:56 -03:00
|
|
|
|
2023-04-24 09:23:47 -03:00
|
|
|
// set focus specified as rate, percentage or auto
|
2022-09-27 22:28:56 -03:00
|
|
|
// focus in = -1, focus hold = 0, focus out = 1
|
2023-06-21 03:03:39 -03:00
|
|
|
virtual SetFocusResult set_focus(FocusType focus_type, float focus_value) { return SetFocusResult::UNSUPPORTED; }
|
2022-09-27 22:28:56 -03:00
|
|
|
|
2023-07-06 01:55:23 -03:00
|
|
|
// 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
|
|
|
|
virtual bool set_tracking(TrackingType tracking_type, const Vector2f& p1, const Vector2f& p2) { return false; }
|
|
|
|
|
2023-07-14 08:21:22 -03:00
|
|
|
// set camera lens as a value from 0 to 5
|
|
|
|
virtual bool set_lens(uint8_t lens) { return false; }
|
|
|
|
|
2023-06-12 00:10:19 -03:00
|
|
|
// send camera information message to GCS
|
|
|
|
virtual void send_camera_information(mavlink_channel_t chan) const {}
|
|
|
|
|
|
|
|
// send camera settings message to GCS
|
|
|
|
virtual void send_camera_settings(mavlink_channel_t chan) const {}
|
2023-09-11 23:23:17 -03:00
|
|
|
|
|
|
|
// send camera capture status message to GCS
|
|
|
|
virtual void send_camera_capture_status(mavlink_channel_t chan) const {}
|
2023-06-12 00:10:19 -03:00
|
|
|
|
2023-09-20 23:36:58 -03:00
|
|
|
#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);
|
|
|
|
#endif
|
|
|
|
|
2023-08-21 09:29:28 -03:00
|
|
|
//
|
|
|
|
// rangefinder
|
|
|
|
//
|
|
|
|
|
|
|
|
// get rangefinder distance. Returns true on success
|
|
|
|
virtual bool get_rangefinder_distance(float& distance_m) const { return false; }
|
|
|
|
|
2015-01-08 16:11:00 -04:00
|
|
|
protected:
|
2015-01-14 23:58:28 -04:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
enum class MountTargetType {
|
|
|
|
ANGLE,
|
|
|
|
RATE,
|
|
|
|
};
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// class for a single angle or rate target
|
|
|
|
class MountTarget {
|
|
|
|
public:
|
2022-06-23 00:37:58 -03:00
|
|
|
float roll;
|
|
|
|
float pitch;
|
|
|
|
float yaw;
|
|
|
|
bool yaw_is_ef;
|
2023-03-25 00:50:56 -03:00
|
|
|
|
|
|
|
// return body-frame yaw angle from a mount target (in radians)
|
|
|
|
float get_bf_yaw() const;
|
|
|
|
|
|
|
|
// return earth-frame yaw angle from a mount target (in radians)
|
|
|
|
float get_ef_yaw() const;
|
2023-07-21 03:35:42 -03:00
|
|
|
|
|
|
|
// set roll, pitch, yaw and yaw_is_ef from Vector3f
|
|
|
|
void set(const Vector3f& rpy, bool yaw_is_ef_in);
|
2022-06-23 00:37:58 -03:00
|
|
|
};
|
|
|
|
|
2022-08-26 00:42:17 -03:00
|
|
|
// returns true if user has configured a valid yaw angle range
|
|
|
|
// allows user to disable yaw even on 3-axis gimbal
|
2022-08-30 08:15:10 -03:00
|
|
|
bool yaw_range_valid() const { return (_params.yaw_angle_min < _params.yaw_angle_max); }
|
2022-08-26 00:42:17 -03:00
|
|
|
|
2022-07-11 05:07:22 -03:00
|
|
|
// returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
|
|
|
|
virtual bool suppress_heartbeat() const { return false; }
|
|
|
|
|
2023-09-20 23:36:58 -03:00
|
|
|
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
|
|
|
|
// calculate the Location that the gimbal is pointing at
|
|
|
|
void calculate_poi();
|
|
|
|
#endif
|
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// get pilot input (in the range -1 to +1) received through RC
|
|
|
|
void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
|
|
|
|
|
2023-07-21 03:35:42 -03:00
|
|
|
// get angle or rate targets from pilot RC
|
|
|
|
// target_type will be either ANGLE or RATE, rpy will be the target angle in deg or rate in deg/s
|
|
|
|
void get_rc_target(MountTargetType& target_type, MountTarget& rpy) const;
|
2015-01-15 01:11:17 -04:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// get angle targets (in radians) to a Location
|
|
|
|
// returns true on success, false on failure
|
|
|
|
bool get_angle_target_to_location(const Location &loc, MountTarget& angle_rad) const WARN_IF_UNUSED;
|
2015-01-15 01:11:17 -04:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// get angle targets (in radians) to ROI location
|
|
|
|
// returns true on success, false on failure
|
|
|
|
bool get_angle_target_to_roi(MountTarget& angle_rad) const WARN_IF_UNUSED;
|
2019-03-16 04:06:02 -03:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// get angle targets (in radians) to home location
|
|
|
|
// returns true on success, false on failure
|
|
|
|
bool get_angle_target_to_home(MountTarget& angle_rad) const WARN_IF_UNUSED;
|
2019-03-16 04:06:02 -03:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// get angle targets (in radians) to a vehicle with sysid of _target_sysid
|
|
|
|
// returns true on success, false on failure
|
|
|
|
bool get_angle_target_to_sysid(MountTarget& angle_rad) const WARN_IF_UNUSED;
|
2015-01-14 23:58:28 -04:00
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// update angle targets using a given rate target
|
|
|
|
// the resulting angle_rad yaw frame will match the rate_rad yaw frame
|
|
|
|
// assumes a 50hz update rate
|
|
|
|
void update_angle_target_from_rate(const MountTarget& rate_rad, MountTarget& angle_rad) const;
|
2015-01-29 02:23:16 -04:00
|
|
|
|
2022-07-11 05:07:22 -03:00
|
|
|
// helper function to provide GIMBAL_DEVICE_FLAGS for use in GIMBAL_DEVICE_ATTITUDE_STATUS message
|
|
|
|
uint16_t get_gimbal_device_flags() const;
|
|
|
|
|
2022-08-25 06:30:24 -03:00
|
|
|
// sent warning to GCS
|
|
|
|
void send_warning_to_GCS(const char* warning_str);
|
|
|
|
|
2015-01-08 16:11:00 -04:00
|
|
|
AP_Mount &_frontend; // reference to the front end which holds parameters
|
2022-08-30 08:15:10 -03:00
|
|
|
AP_Mount_Params &_params; // parameters for this backend
|
2015-01-08 16:11:00 -04:00
|
|
|
uint8_t _instance; // this instance's number
|
2022-06-20 08:24:26 -03:00
|
|
|
|
|
|
|
MAV_MOUNT_MODE _mode; // current mode (see MAV_MOUNT_MODE enum)
|
|
|
|
bool _yaw_lock; // True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
|
|
|
|
|
2022-06-23 00:37:58 -03:00
|
|
|
// structure for MAVLink Targeting angle and rate targets
|
|
|
|
struct {
|
|
|
|
MountTargetType target_type;// MAVLink targeting mode's current target type (e.g. angle or rate)
|
|
|
|
MountTarget angle_rad; // angle target in radians
|
|
|
|
MountTarget rate_rads; // rate target in rad/s
|
2023-07-21 03:35:42 -03:00
|
|
|
} mnt_target;
|
2022-06-23 00:37:58 -03:00
|
|
|
|
2023-09-20 23:36:58 -03:00
|
|
|
#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
|
|
|
|
struct {
|
|
|
|
HAL_Semaphore sem; // semaphore protecting this structure
|
|
|
|
uint32_t poi_request_ms; // system time POI was last requested
|
|
|
|
uint32_t poi_update_ms; // system time POI was calculated
|
|
|
|
Location loc; // gimbal location used for poi calculation
|
|
|
|
Location poi_loc; // location of the POI
|
|
|
|
Quaternion att_quat; // attitude quaternion of the gimbal
|
|
|
|
} poi_calculation;
|
|
|
|
#endif
|
|
|
|
|
2022-06-20 08:24:26 -03:00
|
|
|
Location _roi_target; // roi target location
|
|
|
|
bool _roi_target_set; // true if the roi target has been set
|
|
|
|
|
|
|
|
uint8_t _target_sysid; // sysid to track
|
|
|
|
Location _target_sysid_location;// sysid target location
|
|
|
|
bool _target_sysid_location_set;// true if _target_sysid has been set
|
2022-08-25 06:30:24 -03:00
|
|
|
|
|
|
|
uint32_t _last_warning_ms; // system time of last warning sent to GCS
|
2023-05-07 20:07:14 -03:00
|
|
|
|
|
|
|
// structure holding mavlink sysid and compid of controller of this gimbal
|
|
|
|
// see MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE and GIMBAL_MANAGER_STATUS
|
|
|
|
struct {
|
|
|
|
uint8_t sysid;
|
|
|
|
uint8_t compid;
|
|
|
|
} mavlink_control_id;
|
2015-01-08 16:11:00 -04:00
|
|
|
};
|
2020-07-24 14:01:21 -03:00
|
|
|
|
|
|
|
#endif // HAL_MOUNT_ENABLED
|