aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Mount: Axis mask speedup

Browse Source 
Using a simple bit mask to avoid calculating an unneeded ATAN2() for AVR users.
This commit is contained in:
Jason Short 2014-06-16 10:50:19 -07:00 committed by Andrew Tridgell
parent 1d3a49e466
commit 7ee5b58535
2 changed files with 37 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
libraries/AP_Mount/AP_Mount.cpp
View File

@ -9,6 +9,13 @@
#define ENABLED 1
#define DISABLED 0
#define MASK_TILT (1<<0)
#define MASK_ROLL (1<<1)
#define MASK_YAW (1<<2)
#define MASK_RETRACT (1<<3)
#if defined( __AVR_ATmega1280__ )
# define MNT_JSTICK_SPD_OPTION DISABLED // Allow RC joystick to control the speed of the mount movements instead of the position of the mount
# define MNT_RETRACT_OPTION DISABLED // Use a servo to retract the mount inside the fuselage (i.e. for landings)
@ -276,21 +283,31 @@ void
AP_Mount::update_mount_type()
{
bool have_roll, have_tilt, have_pan;
have_roll = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_roll) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_roll);
have_tilt = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_tilt) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_tilt);
have_pan = RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount_pan) ||
RC_Channel_aux::function_assigned(RC_Channel_aux::k_mount2_pan);
if (have_pan && have_tilt && !have_roll) {
mount_axis_mask = 0;
if (have_pan)
mount_axis_mask |= MASK_YAW;
if (have_tilt)
mount_axis_mask |= MASK_TILT;
if (have_roll)
mount_axis_mask |= MASK_ROLL;
if (have_pan && have_tilt && !have_roll)
_mount_type = k_pan_tilt;
}
if (!have_pan && have_tilt && have_roll) {
if (!have_pan && have_tilt && have_roll)
_mount_type = k_tilt_roll;
}
if (have_pan && have_tilt && have_roll) {
if (have_pan && have_tilt && have_roll)
_mount_type = k_pan_tilt_roll;
}
}
/// sets the servo angles for retraction, note angles are in degrees
@ -598,8 +615,18 @@ AP_Mount::calc_GPS_target_angle(const struct Location *target)
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.0f*pythagorous2(GPS_vector_x, GPS_vector_y); // Careful , centimeters here locally. Baro/alt is in cm, lat/lon is in meters.
_roll_control_angle = 0;
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
if (mount_axis_mask & MASK_TILT) {
_tilt_control_angle = atan2f(GPS_vector_z, target_distance);
} else {
_tilt_control_angle = 0;
}
if (mount_axis_mask & MASK_YAW) {
_pan_control_angle = atan2f(GPS_vector_x, GPS_vector_y);
} else {
_pan_control_angle = 0;
}
}
/// Stabilizes mount relative to the Earth's frame

2
libraries/AP_Mount/AP_Mount.h
View File

@ -98,6 +98,8 @@ private:
uint8_t _pan_idx; ///< RC_Channel_aux mount pan function index
uint8_t _open_idx; ///< RC_Channel_aux mount open function index
uint8_t mount_axis_mask; // used to track user input on each axis
float _roll_control_angle; ///< radians
float _tilt_control_angle; ///< radians
float _pan_control_angle; ///< radians