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

fixed yaw calculation for all yaw modes and stay during loiter items

Browse Source
This commit is contained in:
Andreas Antener
2016-02-08 09:30:05 +01:00
parent 20ef3adb1f
commit 7af0816570
1 changed files with 20 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -83,7 +83,6 @@ Mission::Mission(Navigator *navigator, const char *name) :
_missionFeasibilityChecker(),
_min_current_sp_distance_xy(FLT_MAX),
_mission_item_previous_alt(NAN),
_on_arrival_yaw(NAN),
_distance_current_previous(0.0f),
_work_item_type(WORK_ITEM_TYPE_DEFAULT)
{
@@ -177,11 +176,11 @@ Mission::on_active()
/* switch to next waypoint if 'autocontinue' flag set */
advance_mission();
set_mission_items();
}
} else if (_mission_type != MISSION_TYPE_NONE &&_param_altmode.get() == MISSION_ALTMODE_FOH) {
altitude_sp_foh_update();
} else {
/* if waypoint position reached allow loiter on the setpoint */
if (_waypoint_position_reached && _mission_item.nav_cmd != NAV_CMD_IDLE) {
@@ -336,6 +335,12 @@ Mission::set_mission_items()
work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT;
/* copy information about the previous mission item */
if (item_contains_position(&_mission_item) && pos_sp_triplet->current.valid) {
/* Copy previous mission item altitude */
_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
}
/* try setting onboard mission item */
if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next_position, &has_next_position_item)) {
/* if mission type changed, notify */
@@ -412,15 +417,6 @@ Mission::set_mission_items()
/* 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;
}
/* do takeoff before going to setpoint if needed and not already in takeoff */
if (do_need_takeoff() && _work_item_type != WORK_ITEM_TYPE_TAKEOFF) {
new_work_item_type = WORK_ITEM_TYPE_TAKEOFF;
@@ -680,44 +676,41 @@ Mission::heading_sp_update()
struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
/* Don't change setpoint if last and current waypoint are not valid */
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid ||
!PX4_ISFINITE(_on_arrival_yaw)) {
if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid) {
return;
}
/* 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;
// XXX: should actually be param4 from mission item
// at the moment it will just keep the heading it has
//_mission_item.yaw = _on_arrival_yaw;
} else {
/**
* Calculate direction the vehicle should point to.
* To avoid excessive yawing when near the next waypoint we calculate heading between
* previous and current waypoint instead of current location.
*/
/* Calculate direction the vehicle should point to. */
double point_from_latlon[2];
double point_to_latlon[2];
point_from_latlon[0] = pos_sp_triplet->previous.lat;
point_from_latlon[1] = pos_sp_triplet->previous.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;
point_from_latlon[0] = _navigator->get_global_position()->lat;
point_from_latlon[1] = _navigator->get_global_position()->lon;
/* 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;
/* target location is next (current) waypoint */
} else {
point_to_latlon[0] = pos_sp_triplet->current.lat;
point_to_latlon[1] = pos_sp_triplet->current.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 */
/* stop if positions are close together to prevent excessive yawing */
if (d_current > _navigator->get_acceptance_radius()) {
float yaw = get_bearing_to_next_waypoint(
point_from_latlon[0],