From b61e6f2706fed68257c909bdd8a84feda5121344 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:23:48 +0100 Subject: [PATCH 1/4] mtecs: don't flow the stdout too much --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 35 ++++++++++++------- src/modules/fw_pos_control_l1/mtecs/mTecs.h | 1 + 2 files changed, 24 insertions(+), 12 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index ad108e7eec..fa9a6d9475 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -59,7 +59,8 @@ mTecs::mTecs() : _throttleSp(0.0f), _pitchSp(0.0f), timestampLastIteration(hrt_absolute_time()), - _firstIterationAfterReset(true) + _firstIterationAfterReset(true), + _counter(0) { } @@ -69,16 +70,21 @@ mTecs::~mTecs() void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp) { - warnx("***"); + float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); - warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + if (_counter % 10 == 0) { + warnx("***"); + warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + } updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp); } void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) { float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); - warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + if (_counter % 10 == 0) { + warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + } updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp); } @@ -117,10 +123,12 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateSp = flightPathAngleSp - airspeedSpDerivativeNorm; float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; - warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", - (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); - warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", - (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + if (_counter % 10 == 0) { + warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + (double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2, (double)airspeedDerivativeNorm); + warnx("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", + (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); + } /** update control blocks **/ /* update total energy rate control block */ @@ -129,15 +137,18 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh /* update energy distribution rate control block */ _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError); - warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", - (double)_throttleSp, (double)_pitchSp, - (double)flightPathAngleSp, (double)flightPathAngle, - (double)accelerationLongitudinalSp, (double)airspeedDerivative); + if (_counter % 10 == 0) { + warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + (double)_throttleSp, (double)_pitchSp, + (double)flightPathAngleSp, (double)flightPathAngle, + (double)accelerationLongitudinalSp, (double)airspeedDerivative); + } /* clean up */ _firstIterationAfterReset = false; + _counter++; } void mTecs::resetIntegrators() diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h index 5877a8312b..32d9879db6 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -106,6 +106,7 @@ protected: bool _firstIterationAfterReset; /**< True during the first iteration after a reset */ + int _counter; }; } /* namespace fwPosctrl */ From c81aefc756c6587e306986312370cf9d22a5f534 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:24:20 +0100 Subject: [PATCH 2/4] mtecs: commented out warnx --- .../fw_pos_control_l1/mtecs/mTecs_blocks.h | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index 7dd5c7c2e2..aff01479d1 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -89,8 +89,8 @@ public: float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", - name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); +// warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", +// name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { difference = value - minimum; value = minimum; @@ -136,15 +136,15 @@ public: float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, - (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); +// warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, +// (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && (((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) || ((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) { getIntegral().setY(integralYPrevious); } - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors @@ -179,11 +179,11 @@ public: float output = getKP() * input; char name[blockNameLengthMax]; getName(name, blockNameLengthMax); - warnx("%s output %.2f _kP.get() %.2f, input", - name,(double)output, (double)_kP.get(), (double)input); +// warnx("%s output %.2f _kP.get() %.2f, input", +// name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference); - warnx("%s output limited %.2f", - name,(double)output); +// warnx("%s output limited %.2f", +// name,(double)output); return output; } // accessors From f949395edf873b423cae3d62a95cd0800a2d344f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 22:25:16 +0100 Subject: [PATCH 3/4] mtecs: correct a very wrong default param value --- src/modules/fw_pos_control_l1/mtecs/mTecs_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c index 76ed953033..705b2cd595 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c @@ -213,7 +213,7 @@ PARAM_DEFINE_FLOAT(MT_ACC_P, 0.1f); * @unit m/s^2 * @group mTECS */ -PARAM_DEFINE_FLOAT(MT_ACC_MIN, 0.0f); +PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f); /** * Maximal acceleration (air) From d8eeec6cdbff505ac7f50877cc552b33ccf8837c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Mar 2014 23:03:06 +0100 Subject: [PATCH 4/4] mtecs: comment out debug code --- src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h index aff01479d1..c15d90cdb3 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h @@ -87,8 +87,8 @@ public: bool limit(float& value, float& difference) { float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin(); float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax(); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u", // name,(double)minimum,(double)maximum, (double)getMin(), (double)getMax(), isAngularLimit()); if (value < minimum) { @@ -134,8 +134,8 @@ public: float difference = 0.0f; float integralYPrevious = _integral.getY(); float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError); - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f getKFF() %.2f, inputValue %.2f, getKP() %.2f, getKI() %.2f, getIntegral().getY() %.2f, inputError %.2f getIntegral().getDt() %.2f", name, // (double)output, (double)getKFF(), (double)inputValue, (double)getKP(), (double)getKI(), (double)getIntegral().getY(), (double)inputError, (double)getIntegral().getDt()); if(!getOutputLimiter().limit(output, difference) && @@ -177,8 +177,8 @@ public: float update(float input) { float difference = 0.0f; float output = getKP() * input; - char name[blockNameLengthMax]; - getName(name, blockNameLengthMax); +// char name[blockNameLengthMax]; +// getName(name, blockNameLengthMax); // warnx("%s output %.2f _kP.get() %.2f, input", // name,(double)output, (double)_kP.get(), (double)input); getOutputLimiter().limit(output, difference);