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

Add "3 axis camera stabilization" and "point camera to 3D point" functionality 

Patch by Gregory Fletcher and reviewed by me
This commit is contained in:
Amilcar Lucas 2012-06-13 20:55:19 +02:00
parent 881a66ae01
commit a32b7c200b
6 changed files with 502 additions and 371 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/ArduPlane.pde
View File

@ -258,11 +258,6 @@ AP_AnalogSource_Arduino pitot_analog_source(CONFIG_PITOT_SOURCE_ANALOG_PIN, 4.0)
AP_Relay relay;
// Camera/Antenna mount tracking and stabilisation stuff
// --------------------------------------
#if MOUNT == ENABLED
AP_Mount camera_mount(g_gps, &ahrs);
#endif
////////////////////////////////////////////////////////////////////////////////
@ -637,6 +632,14 @@ static unsigned long dTnav;
static float load;
// Camera/Antenna mount tracking and stabilisation stuff
// --------------------------------------
#if MOUNT == ENABLED
// current_loc uses the baro/gps soloution for altitude rather than gps only.
// mabe one could use current_loc for lat/lon too and eliminate g_gps alltogether?
AP_Mount camera_mount(&current_loc, g_gps, &ahrs);
#endif
////////////////////////////////////////////////////////////////////////////////
// Top-level logic

10
ArduPlane/Parameters.pde
View File

