Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-23 09:08:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Move local temp variables to the stack it saves 1952 bytes

Browse Source 
Add function comments
Only use _ on class member variables
Only point to a 3D GPS point if GPS has a fix
Implement MAV_MOUNT_MODE_MAVLINK_TARGETING
This commit is contained in:
Amilcar Lucas 2012-06-17 22:25:51 +02:00
parent 7efb4ecac4
commit f4e9587aca
2 changed files with 38 additions and 38 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

62
libraries/AP_Mount/AP_Mount.cpp
View File

@ -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()
{
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;
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

6
libraries/AP_Mount/AP_Mount.h
View File

@ -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