navigator: add position setpoint reset helper with safe defaults

- use in reset_triplets()

src/modules/navigator/navigator.h

@@ -222,12 +222,16 @@ public:
 	 */
 	void		reset_cruising_speed();
 
-
 	/**
 	 *  Set triplets to invalid
 	 */
 	void 		reset_triplets();
 
+	/**
+	 *  Set position setpoint to safe defaults
+	 */
+	void		reset_position_setpoint(position_setpoint_s &sp);
+
 	/**
 	 * Get the target throttle
 	 *

src/modules/navigator/navigator_main.cpp

@@ -296,7 +296,11 @@ Navigator::run()
 				}
 
 				rep->previous.valid = true;
+				rep->previous.timestamp = hrt_absolute_time();
+
 				rep->current.valid = true;
+				rep->current.timestamp = hrt_absolute_time();
+
 				rep->next.valid = false;
 
 				// CMD_DO_REPOSITION is acknowledged by commander
@@ -316,7 +320,9 @@ Navigator::run()
 
 				if (home_position_valid()) {
 					rep->current.yaw = cmd.param4;
+
 					rep->previous.valid = true;
+					rep->previous.timestamp = hrt_absolute_time();
 
 				} else {
 					rep->current.yaw = get_local_position()->yaw;
@@ -336,6 +342,8 @@ Navigator::run()
 				rep->current.alt = cmd.param7;
 
 				rep->current.valid = true;
+				rep->current.timestamp = hrt_absolute_time();
+
 				rep->next.valid = false;
 
 				// CMD_NAV_TAKEOFF is acknowledged by commander
@@ -669,6 +677,7 @@ Navigator::run()
 		      || (_vstatus.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION))) {
 
 			reset_triplets();
+
 			_pos_sp_triplet.current.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
 			_pos_sp_triplet.current.valid = true;
 			_pos_sp_triplet.current.timestamp = hrt_absolute_time();
@@ -830,14 +839,27 @@ Navigator::reset_cruising_speed()
 void
 Navigator::reset_triplets()
 {
-	_pos_sp_triplet.current.valid = false;
+	reset_position_setpoint(_pos_sp_triplet.previous);
-	_pos_sp_triplet.previous.valid = false;
+	reset_position_setpoint(_pos_sp_triplet.current);
-	_pos_sp_triplet.next.valid = false;
+	reset_position_setpoint(_pos_sp_triplet.next);
-	_pos_sp_triplet_updated = true;
-	_pos_sp_triplet.previous.acceptance_radius = get_default_acceptance_radius();
-	_pos_sp_triplet.current.acceptance_radius = get_default_acceptance_radius();
-	_pos_sp_triplet.next.acceptance_radius = get_default_acceptance_radius();
 
+	_pos_sp_triplet_updated = true;
+}
+
+void
+Navigator::reset_position_setpoint(position_setpoint_s &sp)
+{
+	sp = position_setpoint_s{};
+	sp.timestamp = hrt_absolute_time();
+	sp.lat = static_cast<double>(NAN);
+	sp.lon = static_cast<double>(NAN);;
+	sp.loiter_radius = get_loiter_radius();
+	sp.acceptance_radius = get_default_acceptance_radius();
+	sp.cruising_speed = get_cruising_speed();
+	sp.cruising_throttle = get_cruising_throttle();
+	sp.valid = false;
+	sp.type = position_setpoint_s::SETPOINT_TYPE_IDLE;
+	sp.disable_weather_vane = true;
 }
 
 float

src/modules/navigator/takeoff.cpp

@@ -128,6 +128,7 @@ Takeoff::set_takeoff_position()
 	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
 	mission_apply_limitation(_mission_item);
 	mission_item_to_position_setpoint(_mission_item, &pos_sp_triplet->current);
+
 	pos_sp_triplet->previous.valid = false;
 	pos_sp_triplet->current.yaw_valid = true;
 	pos_sp_triplet->next.valid = false;