diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index 09fae3ca07..313cb692b8 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -52,10 +52,6 @@ void AP_Mount::set_GPS_target_location(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
@@ -75,17 +71,10 @@ void AP_Mount::update_mount_position()
// 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
+ _roll_control_angle = _mavlink_angles.x;
+ _pitch_control_angle = _mavlink_angles.y;
+ _yaw_control_angle = _mavlink_angles.z;
+ calculate();
break;
}
@@ -113,8 +102,6 @@ void AP_Mount::update_mount_position()
{
if(_gps->fix){
calc_GPS_target_angle(&_target_GPS_location);
- }
- if (_ahrs){
calculate();
}
break;
@@ -141,6 +128,8 @@ void AP_Mount::set_mode(enum MAV_MOUNT_MODE mode)
_mount_mode=mode;
}
+// Change the configuration of the mount
+// triggered by a MavLink packet.
void AP_Mount::configure_msg(mavlink_message_t* msg)
{
__mavlink_mount_configure_t packet;
@@ -155,6 +144,8 @@ void AP_Mount::configure_msg(mavlink_message_t* msg)
_stab_yaw = packet.stab_yaw;
}
+// Control the mount (depends on the previously set mount configuration)
+// triggered by a MavLink packet.
void AP_Mount::control_msg(mavlink_message_t *msg)
{
__mavlink_mount_control_t packet;
@@ -199,6 +190,8 @@ void AP_Mount::control_msg(mavlink_message_t *msg)
}
}
+// Return mount status information (depends on the previously set mount configuration)
+// triggered by a MavLink packet.
void AP_Mount::status_msg(mavlink_message_t *msg)
{
__mavlink_mount_status_t packet;
@@ -231,16 +224,19 @@ void AP_Mount::status_msg(mavlink_message_t *msg)
packet.pointing_a, packet.pointing_b, packet.pointing_c);
}
+// Set mount point/region of interest, triggered by mission script commands
void AP_Mount::set_roi_cmd()
{
// TODO get the information out of the mission command and use it
}
+// Set mount configuration, triggered by mission script commands
void AP_Mount::configure_cmd()
{
// TODO get the information out of the mission command and use it
}
+// Control the mount (depends on the previously set mount configuration), triggered by mission script commands
void AP_Mount::control_cmd()
{
// TODO get the information out of the mission command and use it
@@ -249,23 +245,23 @@ void AP_Mount::control_cmd()
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.
+ 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);
+ _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;
}
}
+// Auto-detect the mount gimbal type depending on the functions assigned to the servos
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;
@@ -280,17 +276,27 @@ AP_Mount::update_mount_type()
}
}
+// 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()
{
- 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;
+ 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;
+ }
}
// This function is needed to let the HIL code compile
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index a5d45cb1c4..b8e03b79b3 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -73,9 +73,6 @@ private:
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;
@@ -83,9 +80,6 @@ private:
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