diff --git a/src/modules/mavlink/mavlink_mission.cpp b/src/modules/mavlink/mavlink_mission.cpp index dd94c09294..5fed426c3c 100644 --- a/src/modules/mavlink/mavlink_mission.cpp +++ b/src/modules/mavlink/mavlink_mission.cpp @@ -805,7 +805,6 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t * break; case MAV_CMD_DO_SET_SERVO: - mission_item->actuator_num = mavlink_mission_item->param1; mission_item->actuator_value = mavlink_mission_item->param2; mission_item->time_inside=0.0f; diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 0deee84345..94d269c44c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -77,7 +77,6 @@ Mission::Mission(Navigator *navigator, const char *name) : _current_onboard_mission_index(-1), _current_offboard_mission_index(-1), _need_takeoff(true), - _takeoff(false), _mission_type(MISSION_TYPE_NONE), _inited(false), _home_inited(false), @@ -287,24 +286,23 @@ Mission::update_offboard_mission() void Mission::advance_mission() { - if (_takeoff) { - _takeoff = false; - _takeoff_finished = true; + /* do not advance mission item if we're processing sub mission work items */ + if (_work_item_type != WORK_ITEM_TYPE_DEFAULT) { + return; + } - } else { - switch (_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _current_onboard_mission_index++; + break; - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; + case MISSION_TYPE_OFFBOARD: + _current_offboard_mission_index++; + break; - case MISSION_TYPE_NONE: - default: - break; - } + case MISSION_TYPE_NONE: + default: + break; } } @@ -341,11 +339,13 @@ Mission::set_mission_items() bool user_feedback_done = false; /* mission item that comes after current if available */ - struct mission_item_s mission_item_next; - bool has_next_item = false; + struct mission_item_s mission_item_next_position; + bool has_next_position_item = false; + + work_item_type new_work_item_type = WORK_ITEM_TYPE_DEFAULT; /* try setting onboard mission item */ - if (_param_onboard_enabled.get() && prepare_mission_items(true, &_mission_item, &mission_item_next, &has_next_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 */ if (_mission_type != MISSION_TYPE_ONBOARD) { mavlink_log_critical(_navigator->get_mavlink_fd(), "onboard mission now running"); @@ -354,7 +354,7 @@ Mission::set_mission_items() _mission_type = MISSION_TYPE_ONBOARD; /* try setting offboard mission item */ - } else if (prepare_mission_items(false, &_mission_item, &mission_item_next, &has_next_item)) { + } else if (prepare_mission_items(false, &_mission_item, &mission_item_next_position, &has_next_position_item)) { /* if mission type changed, notify */ if (_mission_type != MISSION_TYPE_OFFBOARD) { mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running"); @@ -410,56 +410,26 @@ Mission::set_mission_items() return; } - if (!item_contains_position(&_mission_item)) { - // XXX: before issuing command, check if we need to align for transition first - issue_command(&_mission_item); - return; - } + /*********************************** handle mission item *********************************************/ - if (pos_sp_triplet->current.valid) { - _on_arrival_yaw = _mission_item.yaw; - } + /* handle position mission items */ + if (item_contains_position(&_mission_item)) { - /* do takeoff on first waypoint for rotary wing vehicles */ - if (_navigator->get_vstatus()->is_rotary_wing) { - /* force takeoff if landed (additional protection) */ - if (!_takeoff && _navigator->get_vstatus()->condition_landed) { - _need_takeoff = true; + if (pos_sp_triplet->current.valid) { + _on_arrival_yaw = _mission_item.yaw; } - /* new current mission item set, check if we need takeoff */ - if (_need_takeoff && ( - _mission_item.nav_cmd == NAV_CMD_TAKEOFF || - _mission_item.nav_cmd == NAV_CMD_WAYPOINT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || - _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || - _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || - _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { - _takeoff = true; - _need_takeoff = false; - } - } + /* 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; - if (_takeoff) { - /* do takeoff before going to setpoint */ - /* set mission item as next position setpoint */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next); - /* next SP is not takeoff anymore */ - pos_sp_triplet->next.type = position_setpoint_s::SETPOINT_TYPE_POSITION; + /* use current mission item as next position item */ + memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); + mission_item_next_position.nav_cmd = NAV_CMD_WAYPOINT; + has_next_position_item = true; - /* calculate takeoff altitude */ - float takeoff_alt = get_absolute_altitude_for_item(_mission_item); + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); - /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ - if (_navigator->get_vstatus()->condition_landed) { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); - - } else { - takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); - } - - /* check if we already above takeoff altitude */ - if (_navigator->get_global_position()->alt < takeoff_alt) { mavlink_log_critical(_navigator->get_mavlink_fd(), "takeoff to %.1f meters above home", (double)(takeoff_alt - _navigator->get_home_position()->alt)); _mission_item.nav_cmd = NAV_CMD_TAKEOFF; @@ -470,27 +440,47 @@ Mission::set_mission_items() _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; _mission_item.time_inside = 0; - - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); - - _navigator->set_position_setpoint_triplet_updated(); - return; - - } else { - /* skip takeoff */ - _takeoff = false; } + + /* if we just did a takeoff navigate to the actual waypoint now */ + if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { + new_work_item_type = WORK_ITEM_TYPE_DEFAULT; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + } + + /* move to landing wayponit 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; + + /* use current mission item as next position item */ + memcpy(&mission_item_next_position, &_mission_item, sizeof(struct mission_item_s)); + has_next_position_item = true; + + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.autocontinue = true; + _mission_item.time_inside = 0; + } + + /* we just moved to the landing waypoint, now descend */ + if (_work_item_type == WORK_ITEM_TYPE_MOVE_TO_LAND) { + 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); + } - if (_takeoff_finished) { - /* we just finished takeoff */ - /* in case we still have to move to the takeoff waypoint we need a waypoint mission item */ - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; - _takeoff_finished = false; - } + /*********************************** clean up and set next *********************************************/ - /* set current position setpoint from mission item */ - mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + /* set current work item type */ + _work_item_type = new_work_item_type; /* require takeoff after landing or idle */ if (pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_LAND || pos_sp_triplet->current.type == position_setpoint_s::SETPOINT_TYPE_IDLE) { @@ -508,9 +498,9 @@ Mission::set_mission_items() if (_mission_item.autocontinue && _mission_item.time_inside <= 0.001f) { /* try to process next mission item */ - if (has_next_item) { + if (has_next_position_item) { /* got next mission item, update setpoint triplet */ - mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); + mission_item_to_position_setpoint(&mission_item_next_position, &pos_sp_triplet->next); } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; @@ -532,10 +522,73 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } +bool +Mission::do_need_takeoff() +{ + if (_navigator->get_vstatus()->is_rotary_wing) { + float takeoff_alt = calculate_takeoff_altitude(&_mission_item); + + /* force takeoff if landed (additional protection) */ + if (_navigator->get_vstatus()->condition_landed) { + _need_takeoff = true; + + /* if in-air and already above takeoff height, don't do takeoff */ + } else if (_navigator->get_global_position()->alt > takeoff_alt) { + _need_takeoff = false; + } + + /* check if current mission item is one that requires takeoff before */ + if (_need_takeoff && ( + _mission_item.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || + _mission_item.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH)) { + + _need_takeoff = false; + return true; + } + } + + return false; +} + +bool +Mission::do_need_move_to_land() +{ + if (_navigator->get_vstatus()->is_rotary_wing && _mission_item.nav_cmd == NAV_CMD_LAND) { + + float d_current = get_distance_to_next_waypoint(_mission_item.lat, _mission_item.lon, + _navigator->get_global_position()->lat, _navigator->get_global_position()->lon); + + return d_current > _navigator->get_acceptance_radius(); + } + + return false; +} + +float +Mission::calculate_takeoff_altitude(struct mission_item_s *mission_item) +{ + /* calculate takeoff altitude */ + float takeoff_alt = get_absolute_altitude_for_item(*mission_item); + + /* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */ + if (_navigator->get_vstatus()->condition_landed) { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_global_position()->alt + _param_takeoff_alt.get()); + + } else { + takeoff_alt = fmaxf(takeoff_alt, _navigator->get_home_position()->alt + _param_takeoff_alt.get()); + } + + return takeoff_alt; +} + void Mission::heading_sp_update() { - if (_takeoff) { + if (_work_item_type == WORK_ITEM_TYPE_TAKEOFF) { /* we don't want to be yawing during takeoff */ return; } @@ -662,7 +715,7 @@ Mission::altitude_sp_foh_reset() bool Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item, - struct mission_item_s *next_mission_item, bool *has_next_item) + struct mission_item_s *next_position_mission_item, bool *has_next_position_item) { bool first_res = false; int offset = 1; @@ -672,10 +725,10 @@ Mission::prepare_mission_items(bool onboard, struct mission_item_s *mission_item first_res = true; /* trying to find next position mission item */ - while(read_mission_item(onboard, offset, next_mission_item)) { + while(read_mission_item(onboard, offset, next_position_mission_item)) { - if (item_contains_position(next_mission_item)) { - *has_next_item = true; + if (item_contains_position(next_position_mission_item)) { + *has_next_position_item = true; break; } diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index 036968f317..136475df67 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -119,6 +119,21 @@ private: */ void set_mission_items(); + /** + * Returns true if we need to do a takeoff at the current state + */ + bool do_need_takeoff(); + + /** + * Returns true if we need to move to waypoint location before starting descent + */ + bool do_need_move_to_land(); + + /** + * Calculate takeoff height for mission item considering ground clearance + */ + float calculate_takeoff_altitude(struct mission_item_s *mission_item); + /** * Updates the heading of the vehicle. Rotary wings only. */ @@ -136,11 +151,19 @@ private: float get_absolute_altitude_for_item(struct mission_item_s &mission_item); + /** + * Read the current and the next mission item. The next mission item read is the + * next mission item that contains a position. + * + * @return true if current mission item available + */ bool prepare_mission_items(bool onboard, struct mission_item_s *mission_item, - struct mission_item_s *next_mission_item, bool *has_next_item); + struct mission_item_s *next_position_mission_item, bool *has_next_position_item); /** - * Read current or next mission item from the dataman and watch out for DO_JUMPS + * Read current (offset == 0) or a specific (offset > 0) mission item + * from the dataman and watch out for DO_JUMPS + * * @return true if successful */ bool read_mission_item(bool onboard, int offset, struct mission_item_s *mission_item); @@ -187,8 +210,6 @@ private: int _current_onboard_mission_index; int _current_offboard_mission_index; bool _need_takeoff; /**< if true, then takeoff must be performed before going to the first waypoint (if needed) */ - bool _takeoff; /**< takeoff state flag */ - bool _takeoff_finished; /**< set if takeoff was requested before and is now done */ enum { MISSION_TYPE_NONE, @@ -208,13 +229,12 @@ private: float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ - enum { - WORK_ITEM_TYPE_DEFAULT, - WORK_ITEM_TYPE_TAKEOFF, - WORK_ITEM_TYPE_LAND, - WORK_ITEM_TYPE_TRANSITION, - WORK_ITEM_TYPE_YAW - } _work_item_type; /**< current type of work to do, intermediate to complete mission item */ + enum work_item_type { + WORK_ITEM_TYPE_DEFAULT, /**< default mission item */ + WORK_ITEM_TYPE_TAKEOFF, /**< takeoff before moving to waypoint */ + WORK_ITEM_TYPE_MOVE_TO_LAND, /**< move to land waypoint before descent */ + WORK_ITEM_TYPE_ALIGN /**< align for next waypoint */ + } _work_item_type; /**< current type of work to do (sub mission item) */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 6020e087ac..e2a39c846e 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -83,7 +83,7 @@ MissionBlock::is_mission_item_reached() { /* handle non-navigation or indefinite waypoints */ switch (_mission_item.nav_cmd) { - // XXX: move handling to mission as well + // XXX: move handling to issue_command() as well case NAV_CMD_DO_SET_SERVO: { memset(&actuators, 0, sizeof(actuators)); actuators.control[_mission_item.actuator_num] = 1.0f / 2000 * -_mission_item.actuator_value; @@ -296,7 +296,7 @@ MissionBlock::mission_item_to_position_setpoint(const struct mission_item_s *ite sp->acceptance_radius = item->acceptance_radius; switch (item->nav_cmd) { - case NAV_CMD_DO_SET_SERVO: // XXX: should be also return in the beginning for this? + case NAV_CMD_DO_SET_SERVO: // XXX: actually also a non position item /* Set current position for loitering set point*/ sp->lat = _navigator->get_global_position()->lat; sp->lon = _navigator->get_global_position()->lon; diff --git a/src/modules/navigator/navigation.h b/src/modules/navigator/navigation.h index 0a8de21b95..f5175b3300 100644 --- a/src/modules/navigator/navigation.h +++ b/src/modules/navigator/navigation.h @@ -103,8 +103,8 @@ struct mission_item_s { int do_jump_mission_index; /**< index where the do jump will go to */ unsigned do_jump_repeat_count; /**< how many times do jump needs to be done */ unsigned do_jump_current_count; /**< count how many times the jump has been done */ - int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ - int actuator_value; /**< new value for selected actuator in ms 900...2000 */ + int actuator_num; /**< actuator number to be set 0..5 ( corresponds to AUX outputs 1..6 */ + int actuator_value; /**< new value for selected actuator in ms 900...2000 */ float params[7]; /**< array to store mission command values for MAV_FRAME_MISSION ***/ }; #pragma pack(pop)