mirror of https://github.com/ArduPilot/ardupilot
297 lines
9.7 KiB
C++
297 lines
9.7 KiB
C++
// -*- 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_DCM *dcm)
|
|
{
|
|
_dcm=dcm;
|
|
_dcm_hil=NULL;
|
|
_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;
|
|
}
|
|
|
|
AP_Mount::AP_Mount(GPS *gps, AP_DCM_HIL *dcm)
|
|
{
|
|
_dcm=NULL;
|
|
_dcm_hil=dcm;
|
|
_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_MAVLINK_TARGETING); // 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 = _dcm?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
|
|
//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 (_dcm)
|
|
{
|
|
roll_angle = -_dcm->roll_sensor;
|
|
pitch_angle = -_dcm->pitch_sensor;
|
|
yaw_angle = -_dcm->yaw_sensor;
|
|
}
|
|
if (_dcm_hil)
|
|
{
|
|
roll_angle = -_dcm_hil->roll_sensor;
|
|
pitch_angle = -_dcm_hil->pitch_sensor;
|
|
yaw_angle = -_dcm_hil->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 = (_dcm)?_dcm->get_dcm_transposed():_dcm_hil->get_dcm_transposed();
|
|
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;
|
|
}
|
|
|
|
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;
|
|
}
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
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;
|
|
}
|
|
|