fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-27 06:23:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix waypoint handling in position control

Browse Source
This commit is contained in:
Roman Bapst
2016-01-14 18:30:30 +01:00
committed by Lorenz Meier
parent 9fb29d3a38
commit b26fc1f089
1 changed files with 28 additions and 11 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
+28 -11
View File
@@ -973,24 +973,41 @@ void FixedwingPositionControl::get_waypoint_heading_distance(float heading, floa
{ {
waypoint_prev.valid = true; waypoint_prev.valid = true;
waypoint_prev.alt = _hold_alt; waypoint_prev.alt = _hold_alt;
position_setpoint_s temp_next {};
position_setpoint_s temp_prev {};
if (flag_init) { if (flag_init) {
// on init set first waypoint to momentary position // on init set previous waypoint HDG_HOLD_SET_BACK_DIST meters behind us
waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading + 180.0f * M_DEG_TO_RAD_F ,
waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6; HDG_HOLD_SET_BACK_DIST,
&temp_prev.lat, &temp_prev.lon);
// set next waypoint HDG_HOLD_DIST_NEXT meters in front of us
waypoint_from_heading_and_distance(_global_pos.lat, _global_pos.lon, heading, HDG_HOLD_DIST_NEXT,
&temp_next.lat, &temp_next.lon);
waypoint_prev = temp_prev;
waypoint_next = temp_next;
waypoint_next.valid = true;
waypoint_next.alt = _hold_alt;
return;
} else { } else {
/* // for previous waypoint use the one still in front of us but shift it such that it is
for previous waypoint use the one still in front of us but shift it such that it is // located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us
located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon,
*/ HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST,
waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6; &temp_prev.lat, &temp_prev.lon);
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 + HDG_HOLD_SET_BACK_DIST) / 1e6;
waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6; create_waypoint_from_line_and_dist(waypoint_next.lat, waypoint_next.lon, waypoint_prev.lat, waypoint_prev.lon,
-(HDG_HOLD_DIST_NEXT + HDG_HOLD_REACHED_DIST),
&temp_next.lat, &temp_next.lon);
waypoint_prev = temp_prev;
waypoint_next = temp_next;
waypoint_next.alt = _hold_alt; waypoint_next.alt = _hold_alt;
} }