Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-01 11:06:04 +08:00
Code Issues Actions 18 Packages Projects Releases Wiki Activity

prevent yaw setpoint from changeing constantly when we're over it

Browse Source
This commit is contained in:
Andreas Antener
2016-02-02 21:30:24 +01:00
parent 26c635359e
commit fec7950424
1 changed files with 17 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/navigator/mission.cpp
+17 -21
View File
@@ -648,8 +648,9 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item)
void void
Mission::heading_sp_update() Mission::heading_sp_update()
{ {
/* we don't want to be yawing during takeoff or align */ /* we don't want to be yawing during takeoff, landing or align */
if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF
|| _mission_item.nav_cmd == NAV_CMD_LAND
|| _work_item_type == WORK_ITEM_TYPE_ALIGN) { || _work_item_type == WORK_ITEM_TYPE_ALIGN) {
return; return;
} }
@@ -665,42 +666,37 @@ Mission::heading_sp_update()
/* set yaw angle for the waypoint iff a loiter time has been specified */ /* set yaw angle for the waypoint iff a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
_mission_item.yaw = _on_arrival_yaw; _mission_item.yaw = _on_arrival_yaw;
} else { } else {
/* calculate direction the vehicle should point to: /**
* normal waypoint: current position to {waypoint or home or home + 180deg} * Calculate direction the vehicle should point to.
* landing waypoint: last waypoint to {waypoint or home or home + 180deg} * To avoid excessive yawing when near the next waypoint we calculate heading between
* For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground * previous and current waypoint instead of current location.
*/ */
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 */ /* always keep the front of the rotary wing pointing to the next waypoint */
if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
|| _navigator->get_vstatus()->is_vtol) { || _navigator->get_vstatus()->is_vtol) {
_mission_item.yaw = get_bearing_to_next_waypoint( _mission_item.yaw = get_bearing_to_next_waypoint(
point_from_latlon[0], pos_sp_triplet->previous.lat,
point_from_latlon[1], pos_sp_triplet->previous.lon,
_mission_item.lat, pos_sp_triplet->current.lat,
_mission_item.lon); pos_sp_triplet->current.lon);
/* always keep the back of the rotary wing pointing towards home */ /* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
_mission_item.yaw = get_bearing_to_next_waypoint( _mission_item.yaw = get_bearing_to_next_waypoint(
point_from_latlon[0], pos_sp_triplet->previous.lat,
point_from_latlon[1], pos_sp_triplet->previous.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lat,
_navigator->get_home_position()->lon); _navigator->get_home_position()->lon);
/* always keep the back of the rotary wing pointing towards home */ /* always keep the back of the rotary wing pointing towards home */
} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
point_from_latlon[0], pos_sp_triplet->previous.lat,
point_from_latlon[1], pos_sp_triplet->previous.lon,
_navigator->get_home_position()->lat, _navigator->get_home_position()->lat,
_navigator->get_home_position()->lon) + M_PI_F); _navigator->get_home_position()->lon) + M_PI_F);
} }