From 854bb7fe089daf8bf3d4e9f2cac1cb2b99a67ac7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 3 Jun 2014 16:01:28 +0200 Subject: navigator: mission class added (WIP) --- src/modules/navigator/mission.cpp | 438 +++++++++++++++++++++++++++----------- 1 file changed, 315 insertions(+), 123 deletions(-) (limited to 'src/modules/navigator/mission.cpp') diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index adbb0cfa2..f3a86666f 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -38,30 +38,42 @@ * @author Julian Oes */ +#include #include #include +#include + +#include #include #include +#include #include #include +#include "navigator.h" #include "mission.h" -Mission::Mission() : - - _offboard_dataman_id(-1), - _current_offboard_mission_index(0), - _current_onboard_mission_index(0), - _offboard_mission_item_count(0), - _onboard_mission_item_count(0), - _onboard_mission_allowed(false), - _current_mission_type(MISSION_TYPE_NONE), +Mission::Mission(Navigator *navigator) : + SuperBlock(NULL, "MIS"), + _navigator(navigator), + _first_run(true), + _param_onboard_enabled(this, "ONBOARD_EN"), + _onboard_mission_sub(-1), + _offboard_mission_sub(-1), + _onboard_mission({0}), + _offboard_mission({0}), + _mission_item({0}), _mission_result_pub(-1), - _mission_result({}) + _mission_result({0}), + _mission_type(MISSION_TYPE_NONE) { + /* load initial params */ + updateParams(); + /* set initial mission items */ + reset(); } Mission::~Mission() @@ -69,134 +81,335 @@ Mission::~Mission() } void -Mission::set_offboard_dataman_id(const int new_id) +Mission::reset() { - _offboard_dataman_id = new_id; + _first_run = true; } -void -Mission::set_offboard_mission_count(int new_count) +bool +Mission::update(struct position_setpoint_triplet_s *pos_sp_triplet) { - _offboard_mission_item_count = new_count; + bool updated = false; + /* check if anything has changed, and reset mission items if needed */ + if (is_onboard_mission_updated() || is_offboard_mission_updated() || _first_run) { + set_mission_items(pos_sp_triplet); + updated = true; + _first_run = false; + } + + if (_mission_type != MISSION_TYPE_NONE && is_mission_item_reached()) { + advance_mission(); + set_mission_items(pos_sp_triplet); + updated = true; + } + return updated; } -void -Mission::set_onboard_mission_count(int new_count) +bool +Mission::is_mission_item_reached() { - _onboard_mission_item_count = new_count; + /* TODO: count turns */ +#if 0 + if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) && + _mission_item.loiter_radius > 0.01f) { + + return false; + } +#endif + + hrt_abstime now = hrt_absolute_time(); + + if (!_waypoint_position_reached) { + + float dist = -1.0f; + float dist_xy = -1.0f; + float dist_z = -1.0f; + + float altitude_amsl = _mission_item.altitude_is_relative + ? _mission_item.altitude + _navigator->get_home_position()->alt + : _mission_item.altitude; + + dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, altitude_amsl, + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_global_position()->alt, + &dist_xy, &dist_z); + + if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + + /* require only altitude for takeoff */ + if (_navigator->get_global_position()->alt > altitude_amsl - _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } else { + if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) { + _waypoint_position_reached = true; + } + } + } + + if (_waypoint_position_reached && !_waypoint_yaw_reached) { + + /* TODO: removed takeoff, why? */ + if (_navigator->get_vstatus()->is_rotary_wing && isfinite(_mission_item.yaw)) { + + /* check yaw if defined only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item.yaw - _navigator->get_global_position()->yaw); + + if (fabsf(yaw_err) < 0.2f) { /* TODO: get rid of magic number */ + _waypoint_yaw_reached = true; + } + + } else { + _waypoint_yaw_reached = true; + } + } + + /* check if the current waypoint was reached */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { + + if (_time_first_inside_orbit == 0) { + _time_first_inside_orbit = now; + + // if (_mission_item.time_inside > 0.01f) { + // mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", + // (double)_mission_item.time_inside); + // } + } + + /* check if the MAV was long enough inside the waypoint orbit */ + if (now - _time_first_inside_orbit >= (hrt_abstime)_mission_item.time_inside * 1e6f) { + return true; + } + } + return false; } void -Mission::set_onboard_mission_allowed(bool allowed) -{ - _onboard_mission_allowed = allowed; +Mission::reset_mission_item_reached() { + _waypoint_position_reached = false; + _waypoint_yaw_reached = false; + _time_first_inside_orbit = 0; } -bool -Mission::command_current_offboard_mission_index(const int new_index) +void +Mission::mission_item_to_position_setpoint(const mission_item_s *item, position_setpoint_s *sp) { - if (new_index >= 0) { - _current_offboard_mission_index = (unsigned)new_index; - } else { + sp->valid = true; - /* if less WPs available, reset to first WP */ - if (_current_offboard_mission_index >= _offboard_mission_item_count) { - _current_offboard_mission_index = 0; - } + sp->lat = item->lat; + sp->lon = item->lon; + sp->alt = item->altitude_is_relative ? item->altitude + _navigator->get_home_position()->alt : item->altitude; + sp->yaw = item->yaw; + sp->loiter_radius = item->loiter_radius; + sp->loiter_direction = item->loiter_direction; + sp->pitch_min = item->pitch_min; + + if (item->nav_cmd == NAV_CMD_TAKEOFF) { + sp->type = SETPOINT_TYPE_TAKEOFF; + + } else if (item->nav_cmd == NAV_CMD_LAND) { + sp->type = SETPOINT_TYPE_LAND; + + } else if (item->nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + item->nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + item->nav_cmd == NAV_CMD_LOITER_UNLIMITED) { + sp->type = SETPOINT_TYPE_LOITER; + + } else { + sp->type = SETPOINT_TYPE_POSITION; } - report_current_offboard_mission_item(); } bool -Mission::command_current_onboard_mission_index(int new_index) +Mission::is_onboard_mission_updated() { - if (new_index != -1) { - _current_onboard_mission_index = (unsigned)new_index; - } else { + bool updated; + orb_check(_onboard_mission_sub, &updated); - /* if less WPs available, reset to first WP */ - if (_current_onboard_mission_index >= _onboard_mission_item_count) { - _current_onboard_mission_index = 0; - } + if (!updated) { + return false; } - // TODO: implement this for onboard missions as well - // report_current_mission_item(); + + if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &_onboard_mission) != OK) { + _onboard_mission.count = 0; + _onboard_mission.current_index = 0; + } + return true; } bool -Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool *onboard, int *index) +Mission::is_offboard_mission_updated() { - *onboard = false; - *index = -1; - - /* try onboard mission first */ - if (_current_onboard_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_current_onboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_ONBOARD; - *onboard = true; - *index = _current_onboard_mission_index; - - return true; - } + bool updated; + orb_check(_offboard_mission_sub, &updated); + if (!updated) { + return false; } - /* otherwise fallback to offboard */ - if (_current_offboard_mission_index < _offboard_mission_item_count) { + if (orb_copy(ORB_ID(offboard_mission), _offboard_mission_sub, &_offboard_mission) == OK) { + /* Check mission feasibility, for now do not handle the return value, + * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */ dm_item_t dm_current; - if (_offboard_dataman_id == 0) { + + if (_offboard_mission.dataman_id == 0) { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; } else { dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - if (read_mission_item(dm_current, true, &_current_offboard_mission_index, new_mission_item)) { - _current_mission_type = MISSION_TYPE_OFFBOARD; - *onboard = false; - *index = _current_offboard_mission_index; + missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing, dm_current, + (size_t)_offboard_mission.count, + _navigator->get_geofence()); + } else { + _offboard_mission.count = 0; + _offboard_mission.current_index = 0; + } + return true; +} - return true; - } + +void +Mission::advance_mission() +{ + switch (_mission_type) { + case MISSION_TYPE_ONBOARD: + _onboard_mission.current_index++; + break; + + case MISSION_TYPE_OFFBOARD: + _offboard_mission.current_index++; + break; + + case MISSION_TYPE_NONE: + default: + break; } +} + +void +Mission::set_mission_items(struct position_setpoint_triplet_s *pos_sp_triplet) +{ + warnx("set mission items"); + + set_previous_pos_setpoint(&pos_sp_triplet->current, &pos_sp_triplet->previous); - /* happens when no more mission items can be added as a next item */ - _current_mission_type = MISSION_TYPE_NONE; + if (is_current_onboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting onboard mission item */ + _mission_type = MISSION_TYPE_ONBOARD; + } else if (is_current_offboard_mission_item_set(&pos_sp_triplet->current)) { + /* try setting offboard mission item */ + _mission_type = MISSION_TYPE_OFFBOARD; + } else { + _mission_type = MISSION_TYPE_NONE; + } +} + +void +Mission::set_previous_pos_setpoint(const struct position_setpoint_s *current_pos_sp, + struct position_setpoint_s *previous_pos_sp) +{ + /* reuse current setpoint as previous setpoint */ + if (current_pos_sp->valid) { + memcpy(previous_pos_sp, current_pos_sp, sizeof(struct position_setpoint_s)); + } +} + +bool +Mission::is_current_onboard_mission_item_set(struct position_setpoint_s *current_pos_sp) +{ + if (_param_onboard_enabled.get() > 0 + && _onboard_mission.current_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, true, &_onboard_mission.current_index, + &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; + + /* TODO: report this somehow */ + return true; + } + } return false; } bool -Mission::get_next_mission_item(struct mission_item_s *new_mission_item) +Mission::is_current_offboard_mission_item_set(struct position_setpoint_s *current_pos_sp) { - int next_temp_mission_index = _current_onboard_mission_index + 1; + if (_offboard_mission.current_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, true, &_offboard_mission.current_index, &new_mission_item)) { + /* convert the current mission item and set it valid */ + mission_item_to_position_setpoint(&_mission_item, current_pos_sp); + current_pos_sp->valid = true; - /* try onboard mission first */ - if (next_temp_mission_index < _onboard_mission_item_count && _onboard_mission_allowed) { - if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, new_mission_item)) { + report_current_offboard_mission_item(); return true; } } + return false; +} - /* then try offboard mission */ - dm_item_t dm_current; - if (_offboard_dataman_id == 0) { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; - } else { - dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; - } - next_temp_mission_index = _current_offboard_mission_index + 1; - if (next_temp_mission_index < _offboard_mission_item_count) { - if (read_mission_item(dm_current, false, &next_temp_mission_index, new_mission_item)) { - return true; +void +Mission::get_next_onboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + int next_temp_mission_index = _onboard_mission.current_index + 1; + + /* try if there is a next onboard mission */ + if (next_temp_mission_index < (int)_onboard_mission.count) { + struct mission_item_s new_mission_item; + if (read_mission_item(DM_KEY_WAYPOINTS_ONBOARD, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; } } - /* both failed, bail out */ - return false; + /* give up */ + next_pos_sp->valid = false; + return; +} + +void +Mission::get_next_offboard_mission_item(struct position_setpoint_s *next_pos_sp) +{ + /* try if there is a next offboard mission */ + int next_temp_mission_index = _offboard_mission.current_index + 1; + if (next_temp_mission_index < (int)_offboard_mission.count) { + dm_item_t dm_current; + if (_offboard_mission.dataman_id == 0) { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_0; + } else { + dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; + } + struct mission_item_s new_mission_item; + if (read_mission_item(dm_current, false, &next_temp_mission_index, &new_mission_item)) { + /* convert next mission item to position setpoint */ + mission_item_to_position_setpoint(&new_mission_item, next_pos_sp); + next_pos_sp->valid = true; + return; + } + } + /* give up */ + next_pos_sp->valid = false; + return; } bool -Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, struct mission_item_s *new_mission_item) +Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *mission_index, + struct mission_item_s *new_mission_item) { /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */ for (int i=0; i<10; i++) { @@ -211,25 +424,25 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio /* check for DO_JUMP item, and whether it hasn't not already been repeated enough times */ if (new_mission_item->nav_cmd == NAV_CMD_DO_JUMP) { - if (new_mission_item->do_jump_current_count < new_mission_item->do_jump_repeat_count) { + if (new_mission_item->do_jump_current_count >= new_mission_item->do_jump_repeat_count) { + return false; + } - /* only raise the repeat count if this is for the current mission item - * but not for the next mission item */ - if (is_current) { - (new_mission_item->do_jump_current_count)++; + /* only raise the repeat count if this is for the current mission item + * but not for the next mission item */ + if (is_current) { + (new_mission_item->do_jump_current_count)++; - /* save repeat count */ - if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, new_mission_item, len) != len) { - /* not supposed to happen unless the datamanager can't access the SD card, etc. */ - return false; - } + /* save repeat count */ + if (dm_write(dm_item, *mission_index, DM_PERSIST_IN_FLIGHT_RESET, + new_mission_item, len) != len) { + /* not supposed to happen unless the datamanager can't access the dataman */ + return false; } - /* set new mission item index and repeat - * we don't have to validate here, if it's invalid, we should realize this later .*/ - *mission_index = new_mission_item->do_jump_mission_index; - } else { - return false; } + /* set new mission item index and repeat + * we don't have to validate here, if it's invalid, we should realize this later .*/ + *mission_index = new_mission_item->do_jump_mission_index; } else { /* if it's not a DO_JUMP, then we were successful */ @@ -242,32 +455,12 @@ Mission::read_mission_item(const dm_item_t dm_item, bool is_current, int *missio return false; } -void -Mission::move_to_next() -{ - report_mission_item_reached(); - - switch (_current_mission_type) { - case MISSION_TYPE_ONBOARD: - _current_onboard_mission_index++; - break; - - case MISSION_TYPE_OFFBOARD: - _current_offboard_mission_index++; - break; - - case MISSION_TYPE_NONE: - default: - break; - } -} - void Mission::report_mission_item_reached() { - if (_current_mission_type == MISSION_TYPE_OFFBOARD) { + if (_mission_type == MISSION_TYPE_OFFBOARD) { _mission_result.mission_reached = true; - _mission_result.mission_index_reached = _current_offboard_mission_index; + _mission_result.mission_index_reached = _offboard_mission.current_index; } publish_mission_result(); } @@ -275,7 +468,7 @@ Mission::report_mission_item_reached() void Mission::report_current_offboard_mission_item() { - _mission_result.index_current_mission = _current_offboard_mission_index; + _mission_result.index_current_mission = _offboard_mission.current_index; publish_mission_result(); } @@ -294,4 +487,3 @@ Mission::publish_mission_result() /* reset reached bool */ _mission_result.mission_reached = false; } - -- cgit v1.2.3