From 3c72c9bf7fce882a1d9138eb83e5d5745e79c271 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 1 Jan 2014 23:21:29 +0400 Subject: navigator: force takeoff if first mission item is not takeoff and vehicle landed --- src/modules/commander/commander_params.c | 2 - src/modules/navigator/navigator_main.cpp | 154 ++++++++++++++++++++++++------- src/modules/navigator/navigator_params.c | 4 +- src/modules/uORB/topics/mission.h | 2 +- 4 files changed, 123 insertions(+), 39 deletions(-) (limited to 'src/modules') diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c index 40d0386d5..691d3efcb 100644 --- a/src/modules/commander/commander_params.c +++ b/src/modules/commander/commander_params.c @@ -45,8 +45,6 @@ #include #include -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f); -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f); PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 982ebefcc..757eb4690 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,6 +170,7 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; + bool _force_takeoff; MissionFeasibilityChecker missionFeasiblityChecker; @@ -179,13 +180,16 @@ private: float min_altitude; float loiter_radius; int onboard_mission_enabled; + float land_alt; + float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; param_t loiter_radius; param_t onboard_mission_enabled; - + param_t land_alt; + param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ enum Event { @@ -204,6 +208,14 @@ private: */ static StateTable::Tran const myTable[NAV_STATE_MAX][MAX_EVENT]; + enum RTLState { + RTL_STATE_CLIMB, + RTL_STATE_RETURN, + RTL_STATE_LAND + }; + + enum RTLState _rtl_state; + /** * Update our local parameter cache. */ @@ -289,7 +301,12 @@ private: /** * Helper function to get a loiter item */ - void get_loiter_item(mission_item_s *new_loiter_position); + void get_loiter_item(mission_item_s *item); + + /** + * Helper function to get a takeoff item + */ + void get_takeoff_item(mission_item_s *item); /** * Publish a new mission item triplet for position controller @@ -361,13 +378,16 @@ Navigator::Navigator() : _waypoint_position_reached(false), _waypoint_yaw_reached(false), _time_first_inside_orbit(0), - _set_nav_state_timestamp(0) + _set_nav_state_timestamp(0), + _force_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); + _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); _mission_item_triplet.previous_valid = false; _mission_item_triplet.current_valid = false; @@ -418,6 +438,8 @@ Navigator::parameters_update() param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); + param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); _mission.set_onboard_mission_allowed((bool)_parameter_handles.onboard_mission_enabled); } @@ -706,8 +728,14 @@ Navigator::task_main() if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { if (mission_item_reached()) { if (myState == NAV_STATE_MISSION) { - /* advance by one mission item */ - _mission.move_to_next(); + if (_force_takeoff) { + /* forced takeoff completed */ + _force_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] forced takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } /* if no more mission items available send this to state machine and start loiter at the last WP */ if (_mission.current_mission_available()) { @@ -948,6 +976,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; + _force_takeoff = false; publish_mission_item_triplet(); } @@ -955,6 +984,8 @@ Navigator::start_none() void Navigator::start_loiter() { + _force_takeoff = false; + /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { _reset_loiter_pos = false; @@ -985,51 +1016,77 @@ Navigator::start_loiter() publish_mission_item_triplet(); } - void Navigator::start_mission() { + int ret; + bool onboard; + unsigned index; + // TODO set prev item to current position? _reset_loiter_pos = true; _rtl_done = false; + _force_takeoff = false; - int ret; - bool onboard; - unsigned index; + _mission_item_triplet.previous_valid = false; ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.current); _mission_item_triplet.current_valid = true; + add_home_pos_to_rtl(&_mission_item_triplet.current); - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); - } else { - mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); - } - } else { - /* since a mission is started without knowledge if there are more mission items available, this can fail */ - _mission_item_triplet.current_valid = false; - } + if (_mission_item_triplet.current.nav_cmd != NAV_CMD_TAKEOFF && _vstatus.condition_landed) { + /* if landed, takeoff first, if this not defined in mission */ + _force_takeoff = true; - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); + /* move current mission item to next */ + memcpy(&_mission_item_triplet.next, &_mission_item_triplet.current, sizeof(mission_item_s)); + _mission_item_triplet.next_valid = true; - if (ret == OK) { + /* fill takeoff item */ + get_takeoff_item(&_mission_item_triplet.current); + if (_vstatus.is_rotary_wing) { + /* for VTOL use current position */ + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } else { + /* for fixed wing use first waypoint position */ + // TODO what if first mission item has no lat/lon? + _mission_item_triplet.current.lat = _mission_item_triplet.next.lat; + _mission_item_triplet.current.lon = _mission_item_triplet.next.lon; + } + _mission_item_triplet.current.altitude = _global_pos.alt + _parameters.land_alt; + _mission_item_triplet.current.altitude_is_relative = false; + mavlink_log_info(_mavlink_fd, "[navigator] mission started, force takeoff to %.1fm", _mission_item_triplet.current.altitude); + } else { + /* normal mission start: mission starts with takeoff item or vehicle is already in air */ + ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - add_home_pos_to_rtl(&_mission_item_triplet.next); - _mission_item_triplet.next_valid = true; + if (ret == OK) { + add_home_pos_to_rtl(&_mission_item_triplet.next); + _mission_item_triplet.next_valid = true; + } else { + /* this will fail for the last WP */ + _mission_item_triplet.next_valid = false; + } + + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] mission started, offboard WP %d", index); + } + } } else { - /* this will fail for the last WP */ + /* mission is started without knowledge if there are more mission, indicates bug in navigator */ + _mission_item_triplet.current_valid = false; _mission_item_triplet.next_valid = false; + mavlink_log_critical(_mavlink_fd, "[navigator] error: MISSION mode without mission defined"); } publish_mission_item_triplet(); } - - void Navigator::advance_mission() { @@ -1174,7 +1231,7 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { - // current relative or AMSL altitude depending on _mission_item_triplet.current.altitude_is_relative + /* current relative or AMSL altitude depending on mission item altitude_is_relative flag */ float current_alt = _mission_item_triplet.current.altitude_is_relative ? _global_pos.relative_alt : _global_pos.alt; dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, @@ -1199,7 +1256,7 @@ Navigator::mission_item_reached() if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { /* require only altitude for takeoff */ - if (current_alt > _mission_item_triplet.current.altitude) + if (current_alt >= _mission_item_triplet.current.altitude) _waypoint_position_reached = true; } else { if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { @@ -1237,13 +1294,40 @@ Navigator::mission_item_reached() } void -Navigator::get_loiter_item(struct mission_item_s *new_loiter_position) +Navigator::get_loiter_item(struct mission_item_s *item) +{ + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_LOITER_UNLIMITED; + item->radius = 50.0f; // TODO: get rid of magic number + //item->time_inside + //item->pitch_min + item->autocontinue = false; + item->origin = ORIGIN_ONBOARD; + +} + +void +Navigator::get_takeoff_item(mission_item_s *item) { - new_loiter_position->nav_cmd = NAV_CMD_LOITER_UNLIMITED; - new_loiter_position->loiter_direction = 1; - new_loiter_position->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_loiter_position->radius = 50.0f; // TODO: get rid of magic number - new_loiter_position->autocontinue = false; + //item->altitude_is_relative + //item->lat + //item->lon + //item->altitude + item->yaw = NAN; + item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_direction = 1; + item->nav_cmd = NAV_CMD_TAKEOFF; + item->radius = 50.0f; // TODO: get rid of magic number + item->time_inside = 0.0f; + item->pitch_min = 0.3; // TODO: get rid of magic number + item->autocontinue = true; + item->origin = ORIGIN_ONBOARD; } void diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index b9d887379..63601ce6d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,4 +53,6 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 100.0f); -PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); \ No newline at end of file +PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 10.0f); // default TAKEOFF altitude, slow descend from this altitude when landing in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 10.0f); // min altitude for going home in RTL mode diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h index 9b4250115..95bceb8bb 100644 --- a/src/modules/uORB/topics/mission.h +++ b/src/modules/uORB/topics/mission.h @@ -83,7 +83,7 @@ struct mission_item_s double lat; /**< latitude in degrees */ double lon; /**< longitude in degrees */ float altitude; /**< altitude in meters */ - float yaw; /**< in radians NED -PI..+PI */ + float yaw; /**< in radians NED -PI..+PI, NAN means don't change yaw */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< navigation command */ -- cgit v1.2.3