aboutsummaryrefslogtreecommitdiff
path: root/src/modules/navigator
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/navigator')
-rw-r--r--src/modules/navigator/datalinkloss.cpp4
-rw-r--r--src/modules/navigator/mission.cpp28
-rw-r--r--src/modules/navigator/navigator.h1
-rw-r--r--src/modules/navigator/navigator_main.cpp26
-rw-r--r--src/modules/navigator/rcloss.cpp2
5 files changed, 40 insertions, 21 deletions
diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c73..e789fd10d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
/* Request flight termination from the commander */
_navigator->get_mission_result()->flight_termination = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
warnx("not switched to manual: request flight termination");
pos_sp_triplet->previous.valid = false;
pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
} else {
if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_FLYTOAIRFIELDHOMEWP:
_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case DLL_STATE_TERMINATE:
warnx("dll end");
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..0765e9b7c 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
} else {
/* no mission available or mission finished, switch to loiter */
if (_mission_type != MISSION_TYPE_NONE) {
- mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+ mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
/* use last setpoint for loiter */
_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,15 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
}
- if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
- /* mission item index out of bounds */
- return false;
- }
-
- /* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+ /* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+ * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
for (int i = 0; i < 10; i++) {
+
+ if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+ /* mission item index out of bounds */
+ return false;
+ }
+
const ssize_t len = sizeof(struct mission_item_s);
/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -626,11 +628,12 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
if (is_current) {
(mission_item_tmp.do_jump_current_count)++;
/* save repeat count */
- if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+ if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+ &mission_item_tmp, len) != len) {
/* not supposed to happen unless the datamanager can't access the
* dataman */
mavlink_log_critical(_navigator->get_mavlink_fd(),
- "ERROR DO JUMP waypoint could not be written");
+ "ERROR DO JUMP waypoint could not be written");
return false;
}
}
@@ -639,8 +642,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
} else {
- mavlink_log_critical(_navigator->get_mavlink_fd(),
- "DO JUMP repetitions completed");
+ if (is_current) {
+ mavlink_log_critical(_navigator->get_mavlink_fd(),
+ "DO JUMP repetitions completed");
+ }
/* no more DO_JUMPS, therefore just try to continue with next mission item */
(*mission_index_ptr)++;
}
@@ -707,6 +712,7 @@ Mission::set_mission_item_reached()
_navigator->get_mission_result()->reached = true;
_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
void
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..9cd609955 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
bool _can_loiter_at_sp; /**< flags if current position SP can be used to loiter */
bool _pos_sp_triplet_updated; /**< flags if position SP triplet needs to be published */
+ bool _pos_sp_triplet_published_invalid_once; /**< flags if position SP triplet has been published once to UORB */
control::BlockParamFloat _param_loiter_radius; /**< loiter radius for fixedwing */
control::BlockParamFloat _param_acceptance_radius; /**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0da..df620e5e7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
_gpsFailure(this, "GPSF"),
_can_loiter_at_sp(false),
_pos_sp_triplet_updated(false),
+ _pos_sp_triplet_published_invalid_once(false),
_param_loiter_radius(this, "LOITER_RAD"),
_param_acceptance_radius(this, "ACC_RAD"),
_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -289,8 +290,9 @@ Navigator::task_main()
navigation_capabilities_update();
params_update();
- /* rate limit position updates to 50 Hz */
+ /* rate limit position and sensor updates to 50 Hz */
orb_set_interval(_global_pos_sub, 20);
+ orb_set_interval(_sensor_combined_sub, 20);
hrt_abstime mavlink_open_time = 0;
const hrt_abstime mavlink_open_interval = 500000;
@@ -426,12 +428,15 @@ Navigator::task_main()
_can_loiter_at_sp = false;
break;
case NAVIGATION_STATE_AUTO_MISSION:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_mission;
break;
case NAVIGATION_STATE_AUTO_LOITER:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_loiter;
break;
case NAVIGATION_STATE_AUTO_RCRECOVER:
+ _pos_sp_triplet_published_invalid_once = false;
if (_param_rcloss_obc.get() != 0) {
_navigation_mode = &_rcLoss;
} else {
@@ -439,11 +444,13 @@ Navigator::task_main()
}
break;
case NAVIGATION_STATE_AUTO_RTL:
- _navigation_mode = &_rtl;
+ _pos_sp_triplet_published_invalid_once = false;
+ _navigation_mode = &_rtl;
break;
case NAVIGATION_STATE_AUTO_RTGS:
/* Use complex data link loss mode only when enabled via param
* otherwise use rtl */
+ _pos_sp_triplet_published_invalid_once = false;
if (_param_datalinkloss_obc.get() != 0) {
_navigation_mode = &_dataLinkLoss;
} else {
@@ -451,9 +458,11 @@ Navigator::task_main()
}
break;
case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_engineFailure;
break;
case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+ _pos_sp_triplet_published_invalid_once = false;
_navigation_mode = &_gpsFailure;
break;
default:
@@ -467,9 +476,9 @@ Navigator::task_main()
_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
}
- /* if nothing is running, set position setpoint triplet invalid */
- if (_navigation_mode == nullptr) {
- // TODO publish empty sp only once
+ /* if nothing is running, set position setpoint triplet invalid once */
+ if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+ _pos_sp_triplet_published_invalid_once = true;
_pos_sp_triplet.previous.valid = false;
_pos_sp_triplet.current.valid = false;
_pos_sp_triplet.next.valid = false;
@@ -497,8 +506,8 @@ Navigator::start()
/* start the task */
_navigator_task = task_spawn_cmd("navigator",
SCHED_DEFAULT,
- SCHED_PRIORITY_MAX - 5,
- 2000,
+ SCHED_PRIORITY_DEFAULT + 20,
+ 1800,
(main_t)&Navigator::task_main_trampoline,
nullptr);
@@ -630,9 +639,6 @@ Navigator::publish_mission_result()
/* advertise and publish */
_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
}
- /* reset reached bool */
- _mission_result.reached = false;
- _mission_result.finished = false;
}
void
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42..42392e739 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
_rcl_state = RCL_STATE_TERMINATE;
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
}
break;
case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
_navigator->get_mission_result()->stay_in_failsafe = true;
_navigator->publish_mission_result();
+ reset_mission_item_reached();
break;
case RCL_STATE_TERMINATE:
warnx("rcl end");
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-20 10:45:03 +0000