diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-27 22:12:01 +0100 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-03-27 22:12:01 +0100 |
commit | d102afba8bdb526b85dce6dc47034ddd170d7240 (patch) | |
tree | f527b6de285b856aa8dbf478d323662ea95b6e14 /src/modules/fw_pos_control_l1 | |
parent | 0d526bddca9210bed75cfd26458a48ac87453c9e (diff) | |
download | px4-firmware-d102afba8bdb526b85dce6dc47034ddd170d7240.tar.gz px4-firmware-d102afba8bdb526b85dce6dc47034ddd170d7240.tar.bz2 px4-firmware-d102afba8bdb526b85dce6dc47034ddd170d7240.zip |
mtecs: make sure dt is calculated before any control calculations
Diffstat (limited to 'src/modules/fw_pos_control_l1')
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 31 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/mtecs/mTecs.h | 8 |
2 files changed, 31 insertions, 8 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 */ diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 502a8a8b0..62c4d7014 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/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 */ |