fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-02 15:01:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

POSCTRL:

Browse Source 
- make sure we always intialize the waypoints
  on the desired path and that the previous waypoint is
  behind the plane
- replace magic numbers with defines
This commit is contained in:
tumbili
2015-06-05 22:45:07 +02:00
committed by Lorenz Meier
parent 7f07e4306a
commit e5e4db1923
1 changed files with 14 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+14 -7
View File
@@ -95,6 +95,10 @@
static int _control_task = -1; /**< task handle for sensor task */ static int _control_task = -1; /**< task handle for sensor task */
#define HDG_HOLD_DIST_NEXT 3000.0f
#define HDG_HOLD_REACHED_DIST 1000.0f
#define HDG_HOLD_SET_BACK_DIST 100.0f
/** /**
* L1 control app start / stop handling function * L1 control app start / stop handling function
@@ -911,14 +915,17 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
waypoint_prev.lat = _global_pos.lat; waypoint_prev.lat = _global_pos.lat;
waypoint_prev.lon = _global_pos.lon; waypoint_prev.lon = _global_pos.lon;
} else { } else {
// use waypoint which is still in front of us /*
waypoint_prev.lat = waypoint_next.lat; for previous waypoint use the one still in front of us but shift it such that it is
waypoint_prev.lon = waypoint_next.lon; located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us
*/
waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
} }
waypoint_next.valid = true; waypoint_next.valid = true;
waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)distance / 1e6; waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)distance / 1e6; waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_next.alt = _hold_alt; waypoint_next.alt = _hold_alt;
} }
@@ -1424,8 +1431,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
/* we have a valid heading hold position, are we too close? */ /* we have a valid heading hold position, are we too close? */
if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < 1000) { _hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) {
get_waypoint_heading_distance(_hdg_hold_yaw, 3000, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false); get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
} }
math::Vector<2> prev_wp; math::Vector<2> prev_wp;