FlightTasks: pass the timestamp of the message that causes FlightTask-switch

src/lib/flight_tasks/FlightTasks.cpp

@@ -56,7 +56,7 @@ const landing_gear_s FlightTasks::getGear()
 	}
 }
 
-FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
+FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index, const hrt_abstime& time_stamp_mode_switch)
 {
 	// switch to the running task, nothing to do
 	if (new_task_index == _current_task.index) {
@@ -76,6 +76,9 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 		return FlightTaskError::NoError;
 	}
 
+	// pass modeswitch timestamp
+	_current_task.task->setModeSwitchTimeStamp(time_stamp_mode_switch);
+
 	// activation failed
 	if (!_current_task.task->updateInitialize() || !_current_task.task->activate(last_setpoint)) {
 		_current_task.task->~FlightTask();
@@ -87,7 +90,7 @@ FlightTaskError FlightTasks::switchTask(FlightTaskIndex new_task_index)
 	return FlightTaskError::NoError;
 }
 
-FlightTaskError FlightTasks::switchTask(int new_task_index)
+FlightTaskError FlightTasks::switchTask(int new_task_index, const hrt_abstime& time_stamp_mode_switch)
 {
 	// make sure we are in range of the enumeration before casting
 	if (static_cast<int>(FlightTaskIndex::None) <= new_task_index &&
@@ -146,7 +149,7 @@ void FlightTasks::_updateCommand()
 	}
 
 	// switch to the commanded task
-	FlightTaskError switch_result = switchTask(desired_task);
+	FlightTaskError switch_result = switchTask(desired_task, command.timestamp);
 	uint8_t cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 
 	// if we are in/switched to the desired task
@@ -159,7 +162,7 @@ void FlightTasks::_updateCommand()
 
 			// if we just switched and parameters are not accepted, go to failsafe
 			if (switch_result >= FlightTaskError::NoError) {
-				switchTask(FlightTaskIndex::ManualPosition);
+				switchTask(FlightTaskIndex::ManualPosition, command.timestamp);
 				cmd_result = vehicle_command_ack_s::VEHICLE_RESULT_FAILED;
 			}
 		}

src/lib/flight_tasks/FlightTasks.hpp

@@ -48,6 +48,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/vehicle_command_ack.h>
 #include <uORB/topics/vehicle_command.h>
+#include <drivers/drv_hrt.h>
 
 #include <new>
 
@@ -116,8 +117,8 @@ class FlightTasks
 	 * @param task index to switch to
 	 * @return 0 on success, <0 on error
 	 */
-	FlightTaskError switchTask(FlightTaskIndex new_task_index);
-	FlightTaskError switchTask(int new_task_index);
+	FlightTaskError switchTask(FlightTaskIndex new_task_index, const hrt_abstime& time_stamp_mode_switch = 0);
+	FlightTaskError switchTask(int new_task_index, const hrt_abstime& time_stamp_mode_switch = 0);
 
 	/**
 	 * Get the number of the active task

src/lib/flight_tasks/tasks/Auto/FlightTaskAuto.cpp

@@ -136,11 +136,20 @@ bool FlightTaskAuto::_evaluateTriplets()
 
 	// Check if triplet is valid. There must be at least a valid altitude.
 
-	if (!_sub_triplet_setpoint.get().current.valid || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
-		// Best we can do is to just set all waypoints to current state and return false.
+	// Check if triplet is valid.
+	// * a triplet has been received
+	// * triplet is valid
+	// * at least altitude is valid
+	if ((_time_stamp_mode_switch_requested >= _sub_triplet_setpoint.get().timestamp)
+	    || !_sub_triplet_setpoint.get().current.valid
+	    || !PX4_ISFINITE(_sub_triplet_setpoint.get().current.alt)) {
+		// hover at current location
 		_prev_prev_wp = _triplet_prev_wp = _triplet_target = _triplet_next_wp = _position;
+		_yaw_setpoint = _yaw;
+		_yawspeed_setpoint = NAN;
+		_velocity_setpoint.zero();
 		_type = WaypointType::position;
-		return false;
+		return true;
 	}
 
 	_type = (WaypointType)_sub_triplet_setpoint.get().current.type;

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

@@ -16,6 +16,12 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
 	_initEkfResetCounters();
 	_time_stamp_activate = hrt_absolute_time();
 	_gear = empty_landing_gear_default_keep;
+
+	if (!_time_stamp_mode_switch_requested) {
+		// Modeswitch for timestamp has not been set. Set it equal to activation timestamp.
+		_time_stamp_mode_switch_requested = _time_stamp_activate;
+	}
+
 	return true;
 }
 

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -157,6 +157,11 @@ class FlightTask : public ModuleParams
 		updateParams();
 	}
 
+	/**
+	 *  Set the timestamp of the message that caused a FlightTasks-switch
+	 */
+	void setModeSwitchTimeStamp(const hrt_abstime& t_stamp_mode_switch) { _time_stamp_mode_switch_requested = t_stamp_mode_switch; }
+
 	/**
 	 * Sets an external yaw handler which can be used by any flight task to implement a different yaw control strategy.
 	 * This method does nothing, each flighttask which wants to use the yaw handler needs to override this method.
@@ -202,6 +207,7 @@ class FlightTask : public ModuleParams
 	hrt_abstime _time_stamp_activate = 0; /**< time stamp when task was activated */
 	hrt_abstime _time_stamp_current = 0; /**< time stamp at the beginning of the current task update */
 	hrt_abstime _time_stamp_last = 0; /**< time stamp when task was last updated */
+	hrt_abstime _time_stamp_mode_switch_requested = 0; /**< time stamp when a mode switch was requested. */
 
 	/* Current vehicle state */
 	matrix::Vector3f _position; /**< current vehicle position */

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -795,7 +795,7 @@ MulticopterPositionControl::start_flight_task()
 	// Auto-follow me
 	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET) {
 		should_disable_task = false;
-		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe);
+		FlightTaskError error = _flight_tasks.switchTask(FlightTaskIndex::AutoFollowMe, _vehicle_status.nav_state_timestamp);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
@@ -809,19 +809,35 @@ MulticopterPositionControl::start_flight_task()
 			// we want to be in this mode, reset the failure count
 			_task_failure_count = 0;
 		}
