From 7b03bce416382e63db2fecce598697f743576462 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 2 Sep 2016 01:13:43 -0400 Subject: [PATCH] Navigator mission FOH use previous loiter altitude -the previous item altitude wasn't valid if switching from LOITER -> MISSION -fixes #5395 --- src/modules/navigator/mission.cpp | 22 ++++++++++------------ src/modules/navigator/mission.h | 3 +-- 2 files changed, 11 insertions(+), 14 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 63d79a8c62..a06a1b8783 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -86,7 +86,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _need_mission_reset(false), _missionFeasibilityChecker(), _min_current_sp_distance_xy(FLT_MAX), - _mission_item_previous_alt(NAN), _distance_current_previous(0.0f), _work_item_type(WORK_ITEM_TYPE_DEFAULT) { @@ -401,12 +400,6 @@ 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)) { @@ -928,9 +921,13 @@ Mission::altitude_sp_foh_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(_mission_item_previous_alt)) { + /* Don't change setpoint if last and current waypoint are not valid + * or if the previous altitude isn't from a position or loiter setpoint + */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || !PX4_ISFINITE(pos_sp_triplet->previous.alt) + || !(pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_POSITION || + pos_sp_triplet->previous.type == position_setpoint_s::SETPOINT_TYPE_LOITER)) { + return; } @@ -949,6 +946,7 @@ Mission::altitude_sp_foh_update() || _mission_item.nav_cmd == NAV_CMD_TAKEOFF || _mission_item.nav_cmd == NAV_CMD_LOITER_TO_ALT || !_navigator->is_planned_mission()) { + return; } @@ -972,10 +970,10 @@ Mission::altitude_sp_foh_update() * The setpoint is set linearly and such that the system reaches the current altitude at the acceptance * radius around the current waypoint **/ - float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt); + float delta_alt = (get_absolute_altitude_for_item(_mission_item) - pos_sp_triplet->previous.alt); float grad = -delta_alt / (_distance_current_previous - _navigator->get_acceptance_radius( _mission_item.acceptance_radius)); - float a = _mission_item_previous_alt - grad * _distance_current_previous; + float a = pos_sp_triplet->previous.alt - grad * _distance_current_previous; pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 57e137dd9b..346b6e3e1e 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -257,8 +257,7 @@ private: MissionFeasibilityChecker _missionFeasibilityChecker; /**< class that checks if a mission is feasible */ float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ - float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, - can be replaced by a full copy of the previous mission item if needed */ + float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */