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.cpp130
1 files changed, 52 insertions, 78 deletions
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 5a94b6671..1a45e1345 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;
@@ -510,7 +510,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
{
struct mission_s offboard_mission;
- if (orb_copy(ORB_ID(mission), _offboard_mission_sub, &offboard_mission) == OK) {
+ 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 */
@@ -543,7 +543,7 @@ Navigator::onboard_mission_update()
{
struct mission_s onboard_mission;
- if (orb_copy(ORB_ID(mission), _onboard_mission_sub, &onboard_mission) == OK) {
+ if (orb_copy(ORB_ID(onboard_mission), _onboard_mission_sub, &onboard_mission) == OK) {
_mission.set_onboard_mission_count(onboard_mission.count);
_mission.set_current_onboard_mission_index(onboard_mission.current_index);
@@ -611,7 +611,7 @@ Navigator::task_main()
* do subscriptions
*/
_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
- _offboard_mission_sub = orb_subscribe(ORB_ID(mission));
+ _offboard_mission_sub = orb_subscribe(ORB_ID(offboard_mission));
_onboard_mission_sub = orb_subscribe(ORB_ID(onboard_mission));
_capabilities_sub = orb_subscribe(ORB_ID(navigation_capabilities));
_vstatus_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -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;
- 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);
- }
+ break;
- break;
+ case NAV_STATE_LAND:
+ dispatch(EVENT_LAND_REQUESTED);
- case NAV_STATE_LAND:
- dispatch(EVENT_LAND_REQUESTED);
+ break;
- 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));
@@ -1317,7 +1291,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
- _mission_item.nav_cmd = NAV_CMD_TAKEOFF;
+ _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = 0.0f;
_mission_item.pitch_min = 0.0f;
@@ -1377,7 +1351,7 @@ Navigator::set_rtl_item()
_mission_item.yaw = NAN;
_mission_item.loiter_radius = _parameters.loiter_radius;
_mission_item.loiter_direction = 1;
- _mission_item.nav_cmd = NAV_CMD_WAYPOINT;
+ _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
_mission_item.acceptance_radius = _parameters.acceptance_radius;
_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
_mission_item.pitch_min = 0.0f;
generated by cgit v1.2.3 (git 2.39.1) at 2024-09-13 08:09:06 +0000