@ -277,13 +277,23 @@ static const AP_Param::Info var_info[] PROGMEM = {
// @User: Advanced
GSCALAR(airspeed_use, "ARSPD_USE"),
// RC channel
//-----------
GGROUP(channel_roll, "RC1_", RC_Channel),
GGROUP(channel_pitch, "RC2_", RC_Channel),
GGROUP(channel_throttle, "RC3_", RC_Channel),
GGROUP(channel_rudder, "RC4_", RC_Channel),
// @Group: RC5_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_5, "RC5_", RC_Channel_aux),
// @Group: RC6_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_6, "RC6_", RC_Channel_aux),
// @Group: RC7_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_7, "RC7_", RC_Channel_aux),
// @Group: RC8_
// @Path: ../libraries/RC_Channel/RC_Channel_aux.cpp
GGROUP(rc_8, "RC8_", RC_Channel_aux),
GGROUP(pidNavRoll, "HDNG2RLL_", PID),

594
libraries/AP_Mount/AP_Mount.cpp
View File

@ -1,277 +1,333 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include <AP_Mount.h>
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(GPS *gps, AP_AHRS *ahrs)
{
_ahrs=ahrs;
_gps=gps;
//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
_retract_angles.x=0;
_retract_angles.y=0;
_retract_angles.z=0;
}
//sets the servo angles for retraction, note angles are * 100
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
{
_retract_angles.x=roll;
_retract_angles.y=pitch;
_retract_angles.z=yaw;
}
//sets the servo angles for neutral, note angles are * 100
void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
{
_neutral_angles.x=roll;
_neutral_angles.y=pitch;
_neutral_angles.z=yaw;
}
//sets the servo angles for MAVLink, note angles are * 100
void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
{
_mavlink_angles.x = roll;
_mavlink_angles.y = pitch;
_mavlink_angles.z = yaw;
}
// used to tell the mount to track GPS location
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
{
_target_GPS_location=targetGPSLocation;
}
// This one should be called periodically
void AP_Mount::update_mount_position()
{
Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs
Vector3f targ; //holds target vector, var is used as temp in calcs
Vector3f aux_vec; //holds target vector, var is used as temp in calcs
switch(_mount_mode)
{
case MAV_MOUNT_MODE_RETRACT:
roll_angle =100*_retract_angles.x;
pitch_angle=100*_retract_angles.y;
yaw_angle =100*_retract_angles.z;
break;
case MAV_MOUNT_MODE_NEUTRAL:
roll_angle =100*_neutral_angles.x;
pitch_angle=100*_neutral_angles.y;
yaw_angle =100*_neutral_angles.z;
break;
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
aux_vec.x = _mavlink_angles.x;
aux_vec.y = _mavlink_angles.y;
aux_vec.z = _mavlink_angles.z;
m = _ahrs->get_dcm_matrix();
m.transpose();
//rotate vector
targ = m*aux_vec;
// TODO The next three lines are probably not correct yet
roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_mavlink_angles.y; //roll
pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:_neutral_angles.x; //pitch
yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:_neutral_angles.z; //yaw
break;
}
case MAV_MOUNT_MODE_RC_TARGETING: // radio manual control
{
// TODO It does work, but maybe is a good idea to replace this simplified implementation with a proper one
if (_ahrs)
{
roll_angle = -_ahrs->roll_sensor;
pitch_angle = -_ahrs->pitch_sensor;
yaw_angle = -_ahrs->yaw_sensor;
}
if (g_rc_function[RC_Channel_aux::k_mount_roll])
roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]);
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
yaw_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]);
break;
}
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_gps->fix)
{
calc_GPS_target_vector(&_target_GPS_location);
}
m = _ahrs->get_dcm_matrix();
m.transpose();
targ = m*_GPS_vector;
/* disable stabilization for now, this will help debug */
_stab_roll = 0;_stab_pitch=0;_stab_yaw=0;
/**/
// TODO The next three lines are probably not correct yet
roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_GPS_vector.y; //roll
pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:0; //pitch
yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:degrees(atan2(-_GPS_vector.x,_GPS_vector.y))*100; //yaw
break;
}
default:
//do nothing
break;
}
// write the results to the servos
// 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);
}
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
{
_mount_mode=mode;
}
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
mavlink_msg_mount_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
_stab_pitch = packet.stab_pitch;
_stab_roll = packet.stab_roll;
_stab_yaw = packet.stab_yaw;
}
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
#include <AP_Mount.h>
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):
_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
_retract_angles.x=0;
_retract_angles.y=0;
_retract_angles.z=0;
}
//sets the servo angles for retraction, note angles are * 100
void AP_Mount::set_retract_angles(int roll, int pitch, int yaw)
{
_retract_angles.x=roll;
_retract_angles.y=pitch;
_retract_angles.z=yaw;
}
//sets the servo angles for neutral, note angles are * 100
void AP_Mount::set_neutral_angles(int roll, int pitch, int yaw)
{
_neutral_angles.x=roll;
_neutral_angles.y=pitch;
_neutral_angles.z=yaw;
}
//sets the servo angles for MAVLink, note angles are * 100
void AP_Mount::set_mavlink_angles(int roll, int pitch, int yaw)
{
_mavlink_angles.x = roll;
_mavlink_angles.y = pitch;
_mavlink_angles.z = yaw;
}
// used to tell the mount to track GPS location
void AP_Mount::set_GPS_target_location(Location targetGPSLocation)
{
_target_GPS_location=targetGPSLocation;
}
// This one should be called periodically
void AP_Mount::update_mount_position()
{
Matrix3f m; //holds 3 x 3 matrix, var is used as temp in calcs
Vector3f targ; //holds target vector, var is used as temp in calcs
Vector3f aux_vec; //holds target vector, var is used as temp in calcs
switch(_mount_mode)
{
// 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;
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;
break;
// point to the angles given by a mavlink message
case MAV_MOUNT_MODE_MAVLINK_TARGETING:
{
aux_vec.x = _mavlink_angles.x;
aux_vec.y = _mavlink_angles.y;
aux_vec.z = _mavlink_angles.z;
m = _ahrs->get_dcm_matrix();
m.transpose();
//rotate vector
targ = m*aux_vec;
// TODO The next three lines are probably not correct yet
_roll_angle = _stab_roll? degrees(atan2( targ.y,targ.z))*100:_mavlink_angles.y; //roll
_pitch_angle = _stab_pitch?degrees(atan2(-targ.x,targ.z))*100:_neutral_angles.x; //pitch
_yaw_angle = _stab_yaw? degrees(atan2(-targ.x,targ.y))*100:_neutral_angles.z; //yaw
break;
}
// 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);
if (_ahrs){
calculate();
} else {
if (g_rc_function[RC_Channel_aux::k_mount_roll])
_roll_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_roll]);
if (g_rc_function[RC_Channel_aux::k_mount_pitch])
_pitch_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_pitch]);
if (g_rc_function[RC_Channel_aux::k_mount_yaw])
_yaw_angle = rc_map(g_rc_function[RC_Channel_aux::k_mount_yaw]);
}
break;
}
// point mount to a GPS point given by the mission planner
case MAV_MOUNT_MODE_GPS_POINT:
{
if(_gps->fix){
calc_GPS_target_angle(&_target_GPS_location);
}
if (_ahrs){
calculate();
}
break;
}
default:
//do nothing
break;
}
// write the results to the servos
/*
G_RC_AUX(k_mount_roll)->angle_out(_roll_angle);
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);
}
void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
{
_mount_mode=mode;
}
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
mavlink_msg_mount_configure_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
set_mode((enum MAV_MOUNT_MODE)packet.mount_mode);
_stab_pitch = packet.stab_pitch;
_stab_roll = packet.stab_roll;
_stab_yaw = packet.stab_yaw;
}
void AP_Mount::control_msg(mavlink_message_t *msg)
{
__mavlink_mount_control_t packet;
mavlink_msg_mount_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch (_mount_mode)
{
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);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
}
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);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
}
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);
break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
break;
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
Location targetGPSLocation;
targetGPSLocation.lat = packet.input_a;
targetGPSLocation.lng = packet.input_b;
targetGPSLocation.alt = packet.input_c;
set_GPS_target_location(targetGPSLocation);
break;
}
__mavlink_mount_control_t packet;
mavlink_msg_mount_control_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch (_mount_mode)
{
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);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
}
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);
if (packet.save_position)
{
// TODO: Save current trimmed position on EEPROM
}
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);
break;
case MAV_MOUNT_MODE_RC_TARGETING: // Load neutral position and start RC Roll,Pitch,Yaw control with stabilization
break;
case MAV_MOUNT_MODE_GPS_POINT: // Load neutral position and start to point to Lat,Lon,Alt
Location targetGPSLocation;
targetGPSLocation.lat = packet.input_a;
targetGPSLocation.lng = packet.input_b;
targetGPSLocation.alt = packet.input_c;
set_GPS_target_location(targetGPSLocation);
break;
}
}
void AP_Mount::status_msg(mavlink_message_t *msg)
{
__mavlink_mount_status_t packet;
mavlink_msg_mount_status_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch (_mount_mode)
{
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
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;
}
// status reply
// TODO: is COMM_3 correct ?
mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
packet.pointing_a, packet.pointing_b, packet.pointing_c);
__mavlink_mount_status_t packet;
mavlink_msg_mount_status_decode(msg, &packet);
if (mavlink_check_target(packet.target_system, packet.target_component)) {
// not for us
return;
}
switch (_mount_mode)
{
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
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;
}
// status reply
// TODO: is COMM_3 correct ?
mavlink_msg_mount_status_send(MAVLINK_COMM_3, packet.target_system, packet.target_component,
packet.pointing_a, packet.pointing_b, packet.pointing_c);
}
void AP_Mount::set_roi_cmd()
{
// TODO get the information out of the mission command and use it
}
void AP_Mount::configure_cmd()
{
// TODO get the information out of the mission command and use it
}
void AP_Mount::control_cmd()
{
// TODO get the information out of the mission command and use it
}
void AP_Mount::calc_GPS_target_vector(struct Location *target)
{
_GPS_vector.x = (target->lng-_gps->longitude) * cos((_gps->latitude+target->lat)/2)*.01113195;
_GPS_vector.y = (target->lat-_gps->latitude)*.01113195;
_GPS_vector.z = (_gps->altitude-target->alt);
}
void
AP_Mount::update_mount_type()
{
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
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;
}
}
// This function is needed to let the HIL code compile
long
AP_Mount::rc_map(RC_Channel_aux* rc_ch)
{
return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min;
}
void AP_Mount::set_roi_cmd()
{
// TODO get the information out of the mission command and use it
}
void AP_Mount::configure_cmd()
{
// TODO get the information out of the mission command and use it
}
void AP_Mount::control_cmd()
{
// TODO get the information out of the mission command and use it
}
void
AP_Mount::calc_GPS_target_angle(struct Location *target)
{
float _GPS_vector_x = (target->lng-_current_loc->lng)*cos(ToRad((_current_loc->lat+target->lat)/(t7*2.0)))*.01113195;
float _GPS_vector_y = (target->lat-_current_loc->lat)*.01113195;
float _GPS_vector_z = (target->alt-_current_loc->alt); // baro altitude(IN CM) should be adjusted to known home elevation before take off (Set altimeter).
float target_distance = 100.0*sqrt(_GPS_vector_x*_GPS_vector_x + _GPS_vector_y*_GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
_pitch_control_angle = atan2(_GPS_vector_z, target_distance);
_yaw_control_angle = atan2(_GPS_vector_x,_GPS_vector_y);
// Converts +/- 180 into 0-360.
if(_yaw_control_angle<0){
_yaw_control_angle += 2*M_PI;
}
}
void
AP_Mount::update_mount_type()
{
// Auto-detect the mount gimbal type depending on the functions assigned to the servos
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;
}
}
void
AP_Mount::calculate()
{
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;
}
// This function is needed to let the HIL code compile
long
AP_Mount::rc_map(RC_Channel_aux* rc_ch)
{
return (rc_ch->radio_in - rc_ch->radio_min) * (rc_ch->angle_max - rc_ch->angle_min) / (rc_ch->radio_max - rc_ch->radio_min) + rc_ch->angle_min;
}
// For testing and development. Called in the medium loop.
void
AP_Mount::debug_output()
{ Serial3.print("current - ");
Serial3.print("lat ");
Serial3.print(_current_loc->lat);
Serial3.print(",lon ");
Serial3.print(_current_loc->lng);
Serial3.print(",alt ");
Serial3.println(_current_loc->alt);
Serial3.print("gps - ");
Serial3.print("lat ");
Serial3.print(_gps->latitude);
Serial3.print(",lon ");
Serial3.print(_gps->longitude);
Serial3.print(",alt ");
Serial3.print(_gps->altitude);
Serial3.println();
Serial3.print("target - ");
Serial3.print("lat ");
Serial3.print(_target_GPS_location.lat);
Serial3.print(",lon ");
Serial3.print(_target_GPS_location.lng);
Serial3.print(",alt ");
Serial3.print(_target_GPS_location.alt);
Serial3.print(" hdg to targ ");
Serial3.print(degrees(_yaw_control_angle));
Serial3.println();
}

200
libraries/AP_Mount/AP_Mount.h
View File

@ -1,95 +1,107 @@
// -*- 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; *
* *
* 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>
// -*- 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:
//Constructors
AP_Mount(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
// 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_vector(struct Location *target);
long rc_map(RC_Channel_aux* rc_ch);
//members
AP_AHRS *_ahrs;
GPS *_gps;
int roll_angle; ///< degrees*100
int pitch_angle; ///< degrees*100
int 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
Vector3f _GPS_vector; ///< target vector calculated stored in meters
};
#endif
#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

47
libraries/RC_Channel/RC_Channel_aux.cpp
View File

@ -5,13 +5,33 @@
const AP_Param::GroupInfo RC_Channel_aux::var_info[] PROGMEM = {
AP_NESTEDGROUPINFO(RC_Channel, 0),
// @Param: FUNCTION
// @DisplayName: Function assigned to this APM servo output
// @Description: Setting this to Disabled(0) will disable this output, any other value will enable the corresponding function
// @Values: 0:Disabled,1:Manual,2:Flap,3:Flap_auto,4:Aileron,5:flaperon,6:mount_pan,7:mount_tilt,8:mount_roll,9:mount_open,10:camera_trigger,11:release
// @User: Standard
AP_GROUPINFO("FUNCTION", 1, RC_Channel_aux, function),
// @Param: ANGLE_MIN
// @DisplayName: Minimum physical angular position of the object that this servo output controls
// @Description: Minimum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc
// @Units: Degrees
// @Range: -180 180
// @Increment: .01
// @User: Standard
AP_GROUPINFO("ANGLE_MIN", 2, RC_Channel_aux, angle_min),
// @Param: ANGLE_MAX
// @DisplayName: Maximum physical angular position of the object that this servo output controls
// @Description: Maximum physical angular position of the object that this servo output controls, this could be for example a camera pan angle, an aileron angle, etc
// @Units: Degrees
// @Range: -180 180
// @Increment: .01
// @User: Standard
AP_GROUPINFO("ANGLE_MAX", 3, RC_Channel_aux, angle_max),
AP_GROUPEND
};
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions]; // the aux. servo ch. assigned to each function
// Global pointer array, indexed by a "RC function enum" and points to the RC channel output assigned to that function/operation
RC_Channel_aux* g_rc_function[RC_Channel_aux::k_nr_aux_servo_functions];
int16_t
RC_Channel_aux::closest_limit(int16_t angle)
@ -49,6 +69,31 @@ RC_Channel_aux::closest_limit(int16_t angle)
return angle;
}
// Gets the RC and integrates and then compares with the servo out angles to limit control input to servo travel.
// That way the user doesn't get lost. Rotationally.
void
RC_Channel_aux::rc_input(float *control_angle, int16_t angle)
{
if((radio_in < 1480 && angle < angle_max)||(radio_in > 1520 && angle > angle_min)){
*control_angle += ( 1500 - radio_in ) * .0001; // .0001 is the control speed scaler.
}
}
// Takes the desired servo angle(deg) and converts to microSeconds for PWM
// Like this: 45 deg = 2000 us ; -45 deg/1000 us. 1000us/(90*100 deg) = 0.1111111111111
void
RC_Channel_aux::angle_out(int16_t angle)
{
if(angle >= angle_max){
angle = angle_max;
}
if(angle <= angle_min){
angle = angle_min;
}
// Convert the angle*100 to pwm microseconds. 45 deg = 500 us.
radio_out = (/*_reverse * */ angle * 0.1111111) + 1500;
}
// map a function to a servo channel and output it
void
RC_Channel_aux::output_ch(unsigned char ch_nr)

9
libraries/RC_Channel/RC_Channel_aux.h
View File

@ -1,8 +1,9 @@
// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
/// @file RC_Channel_aux.h
/// @brief RC_Channel manager for Channels 5..8, with EEPROM-backed storage of constants.
/// @brief RC_Channel manager for auxiliary channels (5..8), with EEPROM-backed storage of constants.
/// @author Amilcar Lucas
/// @author Gregory Fletcher
#ifndef RC_CHANNEL_AUX_H_
#define RC_CHANNEL_AUX_H_
@ -50,9 +51,13 @@ public:
int16_t closest_limit(int16_t angle); // saturate to the closest angle limit if outside of min max angle interval
void angle_out(int16_t angle);
void rc_input(float *control_angle, int16_t angle);
void output_ch(unsigned char ch_nr); // map a function to a servo channel and output it
static const struct AP_Param::GroupInfo var_info[];
static const struct AP_Param::GroupInfo var_info[];
};
void update_aux_servo_function(RC_Channel_aux* rc_5, RC_Channel_aux* rc_6, RC_Channel_aux* rc_7, RC_Channel_aux* rc_8);