Merge branch 'paul_mtecs_hil' into paul_mtecs

4 files changed, 41 insertions, 29 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 */
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 7dd5c7c2e..c15d90cdb 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -87,10 +87,10 @@ 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);
-		warnx("%s minimum %.2f maximum %.2f, getMin() %.2f, getMax() %.2f, isAngularLimit() %u",
-				 name,(double)minimum,(double)maximum,  (double)getMin(), (double)getMax(), isAngularLimit());
+//		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) {
 			difference = value - minimum;
 			value = minimum;
@@ -134,17 +134,17 @@ 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);
-		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());
+//		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) &&
 			(((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
@@ -177,13 +177,13 @@ public:
 	float update(float input) {
 		float difference = 0.0f;
 		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);
+//		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);
-		warnx("%s output limited %.2f",
-				 name,(double)output);
+//		warnx("%s output limited %.2f",
+//				 name,(double)output);
 		return output;
 	}
 // accessors
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 76ed95303..705b2cd59 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)