fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-07 04:30:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge branch 'paul_mtecs_hil' into paul_mtecs

Browse Source
This commit is contained in:
Thomas Gubler
2014-03-27 18:29:37 +01:00
parent fa336e4a97 d8eeec6cdb
commit 54e1f2b2ce
4 changed files with 41 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+23 -12
View File
@@ -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()
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+1
View File
@@ -106,6 +106,7 @@ protected:
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
int _counter;
};
} /* namespace fwPosctrl */
src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+16 -16
View File
@@ -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
src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+1 -1
View File
@@ -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)