AutoSmoothVel - Override checkTakeoff with task-specific logic and reactivate z axis with downward velocity to takeoff smoothly

bresch · LorenzMeier · commit bb055fdaf3d1 · 2019-05-22T22:05:38.000+02:00
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.cpp
@@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate()
 
 void FlightTaskAutoLineSmoothVel::reActivate()
 {
-	// Don't reset during takeoff TODO: Find a proper solution
-	// The issue here is that with a small increment of velocity setpoint (generated by this flight task), the
-	// land detector doesn't detect takeoff and without takeoff detection, the
-	// flight task is always reset.
+	// On ground, reset acceleration and velocity to zero
+	for (int i = 0; i < 2; ++i) {
+		_trajectory[i].reset(0.f, 0.f, _position(i));
+	}
+
+	_trajectory[2].reset(0.f, 0.7f, _position(2));
 }
 
 void FlightTaskAutoLineSmoothVel::_generateSetpoints()
@@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 	// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.
 
 	_checkEkfResetCounters();
+	_want_takeoff = false;
 
 	if (PX4_ISFINITE(_position_setpoint(0)) &&
 	    PX4_ISFINITE(_position_setpoint(1))) {
@@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()
 			_velocity_setpoint(2) = vel_sp_z;
 		}
 
+		_want_takeoff = _velocity_setpoint(2) < -0.3f;
 	}
 }
 
diff --git a/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp b/src/lib/FlightTasks/tasks/AutoLineSmoothVel/FlightTaskAutoLineSmoothVel.hpp
@@ -67,6 +67,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 
 	void _generateSetpoints() override; /**< Generate setpoints along line. */
 
+	/** determines when to trigger a takeoff (ignored in flight) */
+	bool _checkTakeoff() override { return _want_takeoff; };
+
 	inline float _constrainOneSide(float val, float constrain);
 	void _checkEkfResetCounters(); /**< Reset the trajectories when the ekf resets velocity or position */
 	void _generateHeading();
@@ -75,7 +78,9 @@ class FlightTaskAutoLineSmoothVel : public FlightTaskAutoMapper2
 	void _prepareSetpoints(); /**< Generate velocity target points for the trajectory generator. */
 	void _generateTrajectory();
 	VelocitySmoothing _trajectory[3]; ///< Trajectories in x, y and z directions
-	float _yaw_sp_prev;
+
+	float _yaw_sp_prev{NAN};
+	bool _want_takeoff{false};
 
 	/* counters for estimator local position resets */
 	struct {

Original file line number	Diff line number	Diff line change
`@@ -57,10 +57,12 @@ bool FlightTaskAutoLineSmoothVel::activate()`
`57`	`57`
`58`	`58`	`void FlightTaskAutoLineSmoothVel::reActivate()`
`59`	`59`	`{`
`60`		`- // Don't reset during takeoff TODO: Find a proper solution`
`61`		`- // The issue here is that with a small increment of velocity setpoint (generated by this flight task), the`
`62`		`- // land detector doesn't detect takeoff and without takeoff detection, the`
`63`		`- // flight task is always reset.`
	`60`	`+ // On ground, reset acceleration and velocity to zero`
	`61`	`+ for (int i = 0; i < 2; ++i) {`
	`62`	`+ _trajectory[i].reset(0.f, 0.f, _position(i));`
	`63`	`+ }`
	`64`	`+`
	`65`	`+ _trajectory[2].reset(0.f, 0.7f, _position(2));`
`64`	`66`	`}`
`65`	`67`
`66`	`68`	`void FlightTaskAutoLineSmoothVel::_generateSetpoints()`
`@@ -143,6 +145,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()`
`143`	`145`	`// If the position setpoints are set to NAN, the values in the velocity setpoints are used as velocity targets: nothing to do here.`
`144`	`146`
`145`	`147`	`_checkEkfResetCounters();`
	`148`	`+ _want_takeoff = false;`
`146`	`149`
`147`	`150`	`if (PX4_ISFINITE(_position_setpoint(0)) &&`
`148`	`151`	`PX4_ISFINITE(_position_setpoint(1))) {`
`@@ -191,6 +194,7 @@ void FlightTaskAutoLineSmoothVel::_prepareSetpoints()`
`191`	`194`	`_velocity_setpoint(2) = vel_sp_z;`
`192`	`195`	`}`
`193`	`196`
	`197`	`+ _want_takeoff = _velocity_setpoint(2) < -0.3f;`
`194`	`198`	`}`
`195`	`199`	`}`
`196`	`200`