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

Improve landing for special yawmode settings

This commit is contained in:
Thomas Gubler 2015-12-28 11:15:03 +01:00
parent 2af066bc6a
commit 7c52e8b96f
1 changed files with 40 additions and 24 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

36
src/modules/navigator/mission.cpp
View File

@ -526,37 +526,53 @@ Mission::heading_sp_update()
return;
}
/* Don't do FOH for landing and takeoff waypoints, the ground may be near
* and the FW controller has a custom landing logic */
if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
/* Don't change heading for takeoff waypoints, the ground may be near */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
return;
}
/* set yaw angle for the waypoint iff a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
_mission_item.yaw = _on_arrival_yaw;
} else {
/* calculate direction the vehicle should point to:
* normal waypoint: current position to {waypoint or home or home + 180deg}
* landing waypoint: last waypoint to {waypoint or home or home + 180deg}
* For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground
*/
double point_from_latlon[2];
if (_mission_item.nav_cmd == NAV_CMD_LAND) {
point_from_latlon[0] = pos_sp_triplet->previous.lat;
point_from_latlon[1] = pos_sp_triplet->previous.lon;
} else {
point_from_latlon[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon;
}
/* always keep the front of the rotary wing pointing to the next waypoint */
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
point_from_latlon[0],
point_from_latlon[1],
_mission_item.lat,
_mission_item.lon);
/* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
_mission_item.yaw = get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
point_from_latlon[0],
point_from_latlon[1],
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon);
/* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
point_from_latlon[0],
point_from_latlon[1],
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon) + M_PI_F);
}
}
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
_navigator->set_position_setpoint_triplet_updated();