mtecs: make sure dt is calculated before any control calculations

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -60,6 +60,7 @@ mTecs::mTecs() :
 	_pitchSp(0.0f),
 	timestampLastIteration(hrt_absolute_time()),
 	_firstIterationAfterReset(true),
+	dtCalculated(false),
 	_counter(0)
 {
 }
@@ -71,6 +72,9 @@ mTecs::~mTecs()
 void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp)
 {
 
+	/* time measurement */
+	updateTimeMeasurement();
+
 	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
 	if (_counter % 10 == 0) {
 		warnx("***");
@@ -81,6 +85,9 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt
 
 void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) {
 
+	/* time measurement */
+	updateTimeMeasurement();
+
 	float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed);
 	if (_counter % 10 == 0) {
 		warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
@@ -91,13 +98,7 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn
 void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp)
 {
 	/* time measurement */
-	float deltaTSeconds = 0.0f;
+	updateTimeMeasurement();
-	if (!_firstIterationAfterReset) {
-		hrt_abstime timestampNow = hrt_absolute_time();
-		deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
-		timestampLastIteration = timestampNow;
-	}
-	setDt(deltaTSeconds);
 
 	/* update parameters first */
 	updateParams();
@@ -147,6 +148,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
 
 	/* clean up */
 	_firstIterationAfterReset = false;
+	dtCalculated = false;
 
 	_counter++;
 }
@@ -159,5 +161,20 @@ void mTecs::resetIntegrators()
 	_firstIterationAfterReset = true;
 }
 
+void mTecs::updateTimeMeasurement()
+{
+	if (!dtCalculated) {
+		float deltaTSeconds = 0.0f;
+		if (!_firstIterationAfterReset) {
+			hrt_abstime timestampNow = hrt_absolute_time();
+			deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
+			timestampLastIteration = timestampNow;
+		}
+		setDt(deltaTSeconds);
+
+		dtCalculated = true;
+	}
+}
+
 } /* namespace fwPosctrl */
 

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -104,9 +104,15 @@ protected:
 	/* Time measurements */
 	hrt_abstime timestampLastIteration;		/**< Saves the result of hrt_absolute_time() of the last iteration */
 
-	bool _firstIterationAfterReset;		/**< True during the first iteration after a reset */
+	bool _firstIterationAfterReset;			/**< True during the first iteration after a reset */
+	bool dtCalculated;				/**< True if dt has been calculated in this iteration */
 
 	int _counter;
+
+	/*
+	 * Measure and update the time step dt if this was not already done in the current iteration
+	 */
+	void updateTimeMeasurement();
 };
 
 } /* namespace fwPosctrl */