From b61e6f2706fed68257c909bdd8a84feda5121344 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Mar 2014 21:23:48 +0100 Subject: 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 ad108e7ee..fa9a6d947 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 5877a8312..32d9879db 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 */ -- cgit v1.2.3