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

AP_L1_Control: implement turn_distance() with turn angle

Browse Source 
uses a linear approximation for now.
This commit is contained in:
Andrew Tridgell 2014-06-05 09:34:23 +10:00
parent 7a6186f7e6
commit 5a1aa8dfe7
2 changed files with 22 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

21
libraries/AP_L1_Control/AP_L1_Control.cpp
View File

@ -70,12 +70,33 @@ int32_t AP_L1_Control::target_bearing_cd(void) const
return wrap_180_cd(_target_bearing_cd);
}
/*
this is the turn distance assuming a 90 degree turn
*/
float AP_L1_Control::turn_distance(float wp_radius) const
{
wp_radius *= sq(_ahrs.get_EAS2TAS());
return min(wp_radius, _L1_dist);
}
/*
this approximates the turn distance for a given turn angle. If the
turn_angle is > 90 then a 90 degree turn distance is used, otherwise
the turn distance is reduced linearly.
This function allows straight ahead mission legs to avoid thinking
they have reached the waypoint early, which makes things like camera
trigger and ball drop at exact positions under mission control much easier
*/
float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const
{
float distance_90 = turn_distance(wp_radius);
turn_angle = fabsf(turn_angle);
if (turn_angle >= 90) {
return distance_90;
}
return distance_90 * turn_angle / 90.0f;
}
bool AP_L1_Control::reached_loiter_target(void)
{
return _WPcircle;

1
libraries/AP_L1_Control/AP_L1_Control.h
View File

@ -44,6 +44,7 @@ public:
int32_t target_bearing_cd(void) const;
float turn_distance(float wp_radius) const;
float turn_distance(float wp_radius, float turn_angle) const;
void update_waypoint(const struct Location &prev_WP, const struct Location &next_WP);
void update_loiter(const struct Location &center_WP, float radius, int8_t loiter_direction);
void update_heading_hold(int32_t navigation_heading_cd);