aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
Diffstat
-rw-r--r--src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp11
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.cpp25
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs.h19
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h46
-rw-r--r--src/modules/fw_pos_control_l1/mtecs/mTecs_params.c39
5 files changed, 112 insertions, 28 deletions
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 1e8447aed..44afa43d8 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -361,7 +361,7 @@ private:
void reset_landing_state();
/*
- * Call TECS
+ * Call TECS : a wrapper function to call one of the TECS implementations (mTECS is called only if enabled via parameter)
*/
void tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
float pitch_min_rad, float pitch_max_rad,
@@ -1364,7 +1364,14 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
if (ground_speed_length > FLT_EPSILON) {
flightPathAngle = -asinf(ground_speed(2)/ground_speed_length);
}
- _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp);
+
+ if (!climbout_mode) {
+ /* Normal operation */
+ _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_NORMAL);
+ } else {
+ /* Climbout/Takeoff mode requested */
+ _mTecs.updateAltitudeSpeed(flightPathAngle, _global_pos.alt, alt_sp, _airspeed.true_airspeed_m_s, v_sp, fwPosctrl::mTecs::TECS_MODE_TAKEOFF);
+ }
} else {
_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, alt_sp, v_sp,
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 3118ea5d1..33bfd4962 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -58,6 +58,8 @@ mTecs::mTecs() :
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
+ BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
+ BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
timestampLastIteration(hrt_absolute_time()),
_firstIterationAfterReset(true),
dtCalculated(false),
@@ -69,7 +71,7 @@ mTecs::~mTecs()
{
}
-void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp)
+void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode)
{
/* time measurement */
@@ -80,10 +82,10 @@ void mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alt
warnx("***");
warnx("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
}
- updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp);
+ updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed, airspeedSp, mode);
}
-void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp) {
+void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode) {
/* time measurement */
updateTimeMeasurement();
@@ -92,10 +94,10 @@ void mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAn
if (_counter % 10 == 0) {
warnx("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f, accelerationLongitudinalSp%.4f", (double)airspeedSp, (double)airspeed, (double)accelerationLongitudinalSp);
}
- updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp);
+ updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeed, accelerationLongitudinalSp, mode);
}
-void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp)
+void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode)
{
/* time measurement */
updateTimeMeasurement();
@@ -131,12 +133,21 @@ void mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float fligh
(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError, (double)energyDistributionRateError2);
}
+ /* Set special ouput limiters if we are not in TECS_MODE_NORMAL */
+ BlockOutputLimiter *outputLimiterThrottle = NULL;
+ BlockOutputLimiter *outputLimiterPitch = NULL;
+ if (mode == TECS_MODE_TAKEOFF) {
+ outputLimiterThrottle = &BlockOutputLimiterTakeoffThrottle; //XXX: accept prelaunch values from launchdetector
+ outputLimiterPitch = &BlockOutputLimiterTakeoffPitch;
+ }
+
/** update control blocks **/
/* update total energy rate control block */
- _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError);
+ _throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
/* update energy distribution rate control block */
- _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError);
+ _pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
+
if (_counter % 10 == 0) {
warnx("_throttleSp %.1f, _pitchSp %.1f, flightPathAngleSp %.1f, flightPathAngle %.1f accelerationLongitudinalSp %.1f, airspeedDerivative %.1f",
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index 62c4d7014..6c30a458a 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -58,20 +58,25 @@ public:
mTecs();
virtual ~mTecs();
+ typedef enum {
+ TECS_MODE_NORMAL,
+ TECS_MODE_TAKEOFF
+ } tecs_mode;
+
/*
* Control in altitude setpoint and speed mode
*/
- void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp);
+ void updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed, float airspeedSp, tecs_mode mode);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
*/
- void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp);
+ void updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed, float airspeedSp, tecs_mode mode);
/*
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
*/
- void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp);
+ void updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeed, float accelerationLongitudinalSp, tecs_mode mode);
/*
* Reset all integrators
@@ -89,8 +94,8 @@ protected:
control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */
/* control blocks */
- BlockFFPILimited _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */
- BlockFFPILimited _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */
+ BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output is throttle */
+ BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control: output is pitch */
BlockPDLimited _controlAltitude; /**< P controller for altitude: output is the flight path angle setpoint */
BlockPLimited _controlAirSpeed; /**< P controller for airspeed: output is acceleration setpoint */
@@ -101,6 +106,10 @@ protected:
float _throttleSp; /**< Throttle Setpoint from 0 to 1 */
float _pitchSp; /**< Pitch Setpoint from -pi to pi */
+ /* Output Limits in special modes */
+ BlockOutputLimiter BlockOutputLimiterTakeoffThrottle; /**< Throttle Limits during takeoff */
+ BlockOutputLimiter BlockOutputLimiterTakeoffPitch; /**< Pitch Limit during takeoff */
+
/* Time measurements */
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
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 6e90a82ba..f3dc9bcb2 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -111,46 +111,64 @@ protected:
typedef
-/* A combination of feed forward, P and I gain using the output limiter*/
+/* A combination of feed forward, P and I gain using the output limiter*/
class BlockFFPILimited: public SuperBlock
{
public:
// methods
BlockFFPILimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
SuperBlock(parent, name),
+ _outputLimiter(this, "", isAngularLimit),
_integral(this, "I"),
_kFF(this, "FF"),
_kP(this, "P"),
_kI(this, "I"),
- _offset(this, "OFF"),
- _outputLimiter(this, "", isAngularLimit)
+ _offset(this, "OFF")
{};
virtual ~BlockFFPILimited() {};
- float update(float inputValue, float inputError) {
+ float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
+// accessors
+ BlockIntegralNoLimit &getIntegral() { return _integral; }
+ float getKFF() { return _kFF.get(); }
+ float getKP() { return _kP.get(); }
+ float getKI() { return _kI.get(); }
+ float getOffset() { return _offset.get(); }
+ BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
+protected:
+ BlockOutputLimiter _outputLimiter;
+
+ float calcUnlimitedOutput(float inputValue, float inputError) {return getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);}
+ float calcLimitedOutput(float inputValue, float inputError, BlockOutputLimiter &outputLimiter) {
float difference = 0.0f;
float integralYPrevious = _integral.getY();
- float output = getOffset() + getKFF() * inputValue + getKP() * inputError + getKI() * getIntegral().update(inputError);
- if(!getOutputLimiter().limit(output, difference) &&
+ float output = calcUnlimitedOutput(inputValue, inputError);
+ if(!outputLimiter.limit(output, difference) &&
(((difference < 0) && (getKI() * getIntegral().update(inputError) < 0)) ||
((difference > 0) && (getKI() * getIntegral().update(inputError) > 0)))) {
getIntegral().setY(integralYPrevious);
}
return output;
}
-// accessors
- BlockIntegralNoLimit &getIntegral() { return _integral; }
- float getKFF() { return _kFF.get(); }
- float getKP() { return _kP.get(); }
- float getKI() { return _kI.get(); }
- float getOffset() { return _offset.get(); }
- BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
private:
BlockIntegralNoLimit _integral;
BlockParamFloat _kFF;
BlockParamFloat _kP;
BlockParamFloat _kI;
BlockParamFloat _offset;
- BlockOutputLimiter _outputLimiter;
+};
+
+/* A combination of feed forward, P and I gain using the output limiter with the option to provide a special output limiter (for example for takeoff)*/
+class BlockFFPILimitedCustom: public BlockFFPILimited
+{
+public:
+// methods
+ BlockFFPILimitedCustom(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
+ BlockFFPILimited(parent, name, isAngularLimit)
+ {};
+ virtual ~BlockFFPILimitedCustom() {};
+ float update(float inputValue, float inputError, BlockOutputLimiter *outputLimiter = NULL) {
+ return calcLimitedOutput(inputValue, inputError, outputLimiter == NULL ? _outputLimiter : *outputLimiter);
+ }
};
/* A combination of P gain and output limiter */
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 f8db70304..bbd957273 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -246,3 +246,42 @@ PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
* @group mTECS
*/
PARAM_DEFINE_FLOAT(MT_AD_LP, 1.0f);
+
+
+/**
+ * Minimal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
+
+/**
+ * Maximal throttle during takeoff
+ *
+ * @min 0.0f
+ * @max 1.0f
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
+
+/**
+ * Minimal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
+
+/**
+ * Maximal pitch during takeoff
+ *
+ * @min -90.0f
+ * @max 90.0f
+ * @unit deg
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-12 02:07:44 +0000