aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator/navigator_main.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator/navigator_main.cpp')
-rw-r--r--src/modules/navigator/navigator_main.cpp120
1 files changed, 47 insertions, 73 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index bcb3c8d0a..12fd35a0a 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -177,7 +177,7 @@ private:
class Mission _mission;
bool _mission_item_valid; /**< current mission item valid */
- bool _global_pos_valid; /**< track changes of global_position.global_valid flag */
+ bool _global_pos_valid; /**< track changes of global_position */
bool _reset_loiter_pos; /**< if true then loiter position should be set to current position */
bool _waypoint_position_reached;
bool _waypoint_yaw_reached;
@@ -690,84 +690,56 @@ Navigator::task_main()
if (fds[6].revents & POLLIN) {
vehicle_status_update();
- /* evaluate state machine from commander and set the navigator mode accordingly */
+ /* evaluate state requested by commander */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
- bool stick_mode = false;
-
- if (!_vstatus.rc_signal_lost) {
- /* RC signal available, use control switches to set mode */
- /* RETURN switch, overrides MISSION switch */
- if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
- /* switch to RTL if not already landed after RTL and home position set */
- if (!(_rtl_state == RTL_STATE_DESCEND &&
- (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
- _vstatus.condition_home_position_valid) {
- dispatch(EVENT_RTL_REQUESTED);
- }
+ /* publish position setpoint triplet on each status update if navigator active */
+ _pos_sp_triplet_updated = true;
- stick_mode = true;
+ if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
+ /* commander requested new navigation mode, try to set it */
+ switch (_vstatus.set_nav_state) {
+ case NAV_STATE_NONE:
+ /* nothing to do */
+ break;
- } else {
- /* MISSION switch */
- if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) {
- request_loiter_or_ready();
- stick_mode = true;
+ case NAV_STATE_LOITER:
+ request_loiter_or_ready();
+ break;
- } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) {
- request_mission_if_available();
- stick_mode = true;
- }
+ case NAV_STATE_MISSION:
+ request_mission_if_available();
+ break;
- 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 */
- request_mission_if_available();
- stick_mode = true;
+ case NAV_STATE_RTL:
+ if (!(_rtl_state == RTL_STATE_DESCEND &&
+ (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+ _vstatus.condition_home_position_valid) {
+ dispatch(EVENT_RTL_REQUESTED);
}
- }
- }
-
- if (!stick_mode) {
- if (_vstatus.set_nav_state_timestamp != _set_nav_state_timestamp) {
- /* commander requested new navigation mode, try to set it */
- _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
-
- switch (_vstatus.set_nav_state) {
- case NAV_STATE_NONE:
- /* nothing to do */
- break;
-
- case NAV_STATE_LOITER:
- request_loiter_or_ready();
- break;
- case NAV_STATE_MISSION:
- request_mission_if_available();
- break;
+ break;
- case NAV_STATE_RTL:
- if (!(_rtl_state == RTL_STATE_DESCEND &&
- (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
- _vstatus.condition_home_position_valid) {
- dispatch(EVENT_RTL_REQUESTED);
- }
+ case NAV_STATE_LAND:
+ dispatch(EVENT_LAND_REQUESTED);
- break;
+ break;
- case NAV_STATE_LAND:
- dispatch(EVENT_LAND_REQUESTED);
-
- break;
+ default:
+ warnx("ERROR: Requested navigation state not supported");
+ break;
+ }
- default:
- warnx("ERROR: Requested navigation state not supported");
- break;
- }
+ } else {
+ /* on first switch to AUTO try mission by default, if none is available fallback to loiter */
+ if (myState == NAV_STATE_NONE) {
+ request_mission_if_available();
+ }
+ }
- } else {
- /* on first switch to AUTO try mission by default, if none is available fallback to loiter */
- if (myState == NAV_STATE_NONE) {
- request_mission_if_available();
- }
+ /* check if waypoint has been reached in MISSION, RTL and LAND modes */
+ if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
+ if (check_mission_item_reached()) {
+ on_mission_item_reached();
}
}
@@ -775,6 +747,8 @@ Navigator::task_main()
/* navigator shouldn't act */
dispatch(EVENT_NONE_REQUESTED);
}
+
+ _set_nav_state_timestamp = _vstatus.set_nav_state_timestamp;
}
/* parameters updated */
@@ -813,17 +787,15 @@ Navigator::task_main()
if (fds[1].revents & POLLIN) {
global_position_update();
- /* publish position setpoint triplet on each position update if navigator active */
if (_control_mode.flag_armed && _control_mode.flag_control_auto_enabled) {
+ /* publish position setpoint triplet on each position update if navigator active */
_pos_sp_triplet_updated = true;
- if (myState == NAV_STATE_LAND && _global_pos.global_valid && !_global_pos_valid) {
+ if (myState == NAV_STATE_LAND && !_global_pos_valid) {
/* got global position when landing, update setpoint */
start_land();
}
- _global_pos_valid = _global_pos.global_valid;
-
/* check if waypoint has been reached in MISSION, RTL and LAND modes */
if (myState == NAV_STATE_MISSION || myState == NAV_STATE_RTL || myState == NAV_STATE_LAND) {
if (check_mission_item_reached()) {
@@ -848,6 +820,8 @@ Navigator::task_main()
}
}
+ _global_pos_valid = _vstatus.condition_global_position_valid;
+
/* publish position setpoint triplet if updated */
if (_pos_sp_triplet_updated) {
_pos_sp_triplet_updated = false;
@@ -893,9 +867,9 @@ Navigator::start()
void
Navigator::status()
{
- warnx("Global position is %svalid", _global_pos.global_valid ? "" : "in");
+ warnx("Global position: %svalid", _global_pos_valid ? "" : "in");
- if (_global_pos.global_valid) {
+ if (_global_pos_valid) {
warnx("Longitude %5.5f degrees, latitude %5.5f degrees", _global_pos.lon, _global_pos.lat);
warnx("Altitude %5.5f meters, altitude above home %5.5f meters",
(double)_global_pos.alt, (double)(_global_pos.alt - _home_pos.alt));
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-07 16:27:29 +0000