+	}
 
-	} else if (_control_mode.flag_control_auto_enabled) {
+	// Auto
+	const bool vehicle_in_auto_mode = (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LAND
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDENGFAIL
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTL
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RTGS
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_RCRECOVER
+					   || _vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_AUTO_PRECLAND);
+
+
+	if (vehicle_in_auto_mode) {
 		// Auto related maneuvers
 		should_disable_task = false;
 		FlightTaskError error = FlightTaskError::NoError;
 
 		switch (_param_mpc_auto_mode.get()) {
 		case 1:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel);
+			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLineSmoothVel, _vehicle_status.nav_state_timestamp);
 			break;
 
 		default:
-			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLine);
+			error =  _flight_tasks.switchTask(FlightTaskIndex::AutoLine, _vehicle_status.nav_state_timestamp);
 			break;
 		}
 
@@ -837,14 +853,16 @@ MulticopterPositionControl::start_flight_task()
 			// we want to be in this mode, reset the failure count
 			_task_failure_count = 0;
 		}
+	}
 
-	} else if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
+	// Descend without gps
+	if (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_DESCEND) {
 
 		// Emergency descend
 		should_disable_task = false;
 		FlightTaskError error = FlightTaskError::NoError;
 
-		error =  _flight_tasks.switchTask(FlightTaskIndex::Descend);
+		error =  _flight_tasks.switchTask(FlightTaskIndex::Descend, _vehicle_status.nav_state_timestamp);
 
 		if (error != FlightTaskError::NoError) {
 			if (prev_failure_count == 0) {
@@ -858,7 +876,6 @@ MulticopterPositionControl::start_flight_task()
 			// we want to be in this mode, reset the failure count
 			_task_failure_count = 0;
 		}
-
 	}
 
 	// manual position control