ardupilot/libraries/AP_Mount/AP_Mount.h

108 lines
3.5 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>
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();
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();
private:
//methods
void set_mode(enum MAV_MOUNT_MODE mode);
void set_retract_angles(int roll, int pitch, int yaw); ///< set mount retracted position
void set_neutral_angles(int roll, int pitch, int yaw);
void set_mavlink_angles(int roll, int pitch, int 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 calculate();
long rc_map(RC_Channel_aux* rc_ch);
//members
Matrix3f m; ///< holds 3 x 3 matrix, var is used as temp in calcs
Matrix3f cam; ///< Rotation matrix earth to camera. Desired camera from input.
Matrix3f gimbal_target; ///< Rotation matrix from plane to camera. Then Euler angles to the servos.
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;
float _pitch_control_angle;
float _yaw_control_angle;
float _roll;
float _pitch;
float _yaw;
int16_t _roll_angle; ///< degrees*100
int16_t _pitch_angle; ///< degrees*100
int16_t _yaw_angle; ///< degrees*100
uint8_t _stab_roll; ///< (1 = yes, 0 = no)
uint8_t _stab_pitch; ///< (1 = yes, 0 = no)
uint8_t _stab_yaw; ///< (1 = yes, 0 = no)
enum MAV_MOUNT_MODE _mount_mode;
MountType _mount_type;
struct Location _target_GPS_location;
Vector3i _retract_angles; ///< retracted position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _neutral_angles; ///< neutral position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
Vector3i _mavlink_angles; ///< mavlink position for mount, vector.x = roll vector.y = pitch, vector.z=yaw
};
#endif