From 7af08165706d5552f1825d91b78b65b1021a2c94 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Mon, 8 Feb 2016 09:30:05 +0100 Subject: [PATCH] fixed yaw calculation for all yaw modes and stay during loiter items --- src/modules/navigator/mission.cpp | 47 +++++++++++++------------------ 1 file changed, 20 insertions(+), 27 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 73da4a37cc..becafd6e0b 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -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],