yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

only update previous setpoint if we read a new actual position setpoin, prevent yawing between waypoints inside the acceptance radius

Browse Source
This commit is contained in:
Andreas Antener
2016-02-03 17:37:45 +01:00
parent 4e0559eacf
commit 46bd1cbacf
1 changed files with 49 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -327,14 +327,6 @@ Mission::set_mission_items()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* the home dist check provides user feedback, so we initialize it to this */
bool user_feedback_done = false;
@@ -417,6 +409,14 @@ Mission::set_mission_items()
/* handle position mission items */
if (item_contains_position(&_mission_item)) {
/* we have a new position item so set previous position setpoint to current */
set_previous_pos_setpoint();
/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
if (pos_sp_triplet->previous.valid) {
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
if (pos_sp_triplet->current.valid) {
_on_arrival_yaw = _mission_item.yaw;
}
@@ -451,7 +451,7 @@ Mission::set_mission_items()
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
}
/* move to landing wayponit before descent if necessary */
/* move to landing waypoint before descent if necessary */
if (do_need_move_to_land() && _work_item_type != WORK_ITEM_TYPE_MOVE_TO_LAND) {
new_work_item_type = WORK_ITEM_TYPE_MOVE_TO_LAND;
@@ -525,21 +525,21 @@ Mission::set_mission_items()
&& _work_item_type != WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& !_navigator->get_vstatus()->is_rotary_wing
&& !_navigator->get_vstatus()->condition_landed
&& pos_sp_triplet->previous.valid) {
&& pos_sp_triplet->current.valid) {
new_work_item_type = WORK_ITEM_TYPE_CMD_BEFORE_MOVE;
}
/* after FW to MC transition finish moving to the waypoint */
if (_work_item_type == WORK_ITEM_TYPE_CMD_BEFORE_MOVE
&& pos_sp_triplet->previous.valid) {
&& pos_sp_triplet->current.valid) {
new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.lat = pos_sp_triplet->previous.lat;
_mission_item.lon = pos_sp_triplet->previous.lon;
_mission_item.altitude = pos_sp_triplet->previous.alt;
_mission_item.lat = pos_sp_triplet->current.lat;
_mission_item.lon = pos_sp_triplet->current.lon;
_mission_item.altitude = pos_sp_triplet->current.alt;
_mission_item.altitude_is_relative = false;
_mission_item.autocontinue = true;
_mission_item.time_inside = 0;
@@ -682,7 +682,7 @@ Mission::heading_sp_update()
return;
}
/* set yaw angle for the waypoint iff a loiter time has been specified */
/* set yaw angle for the waypoint if a loiter time has been specified */
if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
_mission_item.yaw = _on_arrival_yaw;
@@ -693,32 +693,44 @@ Mission::heading_sp_update()
* To avoid excessive yawing when near the next waypoint we calculate heading between
* previous and current waypoint instead of current location.
*/
double point_from_latlon[2];
double point_to_latlon[2];
/* always keep the front of the rotary wing pointing to the next waypoint */
if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT
|| _navigator->get_vstatus()->is_vtol) {
_mission_item.yaw = get_bearing_to_next_waypoint(
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon,
pos_sp_triplet->current.lat,
pos_sp_triplet->current.lon);
point_from_latlon[0] = pos_sp_triplet->previous.lat;
point_from_latlon[1] = pos_sp_triplet->previous.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(
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon,
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon);
/* default target location is next (current) waypoint */
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.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(
pos_sp_triplet->previous.lat,
pos_sp_triplet->previous.lon,
_navigator->get_home_position()->lat,
_navigator->get_home_position()->lon) + M_PI_F);
/* target location is home */
if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME
|| _param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
point_to_latlon[0] = _navigator->get_home_position()->lat;
point_to_latlon[1] = _navigator->get_home_position()->lon;
}
float d_current = get_distance_to_next_waypoint(
point_from_latlon[0], point_from_latlon[1],
point_to_latlon[0], point_to_latlon[1]);
/* don't yaw if positions are on top of each other */
if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0],
point_from_latlon[1],
point_to_latlon[0],
point_to_latlon[1]);
/* always keep the back of the rotary wing pointing towards home */
if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
_mission_item.yaw = _wrap_pi(yaw + M_PI_F);
} else {
_mission_item.yaw = yaw;
}
}
}
mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);