mirror of https://github.com/ArduPilot/ardupilot
118 lines
4.1 KiB
C++
118 lines
4.1 KiB
C++
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
|
|
|
|
/************************************************************
|
|
* AP_mount -- library to control a 2 or 3 axis mount. *
|
|
* *
|
|
* Author: Joe Holdsworth; *
|
|
* Ritchie Wilson; *
|
|
* Amilcar Lucas; *
|
|
* Gregory Fletcher; *
|
|
* *
|
|
* 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 * 100, distances in meters*
|
|
* unless otherwise stated. *
|
|
************************************************************/
|
|
#ifndef AP_Mount_H
|
|
#define AP_Mount_H
|
|
|
|
#include <FastSerial.h>
|
|
#include <AP_Math.h>
|
|
#include <AP_Common.h>
|
|
#include <AP_GPS.h>
|
|
#include <AP_AHRS.h>
|
|
#include <GCS_MAVLink.h>
|
|
#include <../RC_Channel/RC_Channel_aux.h>
|
|
|
|
// #defines to control function of RC Channel used to manually provide angular offset to AP_Mount when we can't use RC_Channel_aux (which is the case for ArduCopter).
|
|
#define AP_MOUNT_MANUAL_RC_FUNCTION_DISABLED 0
|
|
#define AP_MOUNT_MANUAL_RC_FUNCTION_ROLL 1
|
|
#define AP_MOUNT_MANUAL_RC_FUNCTION_PITCH 2
|
|
#define AP_MOUNT_MANUAL_RC_FUNCTION_YAW 3
|
|
|
|
class AP_Mount
|
|
{
|
|
public:
|
|
//Constructor
|
|
AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs);
|
|
|
|
//enums
|
|
enum MountType{
|
|
k_pan_tilt = 0, ///< yaw-pitch
|
|
k_tilt_roll = 1, ///< pitch-roll
|
|
k_pan_tilt_roll = 2, ///< yaw-pitch-roll
|
|
};
|
|
|
|
// MAVLink methods
|
|
void configure_msg(mavlink_message_t* msg);
|
|
void control_msg(mavlink_message_t* msg);
|
|
void status_msg(mavlink_message_t* msg);
|
|
void set_roi_cmd(struct Location *target_loc);
|
|
void configure_cmd();
|
|
void control_cmd();
|
|
|
|
// should be called periodically
|
|
void update_mount_position();
|
|
void update_mount_type(); ///< Auto-detect the mount gimbal type depending on the functions assigned to the servos
|
|
void debug_output(); ///< For testing and development. Called in the medium loop.
|
|
// Accessors
|
|
enum MountType get_mount_type() { return _mount_type; }
|
|
|
|
// to allow manual input of an angle from the pilot when RC_Channel_aux cannot be used
|
|
void set_manual_rc_channel(RC_Channel *rc); // define which RC_Channel is to be used for manual control
|
|
void set_manual_rc_channel_function(int8_t fn); // set whether manual rc channel controlls roll (1), pitch (2) or yaw (3).
|
|
|
|
// hook for eeprom variables
|
|
static const struct AP_Param::GroupInfo var_info[];
|
|
|
|
private:
|
|
|
|
//methods
|
|
void set_mode(enum MAV_MOUNT_MODE mode);
|
|
|
|
void set_retract_angles(float roll, float pitch, float yaw); ///< set mount retracted position
|
|
void set_neutral_angles(float roll, float pitch, float yaw);
|
|
void set_control_angles(float roll, float pitch, float yaw);
|
|
void set_GPS_target_location(Location targetGPSLocation); ///< used to tell the mount to track GPS location
|
|
|
|
// internal methods
|
|
void calc_GPS_target_angle(struct Location *target);
|
|
void stabilize();
|
|
|
|
//members
|
|
AP_AHRS *_ahrs; ///< Rotation matrix from earth to plane.
|
|
GPS *&_gps;
|
|
const struct Location *_current_loc;
|
|
static const float t7 = 10000000.0;
|
|
float _roll_control_angle; ///< radians
|
|
float _pitch_control_angle; ///< radians
|
|
float _yaw_control_angle; ///< radians
|
|
|
|
float _roll_angle; ///< degrees
|
|
float _pitch_angle; ///< degrees
|
|
float _yaw_angle; ///< degrees
|
|
|
|
AP_Int8 _stab_roll; ///< (1 = yes, 0 = no)
|
|
AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no)
|
|
AP_Int8 _stab_yaw; ///< (1 = yes, 0 = no)
|
|
|
|
AP_Int8 _mount_mode;
|
|
MountType _mount_type;
|
|
|
|
struct Location _target_GPS_location;
|
|
|
|
AP_Vector3f _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
|
AP_Vector3f _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
|
AP_Vector3f _control_angles; ///< GCS controlled position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
|
|
|
|
// RC_Channel for providing direct angular input from pilot
|
|
RC_Channel* _manual_rc;
|
|
int8_t _manual_rc_function;
|
|
};
|
|
#endif
|