mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
01df18b292
It is fully configurable with the mission planner, there is no need to change the source code to adapt to your setup. It needs more testing, but the SIL is not working for me.
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;
|
|
}
|
|
|