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 & params , 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
2023-11-15 18:36:30 -04:00
// get angular velocity of mount. Only available on some backends
virtual bool get_angular_velocity ( Vector3f & rates ) { return false ; }
2023-11-14 19:02:53 -04:00
// returns true if mode is a valid mode, false otherwise:
bool valid_mode ( MAV_MOUNT_MODE mode ) const ;
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
2023-11-14 19:02:53 -04:00
bool set_mode ( enum MAV_MOUNT_MODE mode ) ;
2022-06-20 08:24:26 -03:00
2024-03-19 07:36:38 -03:00
// 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)
2022-06-20 08:24:26 -03:00
// 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 ; }
2024-03-17 22:51:00 -03:00
# 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
virtual bool set_camera_source ( uint8_t primary_source , uint8_t secondary_source ) { return false ; }
# endif
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
} ;
2024-03-19 07:32:45 -03:00
// options parameter bitmask handling
enum class Options : uint8_t {
RCTARGETING_LOCK_FROM_PREVMODE = ( 1U < < 0 ) , // RC_TARGETING mode's lock/follow state maintained from previous mode
} ;
bool option_set ( Options opt ) const { return ( _params . options . get ( ) & ( uint8_t ) opt ) ! = 0 ; }
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
2023-12-29 03:10:52 -04:00
// change to RC_TARGETTING mode if rc inputs have changed by more than the dead zone
// should be called on every update
void set_rctargeting_on_rcinput_change ( ) ;
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)
2024-03-19 07:36:38 -03:00
bool _yaw_lock ; // yaw_lock used in RC_TARGETING mode. True if the gimbal's yaw target is maintained in earth-frame, if false (aka "follow") it is maintained in body-frame
2022-06-20 08:24:26 -03:00
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
2023-12-29 03:10:52 -04:00
// structure holding the last RC inputs
struct {
2024-04-09 21:06:35 -03:00
bool initialised ;
2023-12-29 03:10:52 -04:00
int16_t roll_in ;
int16_t pitch_in ;
int16_t yaw_in ;
} last_rc_input ;
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
2023-12-30 08:30:08 -04:00
struct mavlink_control_id_t {
2023-05-07 20:07:14 -03:00
uint8_t sysid ;
uint8_t compid ;
2023-12-30 08:30:08 -04:00
// equality operators
bool operator = = ( const mavlink_control_id_t & rhs ) const { return ( sysid = = rhs . sysid & & compid = = rhs . compid ) ; }
bool operator ! = ( const mavlink_control_id_t & rhs ) const { return ! ( * this = = rhs ) ; }
2023-05-07 20:07:14 -03:00
} mavlink_control_id ;
2015-01-08 16:11:00 -04:00
} ;
2020-07-24 14:01:21 -03:00
# endif // HAL_MOUNT_ENABLED