diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 14:49:34 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-01-02 14:49:34 +0400 |
commit | 74e2542c07ffc154139ea12d722e5396b083f308 (patch) | |
tree | 52b7e5d32060cf9771f8dbc5c15dcf34faaded15 /src/modules/navigator/navigator_main.cpp | |
parent | 3c72c9bf7fce882a1d9138eb83e5d5745e79c271 (diff) | |
download | px4-firmware-74e2542c07ffc154139ea12d722e5396b083f308.tar.gz px4-firmware-74e2542c07ffc154139ea12d722e5396b083f308.tar.bz2 px4-firmware-74e2542c07ffc154139ea12d722e5396b083f308.zip |
navigator: takeoff and mission fixes
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 308 |
1 files changed, 155 insertions, 153 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 757eb4690..a6f33b5e7 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -170,7 +170,8 @@ private: bool _waypoint_position_reached; bool _waypoint_yaw_reached; uint64_t _time_first_inside_orbit; - bool _force_takeoff; + bool _need_takeoff; /**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */ + bool _do_takeoff; /**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */ MissionFeasibilityChecker missionFeasiblityChecker; @@ -178,16 +179,20 @@ private: struct { float min_altitude; + float acceptance_radius; float loiter_radius; int onboard_mission_enabled; + float takeoff_alt; float land_alt; float rtl_alt; } _parameters; /**< local copies of parameters */ struct { param_t min_altitude; + param_t acceptance_radius; param_t loiter_radius; param_t onboard_mission_enabled; + param_t takeoff_alt; param_t land_alt; param_t rtl_alt; } _parameter_handles; /**< handles for parameters */ @@ -291,7 +296,12 @@ private: /** * Check if current mission item has been reached. */ - bool mission_item_reached(); + bool check_mission_item_reached(); + + /** + * Perform actions when current mission item reached. + */ + void on_mission_item_reached(); /** * Move to next waypoint @@ -379,13 +389,16 @@ Navigator::Navigator() : _waypoint_yaw_reached(false), _time_first_inside_orbit(0), _set_nav_state_timestamp(0), - _force_takeoff(false) + _need_takeoff(true), + _do_takeoff(false) { memset(&_fence, 0, sizeof(_fence)); _parameter_handles.min_altitude = param_find("NAV_MIN_ALT"); + _parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD"); _parameter_handles.loiter_radius = param_find("NAV_LOITER_RAD"); _parameter_handles.onboard_mission_enabled = param_find("NAV_ONB_MIS_EN"); + _parameter_handles.takeoff_alt = param_find("NAV_TAKEOFF_ALT"); _parameter_handles.land_alt = param_find("NAV_LAND_ALT"); _parameter_handles.rtl_alt = param_find("NAV_RTL_ALT"); @@ -436,8 +449,10 @@ Navigator::parameters_update() orb_copy(ORB_ID(parameter_update), _params_sub, &update); param_get(_parameter_handles.min_altitude, &(_parameters.min_altitude)); + param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius)); param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius)); param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled)); + param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt)); param_get(_parameter_handles.land_alt, &(_parameters.land_alt)); param_get(_parameter_handles.rtl_alt, &(_parameters.rtl_alt)); @@ -726,27 +741,8 @@ Navigator::task_main() global_position_update(); /* only check if waypoint has been reached in MISSION */ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL) { - if (mission_item_reached()) { - if (myState == NAV_STATE_MISSION) { - 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()) { - advance_mission(); - } else { - dispatch(EVENT_MISSION_FINISHED); - } - } else { - /* RTL finished */ - dispatch(EVENT_MISSION_FINISHED); - } + if (check_mission_item_reached()) { + on_mission_item_reached(); } } } @@ -976,7 +972,7 @@ Navigator::start_none() _reset_loiter_pos = true; _rtl_done = false; - _force_takeoff = false; + _do_takeoff = false; publish_mission_item_triplet(); } @@ -984,7 +980,7 @@ Navigator::start_none() void Navigator::start_loiter() { - _force_takeoff = false; + _do_takeoff = false; /* set loiter position if needed */ if (_reset_loiter_pos || !_mission_item_triplet.current_valid) { @@ -997,7 +993,7 @@ Navigator::start_loiter() _mission_item_triplet.current.altitude_is_relative = false; float min_alt_amsl = _parameters.min_altitude + _home_pos.altitude; - /* Use current altitude if above min altitude set by parameter */ + /* use current altitude if above min altitude set by parameter */ if (_global_pos.alt < min_alt_amsl) { _mission_item_triplet.current.altitude = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); @@ -1019,72 +1015,10 @@ Navigator::start_loiter() 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; - - _mission_item_triplet.previous_valid = false; - - ret = _mission.get_current_mission_item(&_mission_item_triplet.current, &onboard, &index); - - if (ret == OK) { - _mission_item_triplet.current_valid = true; - add_home_pos_to_rtl(&_mission_item_triplet.current); - - 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; - - /* 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; + _need_takeoff = true; - /* 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); - - 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 { - /* 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(); + advance_mission(); } void @@ -1095,6 +1029,7 @@ Navigator::advance_mission() _mission_item_triplet.previous_valid = _mission_item_triplet.current_valid; _reset_loiter_pos = true; + _do_takeoff = false; int ret; bool onboard; @@ -1103,14 +1038,54 @@ Navigator::advance_mission() 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 (_vstatus.is_rotary_wing) { + if (_need_takeoff && ( + _mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF || + _mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_RETURN_TO_LAUNCH || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || + _mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED + )) { + /* do special TAKEOFF handling for VTOL */ + _need_takeoff = false; + + /* check if we really need takeoff */ + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + wp_alt_amsl += _home_pos.altitude; + if (_vstatus.condition_landed || _global_pos.alt - _home_pos.altitude < _parameters.takeoff_alt || + (_mission_item_triplet.next.nav_cmd != NAV_CMD_RETURN_TO_LAUNCH && _global_pos.alt > wp_alt_amsl)) { + /* force TAKEOFF if (landed or near ground) and waypoint altitude is more than current */ + _do_takeoff = true; + + /* 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; + + /* set current item to TAKEOFF */ + get_takeoff_item(&_mission_item_triplet.current); + + _mission_item_triplet.current.lat = (double)_global_pos.lat / 1e7d; + _mission_item_triplet.current.lon = (double)_global_pos.lon / 1e7d; + } + } else if (_mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* will need takeoff after landing */ + _need_takeoff = true; + } + } - if (onboard) { - mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + if (_do_takeoff) { + mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm", _mission_item_triplet.current.altitude); } else { - mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + if (onboard) { + mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index); + } else { + mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index); + } } } else { /* since a mission is not advanced without WPs available, this is not supposed to happen */ @@ -1118,16 +1093,16 @@ Navigator::advance_mission() warnx("ERROR: current WP can't be set"); } - ret = _mission.get_next_mission_item(&_mission_item_triplet.next); - - if (ret == OK) { - - add_home_pos_to_rtl(&_mission_item_triplet.next); + if (!_do_takeoff) { + ret = _mission.get_next_mission_item(&_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 (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; + } } publish_mission_item_triplet(); @@ -1149,6 +1124,7 @@ Navigator::start_rtl() { _reset_loiter_pos = true; _rtl_done = false; + _do_takeoff = false; /* discard all mission item and insert RTL item */ _mission_item_triplet.previous_valid = false; @@ -1157,12 +1133,13 @@ Navigator::start_rtl() _mission_item_triplet.current.lat = _home_pos.lat; _mission_item_triplet.current.lon = _home_pos.lon; - _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.min_altitude; + _mission_item_triplet.current.altitude = _home_pos.altitude + _parameters.rtl_alt; + _mission_item_triplet.current.altitude_is_relative = false; _mission_item_triplet.current.yaw = 0.0f; // TODO use heading to waypoint? _mission_item_triplet.current.nav_cmd = NAV_CMD_RETURN_TO_LAUNCH; _mission_item_triplet.current.loiter_direction = 1; _mission_item_triplet.current.loiter_radius = _parameters.loiter_radius; - _mission_item_triplet.current.radius = 50.0f; // TODO: get rid of magic number + _mission_item_triplet.current.acceptance_radius = _parameters.acceptance_radius; _mission_item_triplet.current.autocontinue = false; _mission_item_triplet.current_valid = true; @@ -1184,7 +1161,7 @@ Navigator::finish_rtl() } bool -Navigator::mission_item_reached() +Navigator::check_mission_item_reached() { /* only check if there is actually a mission item to check */ if (!_mission_item_triplet.current_valid) { @@ -1206,22 +1183,15 @@ Navigator::mission_item_reached() } uint64_t now = hrt_absolute_time(); - float orbit; + float acceptance_radius; - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.radius > 0.01f) { - - orbit = _mission_item_triplet.current.radius; + if (_mission_item_triplet.current.nav_cmd == NAV_CMD_WAYPOINT && _mission_item_triplet.current.acceptance_radius > 0.01f) { + acceptance_radius = _mission_item_triplet.current.acceptance_radius; } else { - - // XXX set default orbit via param - orbit = 15.0f; + acceptance_radius = _parameters.acceptance_radius; } - /* keep vertical orbit */ - float vertical_switch_distance = orbit; - - // TODO add frame // int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame; @@ -1232,10 +1202,12 @@ Navigator::mission_item_reached() // if (coordinate_frame == (int)MAV_FRAME_GLOBAL) { /* 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; + float wp_alt_amsl = _mission_item_triplet.current.altitude; + if (_mission_item_triplet.current.altitude_is_relative) + _mission_item_triplet.current.altitude += _home_pos.altitude; - dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude, - (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, current_alt, + dist = get_distance_to_point_global_wgs84(_mission_item_triplet.current.lat, _mission_item_triplet.current.lon, wp_alt_amsl, + (double)_global_pos.lat / 1e7d, (double)_global_pos.lon / 1e7d, _global_pos.alt, &dist_xy, &dist_z); // warnx("1 lat: %2.2f, lon: %2.2f, alt: %2.2f", _mission_item_triplet.current.lat, _mission_item_triplet.current.lon, _mission_item_triplet.current.altitude); @@ -1254,25 +1226,29 @@ Navigator::mission_item_reached() // // XXX TODO // } - if (_mission_item_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - /* require only altitude for takeoff */ - if (current_alt >= _mission_item_triplet.current.altitude) + if (_do_takeoff) { + if (_global_pos.alt > wp_alt_amsl - acceptance_radius) { + /* require only altitude for takeoff */ _waypoint_position_reached = true; + } } else { - if (dist >= 0.0f && dist_xy <= orbit && dist_z >= 0.0f && dist_z <= vertical_switch_distance) { + if (dist >= 0.0f && dist <= acceptance_radius) { _waypoint_position_reached = true; } } - /* check if required yaw reached */ - float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); - - if (fabsf(yaw_err) < 0.05f) { /* XXX get rid of magic number */ + if (_vstatus.is_rotary_wing && !_do_takeoff) { + /* check yow only for rotary wing except takeoff */ + float yaw_err = _wrap_pi(_mission_item_triplet.current.yaw - _global_pos.yaw); + if (fabsf(yaw_err) < 0.05f) { /* XXX 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 */) { /* XXX what about yaw? */ + if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { /* XXX announcement? */ @@ -1294,19 +1270,44 @@ Navigator::mission_item_reached() } void +Navigator::on_mission_item_reached() +{ + if (myState == NAV_STATE_MISSION) { + if (_do_takeoff) { + /* takeoff completed */ + _do_takeoff = false; + mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed"); + } else { + /* advance by one mission item */ + _mission.move_to_next(); + } + + if (_mission.current_mission_available()) { + advance_mission(); + } else { + /* if no more mission items available then finish mission */ + dispatch(EVENT_MISSION_FINISHED); + } + } else { + /* RTL finished */ + dispatch(EVENT_MISSION_FINISHED); + } +} + +void 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->yaw + item->loiter_radius = _parameters.loiter_radius; 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->acceptance_radius = _parameters.acceptance_radius; + item->time_inside = 0.0f; + item->pitch_min = 0.0f; item->autocontinue = false; item->origin = ORIGIN_ONBOARD; @@ -1320,17 +1321,31 @@ Navigator::get_takeoff_item(mission_item_s *item) //item->lon //item->altitude item->yaw = NAN; - item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number + item->loiter_radius = _parameters.loiter_radius; item->loiter_direction = 1; item->nav_cmd = NAV_CMD_TAKEOFF; - item->radius = 50.0f; // TODO: get rid of magic number + item->acceptance_radius = _parameters.acceptance_radius; item->time_inside = 0.0f; - item->pitch_min = 0.3; // TODO: get rid of magic number + item->pitch_min = 0.0; item->autocontinue = true; item->origin = ORIGIN_ONBOARD; } void +Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) +{ + if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { + /* append the home position to RTL item */ + new_mission_item->lat = _home_pos.lat; + new_mission_item->lon = _home_pos.lon; + new_mission_item->altitude = _home_pos.altitude + _parameters.rtl_alt; + new_mission_item->altitude_is_relative = false; + new_mission_item->loiter_radius = _parameters.loiter_radius; + new_mission_item->acceptance_radius = _parameters.acceptance_radius; + } +} + +void Navigator::publish_mission_item_triplet() { /* lazily publish the mission triplet only once available */ @@ -1424,7 +1439,7 @@ bool Navigator::cmp_mission_item_equivalent(const struct mission_item_s a, const fabsf(a.loiter_radius - b.loiter_radius) < FLT_EPSILON && a.loiter_direction == b.loiter_direction && a.nav_cmd == b.nav_cmd && - fabsf(a.radius - b.radius) < FLT_EPSILON && + fabsf(a.acceptance_radius - b.acceptance_radius) < FLT_EPSILON && fabsf(a.time_inside - b.time_inside) < FLT_EPSILON && a.autocontinue == b.autocontinue) { return true; @@ -1485,16 +1500,3 @@ int navigator_main(int argc, char *argv[]) return 0; } - -void -Navigator::add_home_pos_to_rtl(struct mission_item_s *new_mission_item) -{ - if (new_mission_item->nav_cmd == NAV_CMD_RETURN_TO_LAUNCH) { - /* if it is a RTL waypoint, append the home position */ - new_mission_item->lat = _home_pos.lat; - new_mission_item->lon = _home_pos.lon; - new_mission_item->altitude = _home_pos.altitude + _parameters.min_altitude; - new_mission_item->loiter_radius = _parameters.loiter_radius; // TODO: get rid of magic number - new_mission_item->radius = 50.0f; // TODO: get rid of magic number - } -} |