From 02aa0eacd52eeb6c68bfef10430d84249206f984 Mon Sep 17 00:00:00 2001 From: Andreas Antener Date: Thu, 28 Jan 2016 09:05:10 +0100 Subject: [PATCH] align towards next waypoint before MC to FW transition --- src/modules/navigator/mission.cpp | 59 +++++++++++++++++++------ src/modules/navigator/mission_block.cpp | 13 +++--- 2 files changed, 52 insertions(+), 20 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 94d269c44c..0674d468d6 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -412,6 +412,8 @@ Mission::set_mission_items() /*********************************** handle mission item *********************************************/ + bool do_issue_command = true; + /* handle position mission items */ if (item_contains_position(&_mission_item)) { @@ -466,18 +468,50 @@ Mission::set_mission_items() new_work_item_type = WORK_ITEM_TYPE_DEFAULT; } - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - /* handle non-position mission items such as commands */ } else { - // XXX: before issuing command, check if we need to align for transition first - issue_command(&_mission_item); + /* turn towards next waypoint before MC to FW transition */ + if (_mission_item.nav_cmd == NAV_CMD_DO_VTOL_TRANSITION + && _work_item_type != WORK_ITEM_TYPE_ALIGN + && _navigator->get_vstatus()->is_rotary_wing + && !_navigator->get_vstatus()->condition_landed + && has_next_position_item) { + + new_work_item_type = WORK_ITEM_TYPE_ALIGN; + + _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.lat = _navigator->get_global_position()->lat; + _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.altitude = _navigator->get_global_position()->alt; + _mission_item.altitude_is_relative = false; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + mission_item_next_position.lat, + mission_item_next_position.lon); + + do_issue_command = false; + } + + /* yaw is aligned now */ + if (_work_item_type == WORK_ITEM_TYPE_ALIGN) { + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + } } - /*********************************** clean up and set next *********************************************/ + /*********************************** set setpoints and check next *********************************************/ + + /* set current position setpoint from mission item (is protected agains non-position items) */ + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + + /* issue command if ready (will do nothing for position mission items) */ + if (do_issue_command) { + issue_command(&_mission_item); + } /* set current work item type */ _work_item_type = new_work_item_type; @@ -513,7 +547,8 @@ Mission::set_mission_items() /* Save the distance between the current sp and the previous one */ if (pos_sp_triplet->current.valid && pos_sp_triplet->previous.valid) { - _distance_current_previous = get_distance_to_next_waypoint(pos_sp_triplet->current.lat, + _distance_current_previous = get_distance_to_next_waypoint( + pos_sp_triplet->current.lat, pos_sp_triplet->current.lon, pos_sp_triplet->previous.lat, pos_sp_triplet->previous.lon); @@ -588,8 +623,9 @@ Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) void Mission::heading_sp_update() { - if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { - /* we don't want to be yawing during takeoff */ + /* we don't want to be yawing during takeoff or align */ + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF + || _work_item_type == WORK_ITEM_TYPE_ALIGN) { return; } @@ -601,11 +637,6 @@ Mission::heading_sp_update() return; } - /* Don't change heading for takeoff waypoints, the ground may be near */ - if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { - return; - } - /* set yaw angle for the waypoint iff a loiter time has been specified */ if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { _mission_item.yaw = _on_arrival_yaw; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index e2a39c846e..e62d9f3a99 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -105,10 +105,10 @@ MissionBlock::is_mission_item_reached() case NAV_CMD_LOITER_UNLIMITED: return false; - case vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL: /* fallthrough */ - case vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION: + case NAV_CMD_DO_DIGICAM_CONTROL: /* fallthrough */ + case NAV_CMD_DO_VTOL_TRANSITION: // XXX: we should wait on command ACK or status change instead - // currently we just wait so the command be processed + // currently we just wait so the command can be processed if (hrt_absolute_time() - _action_start > 1000) { return true; @@ -185,6 +185,7 @@ MissionBlock::is_mission_item_reached() /* check yaw if defined only for rotary wing except takeoff */ float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + PX4_WARN("yaw err: %.3f", (float)yaw_err); if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ _waypoint_yaw_reached = true; @@ -251,7 +252,7 @@ MissionBlock::issue_command(const struct mission_item_s *item) return; } - warnx("forwarding command.\n"); + warnx("forwarding command %d\n", item->nav_cmd); struct vehicle_command_s cmd = {}; mission_item_to_vehicle_command(item, &cmd); _action_start = hrt_absolute_time(); @@ -268,8 +269,8 @@ bool MissionBlock::item_contains_position(const struct mission_item_s *item) { // XXX: maybe extend that check onto item properties - if (item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_DIGICAM_CONTROL || - item->nav_cmd == vehicle_command_s::VEHICLE_CMD_DO_VTOL_TRANSITION) { + if (item->nav_cmd == NAV_CMD_DO_DIGICAM_CONTROL || + item->nav_cmd == NAV_CMD_DO_VTOL_TRANSITION) { return false; }