Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Mount: enable mount control via eeprom parameters 

this enables MNT_* parameter control of the camera mount code. It also
fixes the conversion of calculated angles between degrees and
integers, and fixes stabilised mount control when yaw control is not
available.
This commit is contained in:
Andrew Tridgell 2012-07-03 10:21:01 +10:00
parent 82e09ce53a
commit 99b11e4f19
5 changed files with 121 additions and 104 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
ArduPlane/ArduPlane.pde
View File

@ -918,10 +918,6 @@ static void slow_loop()
update_servo_switches();
update_aux_servo_function(&g.rc_5, &g.rc_6, &g.rc_7, &g.rc_8);
#if MOUNT == ENABLED
camera_mount.update_mount_type();
#endif
break;
case 2:

5
ArduPlane/Parameters.h
View File

@ -124,9 +124,10 @@ public:
//
// Camera parameters
// Camera and mount parameters
//
k_param_camera,
k_param_camera = 159,
k_param_camera_mount,
//
// 170: Radio settings

7
ArduPlane/Parameters.pde
View File

@ -328,6 +328,13 @@ static const AP_Param::Info var_info[] PROGMEM = {
// @Path: ../libraries/AP_AHRS/AP_AHRS_DCM.cpp, ../libraries/AP_AHRS/AP_AHRS_Quaternion.cpp
GOBJECT(ahrs, "AHRS_", AP_AHRS),
#if MOUNT == ENABLED
// @Group: MNT_
// @Path: ../libraries/AP_Mount/AP_Mount.cpp
GOBJECT(camera_mount, "MNT_", AP_Mount),
#endif
#ifdef DESKTOP_BUILD
// @Group: SIM_
// @Path: ../libraries/SITL/SITL.cpp

177
libraries/AP_Mount/AP_Mount.cpp
View File

@ -1,7 +1,22 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include <FastSerial.h>
#include <AP_Common.h>
#include <AP_Param.h>
#include <AP_Mount.h>
const AP_Param::GroupInfo AP_Mount::var_info[] PROGMEM = {
// index 0 was used for the old orientation matrix
AP_GROUPINFO("MODE", 0, AP_Mount, _mount_mode),
AP_GROUPINFO("RETRACT", 1, AP_Mount, _retract_angles),
AP_GROUPINFO("NEUTRAL", 2, AP_Mount, _neutral_angles),
AP_GROUPINFO("CONTROL", 3, AP_Mount, _control_angles),
AP_GROUPINFO("STAB_ROLL", 4, AP_Mount, _stab_roll),
AP_GROUPINFO("STAB_PITCH", 5, AP_Mount, _stab_pitch),
AP_GROUPINFO("STAB_YAW", 6, AP_Mount, _stab_yaw),
AP_GROUPEND
};
extern RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
AP_Mount::AP_Mount(const struct Location *current_loc, GPS *&gps, AP_AHRS *ahrs):
@ -10,37 +25,31 @@ _gps(gps)
_ahrs = ahrs;
_current_loc = current_loc;
//set_mode(MAV_MOUNT_MODE_RETRACT);
set_mode(MAV_MOUNT_MODE_RC_TARGETING); // FIXME: This is just to test without mavlink
//set_mode(MAV_MOUNT_MODE_GPS_POINT); // FIXME: this is to test ONLY targeting
// startup with the mount retracted
set_mode(MAV_MOUNT_MODE_RETRACT);
_retract_angles.x=0;
_retract_angles.y=0;
_retract_angles.z=0;
// default to zero angles
_retract_angles = Vector3f(0,0,0);
_neutral_angles = Vector3f(0,0,0);
_control_angles = Vector3f(0,0,0);
}
//sets the servo angles for retraction, note angles are * 100
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
//sets the servo angles for retraction, note angles are in degrees
void AP_Mount::set_retract_angles(float roll, float pitch, float yaw)
{
_retract_angles.x=roll;
_retract_angles.y=pitch;
_retract_angles.z=yaw;
_retract_angles = Vector3f(roll, pitch, yaw);
}
//sets the servo angles for neutral, note angles are * 100
void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
//sets the servo angles for neutral, note angles are in degrees
void AP_Mount::set_neutral_angles(float roll, float pitch, float yaw)
{
_neutral_angles.x=roll;
_neutral_angles.y=pitch;
_neutral_angles.z=yaw;
_neutral_angles = Vector3f(roll, pitch, yaw);
}
//sets the servo angles for MAVLink, note angles are * 100
void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
//sets the servo angles for MAVLink, note angles are in degrees
void AP_Mount::set_control_angles(float roll, float pitch, float yaw)
{
_mavlink_angles.x = roll;
_mavlink_angles.y = pitch;
_mavlink_angles.z = yaw;
_control_angles = Vector3f(roll, pitch, yaw);
}
// used to tell the mount to track GPS location
@ -52,28 +61,35 @@ void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
// This one should be called periodically
void AP_Mount::update_mount_position()
{
switch(_mount_mode)
switch((enum MAV_MOUNT_MODE)_mount_mode.get())
{
// move mount to a "retracted position" or to a position where a fourth servo can retract the entire mount into the fuselage
case MAV_MOUNT_MODE_RETRACT:
_roll_angle =100*_retract_angles.x;
_pitch_angle=100*_retract_angles.y;
_yaw_angle =100*_retract_angles.z;
{
Vector3f vec = _retract_angles.get();
_roll_angle = vec.x;
_pitch_angle = vec.y;
_yaw_angle = vec.z;
break;
}
// move mount to a neutral position, typically pointing forward
case MAV_MOUNT_MODE_NEUTRAL:
_roll_angle =100*_neutral_angles.x;
_pitch_angle=100*_neutral_angles.y;
_yaw_angle =100*_neutral_angles.z;
{
Vector3f vec = _neutral_angles.get();
_roll_angle = vec.x;
_pitch_angle = vec.y;
_yaw_angle = vec.z;
break;
}
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
_roll_control_angle = _mavlink_angles.x;
_pitch_control_angle = _mavlink_angles.y;
_yaw_control_angle = _mavlink_angles.z;
Vector3f vec = _control_angles.get();
_roll_control_angle = vec.x;
_pitch_control_angle = vec.y;
_yaw_control_angle = vec.z;
calculate();
break;
}
@ -81,9 +97,10 @@ void AP_Mount::update_mount_position()
// RC radio manual angle control, but with stabilization from the AHRS
case MAV_MOUNT_MODE_RC_TARGETING:
{
G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle);
G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle);
G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle);
// rc_input() takes degrees * 100 units
G_RC_AUX(k_mount_roll)->rc_input(&_roll_control_angle, _roll_angle*100);
G_RC_AUX(k_mount_pitch)->rc_input(&_pitch_control_angle, _pitch_angle*100);
G_RC_AUX(k_mount_yaw)->rc_input(&_yaw_control_angle, _yaw_angle*100);
if (_ahrs){
calculate();
} else {
@ -117,15 +134,15 @@ void AP_Mount::update_mount_position()
G_RC_AUX(k_mount_pitch)->angle_out(_pitch_angle);
G_RC_AUX(k_mount_yaw)->angle_out(_yaw_angle);
*/
// Change scaling to 0.1 degrees in order to avoid overflows in the angle arithmetic
G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle/10);
G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle/10);
G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle/10);
// closest_limit() takes degrees * 10 units
G_RC_AUX(k_mount_roll)->closest_limit(_roll_angle*10);
G_RC_AUX(k_mount_pitch)->closest_limit(_pitch_angle*10);
G_RC_AUX(k_mount_yaw)->closest_limit(_yaw_angle*10);
}
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
{
_mount_mode=mode;
_mount_mode = (int8_t)mode;
}
// Change the configuration of the mount
@ -155,26 +172,26 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
return;
}
switch (_mount_mode)
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
{
case MAV_MOUNT_MODE_RETRACT: // Load and keep safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
set_retract_angles(packet.input_b, packet.input_a, packet.input_c);
set_retract_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
_retract_angles.save();
}
break;
case MAV_MOUNT_MODE_NEUTRAL: // Load and keep neutral position (Roll,Pitch,Yaw) from EEPROM
set_neutral_angles(packet.input_b, packet.input_a, packet.input_c);
set_neutral_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
_neutral_angles.save();
}
break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // Load neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
set_mavlink_angles(packet.input_b, packet.input_a, packet.input_c);
set_control_angles(packet.input_b*0.01, packet.input_a*0.01, packet.input_c*0.01);
break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
@ -187,6 +204,9 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
targetGPSLocation.alt = packet.input_c;
set_GPS_target_location(targetGPSLocation);
break;
case MAV_MOUNT_MODE_ENUM_END:
break;
}
}
@ -201,21 +221,23 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
return;
}
switch (_mount_mode)
switch ((enum MAV_MOUNT_MODE)_mount_mode.get())
{
case MAV_MOUNT_MODE_RETRACT: // safe position (Roll,Pitch,Yaw) from EEPROM and stop stabilization
case MAV_MOUNT_MODE_NEUTRAL: // neutral position (Roll,Pitch,Yaw) from EEPROM
case MAV_MOUNT_MODE_MAVLINK_TARGETING: // neutral position and start MAVLink Roll,Pitch,Yaw control with stabilization
case MAV_MOUNT_MODE_RC_TARGETING: // neutral position and start RC Roll,Pitch,Yaw control with stabilization
packet.pointing_b = _roll_angle; ///< degrees*100
packet.pointing_a = _pitch_angle; ///< degrees*100
packet.pointing_c = _yaw_angle; ///< degrees*100
packet.pointing_b = _roll_angle*100; ///< degrees*100
packet.pointing_a = _pitch_angle*100; ///< degrees*100
packet.pointing_c = _yaw_angle*100; ///< degrees*100
break;
case MAV_MOUNT_MODE_GPS_POINT: // neutral position and start to point to Lat,Lon,Alt
packet.pointing_a = _target_GPS_location.lat; ///< latitude
packet.pointing_b = _target_GPS_location.lng; ///< longitude
packet.pointing_c = _target_GPS_location.alt; ///< altitude
break;
case MAV_MOUNT_MODE_ENUM_END:
break;
}
// status reply
@ -258,44 +280,35 @@ AP_Mount::calc_GPS_target_angle(struct Location *target)
}
}
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
void
AP_Mount::update_mount_type()
{
if ((g_rc_function[RC_Channel_aux::k_mount_roll] == NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
{
_mount_type = k_pan_tilt;
}
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] == NULL))
{
_mount_type = k_tilt_roll;
}
if ((g_rc_function[RC_Channel_aux::k_mount_roll] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_pitch] != NULL) && (g_rc_function[RC_Channel_aux::k_mount_yaw] != NULL))
{
_mount_type = k_pan_tilt_roll;
}
}
// Inputs desired _roll_control_angle, _pitch_control_angle and _yaw_control_angle stabilizes them relative to the airframe
// and calculates output _roll_angle, _pitch_angle and _yaw_angle
void
AP_Mount::calculate()
{
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.
float roll;
float pitch;
float yaw;
if (_ahrs){
m = _ahrs->get_dcm_matrix();
m.transpose();
cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
gimbal_target = m * cam;
gimbal_target.to_euler(&roll, &pitch, &yaw);
_roll_angle = degrees(roll)*100;
_pitch_angle = degrees(pitch)*100;
_yaw_angle = degrees(yaw)*100;
if (_ahrs) {
// only do the full 3D frame transform if we are doing yaw control
if (_stab_yaw) {
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.
m = _ahrs->get_dcm_matrix();
m.transpose();
cam.from_euler(_roll_control_angle, _pitch_control_angle, _yaw_control_angle);
gimbal_target = m * cam;
gimbal_target.to_euler(&_roll_angle, &_pitch_angle, &_yaw_angle);
} else {
// otherwise base mount roll and pitch on the ahrs
// roll/pitch attitude, plus any requested angle
_roll_angle = _roll_control_angle;
_pitch_angle = _pitch_control_angle;
_yaw_angle = _yaw_control_angle;
if (_stab_roll) {
_roll_angle -= degrees(_ahrs->roll);
}
if (_stab_pitch) {
_pitch_angle -= degrees(_ahrs->pitch);
}
}
}
}

32
libraries/AP_Mount/AP_Mount.h
View File

@ -52,19 +52,20 @@ public:
// 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();
// 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(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_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
@ -81,21 +82,20 @@ private:
float _pitch_control_angle;
float _yaw_control_angle;
int16_t _roll_angle; ///< degrees*100
int16_t _pitch_angle; ///< degrees*100
int16_t _yaw_angle; ///< degrees*100
float _roll_angle; ///< degrees
float _pitch_angle; ///< degrees
float _yaw_angle; ///< degrees
uint8_t _stab_roll; ///< (1 = yes, 0 = no)
uint8_t _stab_pitch; ///< (1 = yes, 0 = no)
uint8_t _stab_yaw; ///< (1 = yes, 0 = no)
AP_Int8 _stab_roll; ///< (1 = yes, 0 = no)
AP_Int8 _stab_pitch; ///< (1 = yes, 0 = no)
AP_Int8 _stab_yaw; ///< (1 = yes, 0 = no)
enum MAV_MOUNT_MODE _mount_mode;
MountType _mount_type;
AP_Int8 _mount_mode;
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
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
};
#endif