From 96ddea082690cd826cee10c4c00c95b2f87caf90 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 27 Apr 2014 12:45:02 +0200 Subject: Introduce debug option for mTECS and silence it as default --- src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 37 +++++++++++++++++++++------ src/modules/fw_pos_control_l1/mtecs/mTecs.h | 5 ++++ 2 files changed, 34 insertions(+), 8 deletions(-) (limited to 'src') diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp index 1fb3153e9..a4cf0bdf8 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp @@ -42,7 +42,7 @@ #include "mTecs.h" #include -#include +#include namespace fwPosctrl { @@ -66,7 +66,8 @@ mTecs::mTecs() : timestampLastIteration(hrt_absolute_time()), _firstIterationAfterReset(true), _dtCalculated(false), - _counter(0) + _counter(0), + _debug(false) { } @@ -82,8 +83,8 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude); if (_counter % 10 == 0) { - warnx("***"); - warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); + debug("***"); + debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp); } updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode); } @@ -95,7 +96,7 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeed); if (_counter % 10 == 0) { - warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); + debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp); } updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode); } @@ -130,9 +131,9 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate; if (_counter % 10 == 0) { - warnx("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f", + debug("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", + debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f", (double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2); } @@ -161,7 +162,7 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh if (_counter % 10 == 0) { - warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", + debug("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f", (double)_throttleSp, (double)_pitchSp, (double)flightPathAngleSp, (double)flightPathAngle, (double)accelerationLongitudinalSp, (double)airspeedDerivative); @@ -198,5 +199,25 @@ void mTecs::updateTimeMeasurement() } } +void mTecs::debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[mtecs]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +void mTecs::debug(const char *fmt, ...) { + + if (!_debug) { + return; + } + + va_list args; + + va_start(args, fmt); + debug_print(fmt, args); +} + } /* 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 393dd294d..fab4b161a 100644 --- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h +++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h @@ -121,6 +121,11 @@ protected: bool _dtCalculated; /**< True if dt has been calculated in this iteration */ int _counter; + bool _debug; ///< Set true to enable debug output + + + static void debug_print(const char *fmt, va_list args); + void debug(const char *fmt, ...); /* * Measure and update the time step dt if this was not already done in the current iteration -- cgit v1.2.3