mtecs: make sure dt is calculated before any control calculations

1 files changed, 24 insertions, 7 deletions

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index fa9a6d947..3118ea5d1 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/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;
-	if (!_firstIterationAfterReset) {
-		hrt_abstime timestampNow = hrt_absolute_time();
-		deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
-		timestampLastIteration = timestampNow;
-	}
-	setDt(deltaTSeconds);
+	updateTimeMeasurement();
 
 	/* 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 */