diff options
author | Julian Oes <julian@oes.ch> | 2014-02-19 14:59:46 +0100 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-02-19 14:59:46 +0100 |
commit | b17cdb12b013a99a924b02ccef718c0ea0f776aa (patch) | |
tree | fe65b479ed853a00a3f05a8ffd11ad288174c6dc /src/modules/navigator | |
parent | 366f19c2c07f249a6ac51f16f08b86454e2e2da4 (diff) | |
parent | 84d9e3479f3d2bb29b22906be0dfe65d73b5dd0f (diff) | |
download | px4-firmware-b17cdb12b013a99a924b02ccef718c0ea0f776aa.tar.gz px4-firmware-b17cdb12b013a99a924b02ccef718c0ea0f776aa.tar.bz2 px4-firmware-b17cdb12b013a99a924b02ccef718c0ea0f776aa.zip |
Merge branch 'beta' into beta_mavlink
Diffstat (limited to 'src/modules/navigator')
-rw-r--r-- | src/modules/navigator/navigator_main.cpp | 68 | ||||
-rw-r--r-- | src/modules/navigator/navigator_params.c | 47 |
2 files changed, 69 insertions, 46 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index e1dde35bf..930eae6a3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -305,6 +305,12 @@ private: void start_land_home(); /** + * Fork for state transitions + */ + void request_loiter_or_ready(); + void request_mission_if_available(); + + /** * Guards offboard mission */ bool offboard_mission_available(unsigned relative_index); @@ -699,24 +705,17 @@ Navigator::task_main() } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); + request_mission_if_available(); stick_mode = true; } } @@ -733,17 +732,11 @@ Navigator::task_main() break; case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); break; case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); break; case NAV_STATE_RTL: @@ -770,12 +763,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_mission_if_available(); } } } @@ -1071,7 +1059,7 @@ Navigator::start_loiter() float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { + if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); @@ -1401,6 +1389,28 @@ Navigator::set_rtl_item() } void +Navigator::request_loiter_or_ready() +{ + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } +} + +void +Navigator::request_mission_if_available() +{ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + request_loiter_or_ready(); + } +} + +void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { sp->valid = true; @@ -1561,13 +1571,7 @@ Navigator::on_mission_item_reached() /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - if (_vstatus.condition_landed) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ec7a4e229..9ef359c6d 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,68 +53,87 @@ */ /** - * Minimum altitude + * Minimum altitude (fixed wing only) * + * Minimum altitude above home for LOITER. + * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); /** - * Waypoint acceptance radius. + * Waypoint acceptance radius + * + * Default value of acceptance radius (if not specified in mission item). * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); /** - * Loiter radius. + * Loiter radius (fixed wing only) + * + * Default value of loiter radius (if not specified in mission item). * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** + * Enable onboard mission + * * @group Navigation */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); /** - * Default take-off altitude. + * Take-off altitude + * + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. * + * @unit meters * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); /** - * Landing altitude. + * Landing altitude * - * Slowly descend from this altitude when landing. + * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. * + * @unit meters * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); /** - * Return-to-land altitude. + * Return-To-Launch altitude * - * Minimum altitude for going home in RTL mode. + * Minimum altitude above home position for going home in RTL mode. * + * @unit meters * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); /** - * Return-to-land delay. + * Return-To-Launch delay * - * Delay after descend before landing. + * Delay after descend before landing in RTL mode. * If set to -1 the system will not land but loiter at NAV_LAND_ALT. * + * @unit seconds * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); /** - * Enable parachute deployment. + * Enable parachute deployment * * @group Navigation */ |