dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Robustified flight close to waypoints

This commit is contained in:
Lorenz Meier 2013-10-06 21:04:59 +02:00
parent d1871bd7bb
commit 81e9c06129
2 changed files with 24 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/lib/ecl/l1/ecl_l1_pos_controller.cpp
View File

@ -86,6 +86,7 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
const math::Vector2f &ground_speed_vector)
{
/* this follows the logic presented in [1] */
float eta;
@ -116,6 +117,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* calculate the vector from waypoint A to the aircraft */
math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
math::Vector2f vector_B_to_airplane = get_local_planar_vector(vector_B, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
@ -126,6 +128,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
* If the aircraft is already between A and B normal L1 logic is applied.
*/
float distance_A_to_airplane = vector_A_to_airplane.length();
float distance_B_to_airplane = vector_B_to_airplane.length();
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* extension from [2] */
@ -143,6 +146,19 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
} else if (distance_B_to_airplane < alongTrackDist || distance_B_to_airplane < _L1_distance) {
/* fly directly to B */
/* unit vector from waypoint B to current position */
math::Vector2f vector_B_to_airplane_unit = vector_B_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_B_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_B_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
_nav_bearing = atan2f(-vector_B_to_airplane_unit.getY() , -vector_B_to_airplane_unit.getX());
} else {
/* calculate eta to fly along the line between A and B */

8
src/lib/ecl/l1/ecl_l1_pos_controller.h
View File

@ -130,6 +130,14 @@ public:
bool reached_loiter_target();
/**
* Returns true if following a circle (loiter)
*/
bool circle_mode() {
return _circle_mode;
}
/**
* Get the switch distance
*