navigator: fix triplet resetting/publication logic

julianoes · dagar · commit c17c54b61b4e · 2019-11-25T09:59:45.000-05:00
The navigator has a notion of resetting the triplets which means the
triplet setpoints are set to invalid and therefore not to be used by
downstream modules such as flight tasks.

However, before this patch, the triplets were not published if invalid
meaning that a valid triplet would stay the truth until a new valid
triplet would get published.

E.g. this lead to the corner case case where we publish a valid triplet
with an IDLE setpoint on ground in HOLD and then don't update the
triplet while flying in POSCTL mode. Later, when RTL is engaged, the
flight task will use IDLE until navigator (which runs slower) has
published the next triplet.

The fix involves two main changes:
- Publish invalid triplets to avoid stale triplets.
- Avoid publishing the triplet on every iteration in manual modes by not
  setting `_pos_sp_triplet_published_invalid_once = false`.

When testing this I realized that a mission upload during RTL would stop
RTL. This is because the mission is updated while the mission navigation
mode is not active and reset_triplets() is called from there. This is
now only done for the case where we are actually in mission navigation
mode. The fact that a mission is updated when not active also seems
wrong and is something to fix another time.
diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
@@ -191,12 +191,14 @@ Mission::on_active()
 	bool mission_sub_updated = _mission_sub.updated();
 
 	if (mission_sub_updated) {
+		_navigator->reset_triplets();
 		update_mission();
 	}
 
 	/* reset the current mission if needed */
 	if (need_to_reset_mission(true)) {
 		reset_mission(_mission);
+		_navigator->reset_triplets();
 		update_mission();
 		_navigator->reset_cruising_speed();
 		mission_sub_updated = true;
@@ -442,9 +444,6 @@ Mission::update_mission()
 
 	bool failed = true;
 
-	/* reset triplets */
-	_navigator->reset_triplets();
-
 	/* Reset vehicle_roi
 	 * Missions that do not explicitly configure ROI would not override
 	 * an existing ROI setting from previous missions */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
@@ -620,7 +620,6 @@ Navigator::run()
 		case vehicle_status_s::NAVIGATION_STATE_OFFBOARD:
 		case vehicle_status_s::NAVIGATION_STATE_STAB:
 		default:
-			_pos_sp_triplet_published_invalid_once = false;
 			navigation_mode_new = nullptr;
 			_can_loiter_at_sp = false;
 			break;
@@ -720,16 +719,8 @@ Navigator::print_status()
 void
 Navigator::publish_position_setpoint_triplet()
 {
-	// do not publish an invalid setpoint
-	if (!_pos_sp_triplet.current.valid) {
-		return;
-	}
-
 	_pos_sp_triplet.timestamp = hrt_absolute_time();
-
-	/* lazily publish the position setpoint triplet only once available */
 	_pos_sp_triplet_pub.publish(_pos_sp_triplet);
-
 	_pos_sp_triplet_updated = false;
 }
 
