mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-10-15 07:43:33 +08:00
delete fw_pos_control_l1/mtecs
This commit is contained in:
Daniel Agar
committed by
Lorenz Meier
Lorenz Meier
parent
729e721ef3
commit
a133b12635
@@ -1,75 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file limitoverride.cpp
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include "limitoverride.h"
|
||||
|
||||
namespace fwPosctrl
|
||||
{
|
||||
|
||||
bool LimitOverride::applyOverride(BlockOutputLimiter &outputLimiterThrottle,
|
||||
BlockOutputLimiter &outputLimiterPitch)
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
if (overrideThrottleMinEnabled) {
|
||||
outputLimiterThrottle.setMin(overrideThrottleMin);
|
||||
ret = true;
|
||||
}
|
||||
|
||||
if (overrideThrottleMaxEnabled) {
|
||||
outputLimiterThrottle.setMax(overrideThrottleMax);
|
||||
ret = true;
|
||||
}
|
||||
|
||||
if (overridePitchMinEnabled) {
|
||||
outputLimiterPitch.setMin(overridePitchMin);
|
||||
ret = true;
|
||||
}
|
||||
|
||||
if (overridePitchMaxEnabled) {
|
||||
outputLimiterPitch.setMax(overridePitchMax);
|
||||
ret = true;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
||||
} /* namespace fwPosctrl */
|
||||
@@ -1,119 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file limitoverride.h
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
|
||||
#ifndef LIMITOVERRIDE_H_
|
||||
#define LIMITOVERRIDE_H_
|
||||
|
||||
#include "mTecs_blocks.h"
|
||||
|
||||
namespace fwPosctrl
|
||||
{
|
||||
|
||||
/* A small class which provides helper functions to override control output limits which are usually set by
|
||||
* parameters in special cases
|
||||
*/
|
||||
class LimitOverride
|
||||
{
|
||||
public:
|
||||
LimitOverride() :
|
||||
overrideThrottleMinEnabled(false),
|
||||
overrideThrottleMaxEnabled(false),
|
||||
overridePitchMinEnabled(false),
|
||||
overridePitchMaxEnabled(false)
|
||||
{}
|
||||
|
||||
~LimitOverride() {}
|
||||
|
||||
/*
|
||||
* Override the limits of the outputlimiter instances given by the arguments with the limits saved in
|
||||
* this class (if enabled)
|
||||
* @return true if the limit was applied
|
||||
*/
|
||||
bool applyOverride(BlockOutputLimiter &outputLimiterThrottle,
|
||||
BlockOutputLimiter &outputLimiterPitch);
|
||||
|
||||
/* Functions to enable or disable the override */
|
||||
void enableThrottleMinOverride(float value)
|
||||
{
|
||||
enable(&overrideThrottleMinEnabled,
|
||||
&overrideThrottleMin, value);
|
||||
}
|
||||
void disableThrottleMinOverride() { disable(&overrideThrottleMinEnabled); }
|
||||
void enableThrottleMaxOverride(float value)
|
||||
{
|
||||
enable(&overrideThrottleMaxEnabled,
|
||||
&overrideThrottleMax, value);
|
||||
}
|
||||
void disableThrottleMaxOverride() { disable(&overrideThrottleMaxEnabled); }
|
||||
void enablePitchMinOverride(float value)
|
||||
{
|
||||
enable(&overridePitchMinEnabled,
|
||||
&overridePitchMin, value);
|
||||
}
|
||||
void disablePitchMinOverride() { disable(&overridePitchMinEnabled); }
|
||||
void enablePitchMaxOverride(float value)
|
||||
{
|
||||
enable(&overridePitchMaxEnabled,
|
||||
&overridePitchMax, value);
|
||||
}
|
||||
void disablePitchMaxOverride() { disable(&overridePitchMaxEnabled); }
|
||||
|
||||
protected:
|
||||
bool overrideThrottleMinEnabled;
|
||||
float overrideThrottleMin;
|
||||
bool overrideThrottleMaxEnabled;
|
||||
float overrideThrottleMax;
|
||||
bool overridePitchMinEnabled;
|
||||
float overridePitchMin; //in degrees (replaces param values)
|
||||
bool overridePitchMaxEnabled;
|
||||
float overridePitchMax; //in degrees (replaces param values)
|
||||
|
||||
/* Enable a specific limit override */
|
||||
void enable(bool *flag, float *limit, float value) { *flag = true; *limit = value; };
|
||||
|
||||
/* Disable a specific limit override */
|
||||
void disable(bool *flag) { *flag = false; };
|
||||
};
|
||||
|
||||
} /* namespace fwPosctrl */
|
||||
|
||||
#endif /* LIMITOVERRIDE_H_ */
|
||||
@@ -1,342 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file mTecs.cpp
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#include "mTecs.h"
|
||||
|
||||
#include <lib/geo/geo.h>
|
||||
#include <stdio.h>
|
||||
|
||||
namespace fwPosctrl
|
||||
{
|
||||
|
||||
mTecs::mTecs() :
|
||||
SuperBlock(NULL, "MT"),
|
||||
/* Parameters */
|
||||
_mTecsEnabled(this, "ENABLED"),
|
||||
_airspeedMin(this, "FW_AIRSPD_MIN", false),
|
||||
/* Publications */
|
||||
#if defined(__PX4_NUTTX)
|
||||
_status(ORB_ID(tecs_status), -1, &getPublications()),
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
/* control blocks */
|
||||
_controlTotalEnergy(this, "THR"),
|
||||
_controlEnergyDistribution(this, "PIT", true),
|
||||
_controlAltitude(this, "FPA", true),
|
||||
_controlAirSpeed(this, "ACC"),
|
||||
_flightPathAngleLowpass(this, "FPA_LP", 50),
|
||||
_altitudeLowpass(this, "ALT_LP", 50),
|
||||
_airspeedLowpass(this, "A_LP", 50),
|
||||
_airspeedDerivative(this, "AD"),
|
||||
_throttleSp(0.0f),
|
||||
_pitchSp(0.0f),
|
||||
_BlockOutputLimiterTakeoffThrottle(this, "TKF_THR"),
|
||||
_BlockOutputLimiterTakeoffPitch(this, "TKF_PIT", true),
|
||||
_BlockOutputLimiterUnderspeedThrottle(this, "USP_THR"),
|
||||
_BlockOutputLimiterUnderspeedPitch(this, "USP_PIT", true),
|
||||
_BlockOutputLimiterLandThrottle(this, "LND_THR"),
|
||||
_BlockOutputLimiterLandPitch(this, "LND_PIT", true),
|
||||
timestampLastIteration(hrt_absolute_time()),
|
||||
_firstIterationAfterReset(true),
|
||||
_dtCalculated(false),
|
||||
_counter(0),
|
||||
_debug(false)
|
||||
{
|
||||
}
|
||||
|
||||
mTecs::~mTecs()
|
||||
{
|
||||
}
|
||||
|
||||
int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
|
||||
float airspeedSp, unsigned mode, LimitOverride limitOverride)
|
||||
{
|
||||
/* check if all input arguments are numbers and abort if not so */
|
||||
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(altitude) ||
|
||||
!PX4_ISFINITE(altitudeSp) || !PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* time measurement */
|
||||
updateTimeMeasurement();
|
||||
|
||||
/* Filter altitude */
|
||||
float altitudeFiltered = _altitudeLowpass.update(altitude);
|
||||
|
||||
|
||||
/* calculate flight path angle setpoint from altitude setpoint */
|
||||
float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
|
||||
|
||||
/* Debug output */
|
||||
if (_counter % 10 == 0) {
|
||||
debug("***");
|
||||
debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f",
|
||||
(double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
|
||||
}
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
/* Write part of the status message */
|
||||
_status.get().altitudeSp = altitudeSp;
|
||||
_status.get().altitude_filtered = altitudeFiltered;
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
|
||||
|
||||
/* use flightpath angle setpoint for total energy control */
|
||||
return updateFlightPathAngleSpeed(flightPathAngle, flightPathAngleSp, airspeed,
|
||||
airspeedSp, mode, limitOverride);
|
||||
}
|
||||
|
||||
int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
|
||||
float airspeedSp, unsigned mode, LimitOverride limitOverride)
|
||||
{
|
||||
/* check if all input arguments are numbers and abort if not so */
|
||||
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
|
||||
!PX4_ISFINITE(airspeed) || !PX4_ISFINITE(airspeedSp) || !PX4_ISFINITE(mode)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* time measurement */
|
||||
updateTimeMeasurement();
|
||||
|
||||
/* Filter airspeed */
|
||||
float airspeedFiltered = _airspeedLowpass.update(airspeed);
|
||||
|
||||
/* calculate longitudinal acceleration setpoint from airspeed setpoint*/
|
||||
float accelerationLongitudinalSp = _controlAirSpeed.update(airspeedSp - airspeedFiltered);
|
||||
|
||||
/* Debug output */
|
||||
if (_counter % 10 == 0) {
|
||||
debug("updateFlightPathAngleSpeed airspeedSp %.4f, airspeed %.4f airspeedFiltered %.4f,"
|
||||
"accelerationLongitudinalSp%.4f",
|
||||
(double)airspeedSp, (double)airspeed,
|
||||
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
|
||||
}
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
/* Write part of the status message */
|
||||
_status.get().airspeedSp = airspeedSp;
|
||||
_status.get().airspeed_filtered = airspeedFiltered;
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
|
||||
|
||||
/* use longitudinal acceleration setpoint for total energy control */
|
||||
return updateFlightPathAngleAcceleration(flightPathAngle, flightPathAngleSp, airspeedFiltered,
|
||||
accelerationLongitudinalSp, mode, limitOverride);
|
||||
}
|
||||
|
||||
int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
|
||||
float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride)
|
||||
{
|
||||
/* check if all input arguments are numbers and abort if not so */
|
||||
if (!PX4_ISFINITE(flightPathAngle) || !PX4_ISFINITE(flightPathAngleSp) ||
|
||||
!PX4_ISFINITE(airspeedFiltered) || !PX4_ISFINITE(accelerationLongitudinalSp) || !PX4_ISFINITE(mode)) {
|
||||
return -1;
|
||||
}
|
||||
|
||||
/* time measurement */
|
||||
updateTimeMeasurement();
|
||||
|
||||
/* update parameters first */
|
||||
updateParams();
|
||||
|
||||
/* Filter flightpathangle */
|
||||
float flightPathAngleFiltered = _flightPathAngleLowpass.update(flightPathAngle);
|
||||
|
||||
/* calculate values (energies) */
|
||||
float flightPathAngleError = flightPathAngleSp - flightPathAngleFiltered;
|
||||
float airspeedDerivative = 0.0f;
|
||||
|
||||
if (_airspeedDerivative.getDt() > 0.0f) {
|
||||
airspeedDerivative = _airspeedDerivative.update(airspeedFiltered);
|
||||
}
|
||||
|
||||
float airspeedDerivativeNorm = airspeedDerivative / CONSTANTS_ONE_G;
|
||||
float airspeedDerivativeSp = accelerationLongitudinalSp;
|
||||
float airspeedDerivativeNormSp = airspeedDerivativeSp / CONSTANTS_ONE_G;
|
||||
float airspeedDerivativeNormError = airspeedDerivativeNormSp - airspeedDerivativeNorm;
|
||||
|
||||
float totalEnergyRate = flightPathAngleFiltered + airspeedDerivativeNorm;
|
||||
float totalEnergyRateError = flightPathAngleError + airspeedDerivativeNormError;
|
||||
float totalEnergyRateSp = flightPathAngleSp + airspeedDerivativeNormSp;
|
||||
float totalEnergyRateError2 = totalEnergyRateSp - totalEnergyRate;
|
||||
|
||||
float energyDistributionRate = flightPathAngleFiltered - airspeedDerivativeNorm;
|
||||
float energyDistributionRateError = flightPathAngleError - airspeedDerivativeNormError;
|
||||
float energyDistributionRateSp = flightPathAngleSp - airspeedDerivativeNormSp;
|
||||
float energyDistributionRateError2 = energyDistributionRateSp - energyDistributionRate;
|
||||
|
||||
/* Debug output */
|
||||
if (_counter % 10 == 0) {
|
||||
debug("totalEnergyRateSp %.2f, totalEnergyRate %.2f, totalEnergyRateError %.2f totalEnergyRateError2 %.2f airspeedDerivativeNorm %.4f",
|
||||
(double)totalEnergyRateSp, (double)totalEnergyRate, (double)totalEnergyRateError, (double)totalEnergyRateError2,
|
||||
(double)airspeedDerivativeNorm);
|
||||
debug("energyDistributionRateSp %.2f, energyDistributionRate %.2f, energyDistributionRateError %.2f energyDistributionRateError2 %.2f",
|
||||
(double)energyDistributionRateSp, (double)energyDistributionRate, (double)energyDistributionRateError,
|
||||
(double)energyDistributionRateError2);
|
||||
}
|
||||
|
||||
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
|
||||
if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
|
||||
mode = MTECS_MODE_UNDERSPEED;
|
||||
}
|
||||
|
||||
/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
|
||||
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
|
||||
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
|
||||
|
||||
if (mode == MTECS_MODE_TAKEOFF) {
|
||||
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
|
||||
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
|
||||
|
||||
} else if (mode == MTECS_MODE_LAND) {
|
||||
// only limit pitch but do not limit throttle
|
||||
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
|
||||
|
||||
} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
|
||||
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
|
||||
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
|
||||
|
||||
} else if (mode == MTECS_MODE_UNDERSPEED) {
|
||||
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
|
||||
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
|
||||
}
|
||||
|
||||
/* Apply override given by the limitOverride argument (this is used for limits which are not given by
|
||||
* parameters such as pitch limits with takeoff waypoints or throttle limits when the launchdetector
|
||||
* is running) */
|
||||
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
|
||||
|
||||
// #if defined(__PX4_NUTTX)
|
||||
// /* Write part of the status message */
|
||||
// _status.flightPathAngleSp = flightPathAngleSp;
|
||||
// _status.flightPathAngle = flightPathAngle;
|
||||
// _status.flightPathAngleFiltered = flightPathAngleFiltered;
|
||||
// _status.airspeedDerivativeSp = airspeedDerivativeSp;
|
||||
// _status.airspeedDerivative = airspeedDerivative;
|
||||
// _status.totalEnergyRateSp = totalEnergyRateSp;
|
||||
// _status.totalEnergyRate = totalEnergyRate;
|
||||
// _status.energyDistributionRateSp = energyDistributionRateSp;
|
||||
// _status.energyDistributionRate = energyDistributionRate;
|
||||
// _status.mode = mode;
|
||||
// #endif // defined(__PX4_NUTTX)
|
||||
|
||||
/** update control blocks **/
|
||||
/* update total energy rate control block */
|
||||
_throttleSp = _controlTotalEnergy.update(totalEnergyRateSp, totalEnergyRateError, outputLimiterThrottle);
|
||||
|
||||
/* update energy distribution rate control block */
|
||||
_pitchSp = _controlEnergyDistribution.update(energyDistributionRateSp, energyDistributionRateError, outputLimiterPitch);
|
||||
|
||||
|
||||
if (_counter % 10 == 0) {
|
||||
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);
|
||||
}
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
/* publish status message */
|
||||
_status.update();
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
|
||||
/* clean up */
|
||||
_firstIterationAfterReset = false;
|
||||
_dtCalculated = false;
|
||||
|
||||
_counter++;
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
void mTecs::resetIntegrators()
|
||||
{
|
||||
_controlTotalEnergy.getIntegral().setY(0.0f);
|
||||
_controlEnergyDistribution.getIntegral().setY(0.0f);
|
||||
timestampLastIteration = hrt_absolute_time();
|
||||
_firstIterationAfterReset = true;
|
||||
}
|
||||
|
||||
void mTecs::resetDerivatives(float airspeed)
|
||||
{
|
||||
_airspeedDerivative.setU(airspeed);
|
||||
}
|
||||
|
||||
|
||||
void mTecs::updateTimeMeasurement()
|
||||
{
|
||||
if (!_dtCalculated) {
|
||||
float deltaTSeconds = 0.0f;
|
||||
|
||||
if (!_firstIterationAfterReset) {
|
||||
hrt_abstime timestampNow = hrt_absolute_time();
|
||||
deltaTSeconds = (float)(timestampNow - timestampLastIteration) * 1e-6f;
|
||||
timestampLastIteration = timestampNow;
|
||||
}
|
||||
|
||||
setDt(deltaTSeconds);
|
||||
|
||||
_dtCalculated = true;
|
||||
}
|
||||
}
|
||||
|
||||
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);
|
||||
va_end(args);
|
||||
}
|
||||
|
||||
} /* namespace fwPosctrl */
|
||||
@@ -1,176 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file mTecs.h
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
|
||||
#ifndef MTECS_H_
|
||||
#define MTECS_H_
|
||||
|
||||
#include "mTecs_blocks.h"
|
||||
#include "limitoverride.h"
|
||||
|
||||
#include <controllib/block/BlockParam.hpp>
|
||||
#include <drivers/drv_hrt.h>
|
||||
#include <uORB/Publication.hpp>
|
||||
|
||||
#if defined(__PX4_NUTTX)
|
||||
#include <uORB/topics/tecs_status.h>
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
|
||||
namespace fwPosctrl
|
||||
{
|
||||
|
||||
/* corresponds to TECS_MODE in tecs_status.msg */
|
||||
enum MTECS_MODE {
|
||||
MTECS_MODE_NORMAL = 0,
|
||||
MTECS_MODE_UNDERSPEED = 1,
|
||||
MTECS_MODE_TAKEOFF = 2,
|
||||
MTECS_MODE_LAND = 3,
|
||||
MTECS_MODE_LAND_THROTTLELIM = 4,
|
||||
MTECS_MODE_BAD_DESCENT = 5,
|
||||
MTECS_MODE_CLIMBOUT = 6
|
||||
};
|
||||
|
||||
/* Main class of the mTecs */
|
||||
class mTecs : public control::SuperBlock
|
||||
{
|
||||
public:
|
||||
mTecs();
|
||||
virtual ~mTecs();
|
||||
|
||||
/*
|
||||
* Control in altitude setpoint and speed mode
|
||||
*/
|
||||
int updateAltitudeSpeed(float flightPathAngle, float altitude, float altitudeSp, float airspeed,
|
||||
float airspeedSp, unsigned mode, LimitOverride limitOverride);
|
||||
|
||||
/*
|
||||
* Control in flightPathAngle setpoint (flollow a slope etc.) and speed mode
|
||||
*/
|
||||
int updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAngleSp, float airspeed,
|
||||
float airspeedSp, unsigned mode, LimitOverride limitOverride);
|
||||
|
||||
/*
|
||||
* Control in flightPathAngle setpoint (flollow a slope etc.) and acceleration mode (base case)
|
||||
*/
|
||||
int updateFlightPathAngleAcceleration(float flightPathAngle, float flightPathAngleSp, float airspeedFiltered,
|
||||
float accelerationLongitudinalSp, unsigned mode, LimitOverride limitOverride);
|
||||
|
||||
/*
|
||||
* Reset all integrators
|
||||
*/
|
||||
void resetIntegrators();
|
||||
|
||||
/*
|
||||
* Reset all derivative calculations
|
||||
*/
|
||||
void resetDerivatives(float airspeed);
|
||||
|
||||
/* Accessors */
|
||||
bool getEnabled() { return _mTecsEnabled.get() > 0; }
|
||||
float getThrottleSetpoint() { return _throttleSp; }
|
||||
float getPitchSetpoint() { return _pitchSp; }
|
||||
float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
|
||||
float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
|
||||
float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
|
||||
float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
|
||||
float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
|
||||
|
||||
protected:
|
||||
/* parameters */
|
||||
control::BlockParamInt _mTecsEnabled; /**< 1 if mTecs is enabled */
|
||||
control::BlockParamFloat _airspeedMin; /**< minimal airspeed */
|
||||
|
||||
/* Publications */
|
||||
#if defined(__PX4_NUTTX)
|
||||
uORB::Publication<tecs_status_s> _status; /**< publish internal values for logging */
|
||||
#endif // defined(__PX4_NUTTX)
|
||||
|
||||
/* control blocks */
|
||||
BlockFFPILimitedCustom _controlTotalEnergy; /**< FFPI controller for total energy control: output
|
||||
is throttle */
|
||||
BlockFFPILimitedCustom _controlEnergyDistribution; /**< FFPI controller for energy distribution control:
|
||||
output is pitch */
|
||||
BlockPDLimited _controlAltitude; /**< PD controller for altitude: output is the flight
|
||||
path angle setpoint */
|
||||
BlockPDLimited _controlAirSpeed; /**< PD controller for airspeed: output is acceleration
|
||||
setpoint */
|
||||
|
||||
/* Other calculation Blocks */
|
||||
control::BlockLowPass2 _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
|
||||
control::BlockLowPass2 _altitudeLowpass; /**< low pass filter for altitude */
|
||||
control::BlockLowPass2 _airspeedLowpass; /**< low pass filter for airspeed */
|
||||
control::BlockDerivative _airspeedDerivative; /**< airspeed derivative calulation */
|
||||
|
||||
/* Output setpoints */
|
||||
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 */
|
||||
BlockOutputLimiter _BlockOutputLimiterUnderspeedThrottle; /**< Throttle Limits when underspeed is detected */
|
||||
BlockOutputLimiter _BlockOutputLimiterUnderspeedPitch; /**< Pitch Limit when underspeed is detected */
|
||||
BlockOutputLimiter _BlockOutputLimiterLandThrottle; /**< Throttle Limits during landing (only in
|
||||
last phase)*/
|
||||
BlockOutputLimiter _BlockOutputLimiterLandPitch; /**< Pitch Limit during landing */
|
||||
|
||||
/* Time measurements */
|
||||
hrt_abstime timestampLastIteration; /**< Saves the result of hrt_absolute_time() of the last iteration */
|
||||
|
||||
bool _firstIterationAfterReset; /**< True during the first iteration after a reset */
|
||||
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
|
||||
*/
|
||||
void updateTimeMeasurement();
|
||||
};
|
||||
|
||||
} /* namespace fwPosctrl */
|
||||
|
||||
#endif /* MTECS_H_ */
|
||||
@@ -1,230 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
|
||||
/**
|
||||
* @file mTecs_blocks.h
|
||||
*
|
||||
* Custom blocks for the mTecs
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <controllib/blocks.hpp>
|
||||
#include <systemlib/err.h>
|
||||
|
||||
namespace fwPosctrl
|
||||
{
|
||||
|
||||
using namespace control;
|
||||
|
||||
/* An block which can be used to limit the output */
|
||||
class BlockOutputLimiter: public SuperBlock
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
BlockOutputLimiter(SuperBlock *parent, const char *name, bool fAngularLimit = false) :
|
||||
SuperBlock(parent, name),
|
||||
_isAngularLimit(fAngularLimit),
|
||||
_min(this, "MIN"),
|
||||
_max(this, "MAX")
|
||||
{}
|
||||
virtual ~BlockOutputLimiter() {}
|
||||
/*
|
||||
* Imposes the limits given by _min and _max on value
|
||||
*
|
||||
* @param value is changed to be on the interval _min to _max
|
||||
* @param difference if the value is changed this corresponds to the change of value * (-1)
|
||||
* otherwise unchanged
|
||||
* @return: true if the limit is applied, false otherwise
|
||||
*/
|
||||
bool limit(float &value, float &difference)
|
||||
{
|
||||
float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
|
||||
float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
|
||||
|
||||
if (value < minimum) {
|
||||
difference = value - minimum;
|
||||
value = minimum;
|
||||
return true;
|
||||
|
||||
} else if (value > maximum) {
|
||||
difference = value - maximum;
|
||||
value = maximum;
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
//accessor:
|
||||
bool getIsAngularLimit() {return _isAngularLimit ;}
|
||||
float getMin() { return _min.get(); }
|
||||
float getMax() { return _max.get(); }
|
||||
void setMin(float value) { _min.set(value); }
|
||||
void setMax(float value) { _max.set(value); }
|
||||
protected:
|
||||
//attributes
|
||||
bool _isAngularLimit;
|
||||
control::BlockParamFloat _min;
|
||||
control::BlockParamFloat _max;
|
||||
};
|
||||
|
||||
|
||||
/* 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")
|
||||
{}
|
||||
virtual ~BlockFFPILimited() {}
|
||||
float update(float inputValue, float inputError) { return calcLimitedOutput(inputValue, inputError, _outputLimiter); }
|
||||
// accessors
|
||||
BlockIntegral &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 = calcUnlimitedOutput(inputValue, inputError);
|
||||
|
||||
if (outputLimiter.limit(output, difference) &&
|
||||
(((difference < 0) && (getKI() * getIntegral().getY() < 0)) ||
|
||||
((difference > 0) && (getKI() * getIntegral().getY() > 0)))) {
|
||||
getIntegral().setY(integralYPrevious);
|
||||
}
|
||||
|
||||
return output;
|
||||
}
|
||||
private:
|
||||
BlockIntegral _integral;
|
||||
BlockParamFloat _kFF;
|
||||
BlockParamFloat _kP;
|
||||
BlockParamFloat _kI;
|
||||
BlockParamFloat _offset;
|
||||
};
|
||||
|
||||
/* 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 */
|
||||
class BlockPLimited: public SuperBlock
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
BlockPLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
|
||||
SuperBlock(parent, name),
|
||||
_kP(this, "P"),
|
||||
_outputLimiter(this, "", isAngularLimit)
|
||||
{}
|
||||
virtual ~BlockPLimited() {}
|
||||
float update(float input)
|
||||
{
|
||||
float difference = 0.0f;
|
||||
float output = getKP() * input;
|
||||
getOutputLimiter().limit(output, difference);
|
||||
return output;
|
||||
}
|
||||
// accessors
|
||||
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
|
||||
float getKP() { return _kP.get(); }
|
||||
private:
|
||||
control::BlockParamFloat _kP;
|
||||
BlockOutputLimiter _outputLimiter;
|
||||
};
|
||||
|
||||
/* A combination of P, D gains and output limiter */
|
||||
class BlockPDLimited: public SuperBlock
|
||||
{
|
||||
public:
|
||||
// methods
|
||||
BlockPDLimited(SuperBlock *parent, const char *name, bool isAngularLimit = false) :
|
||||
SuperBlock(parent, name),
|
||||
_kP(this, "P"),
|
||||
_kD(this, "D"),
|
||||
_derivative(this, "D"),
|
||||
_outputLimiter(this, "", isAngularLimit)
|
||||
{}
|
||||
virtual ~BlockPDLimited() {}
|
||||
float update(float input)
|
||||
{
|
||||
float difference = 0.0f;
|
||||
float output = getKP() * input + (getDerivative().getDt() > 0.0f ? getKD() * getDerivative().update(input) : 0.0f);
|
||||
getOutputLimiter().limit(output, difference);
|
||||
|
||||
return output;
|
||||
}
|
||||
// accessors
|
||||
float getKP() { return _kP.get(); }
|
||||
float getKD() { return _kD.get(); }
|
||||
BlockDerivative &getDerivative() { return _derivative; }
|
||||
BlockOutputLimiter &getOutputLimiter() { return _outputLimiter; };
|
||||
private:
|
||||
control::BlockParamFloat _kP;
|
||||
control::BlockParamFloat _kD;
|
||||
BlockDerivative _derivative;
|
||||
BlockOutputLimiter _outputLimiter;
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
@@ -1,419 +0,0 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2013 PX4 Development Team. All rights reserved.
|
||||
* Author: @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
/**
|
||||
* @file mTecs_params.c
|
||||
*
|
||||
* @author Thomas Gubler <thomasgubler@gmail.com>
|
||||
*/
|
||||
|
||||
/*
|
||||
* Controller parameters, accessible via MAVLink
|
||||
*/
|
||||
|
||||
/**
|
||||
* mTECS enabled
|
||||
*
|
||||
* @boolean
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_INT32(MT_ENABLED, 0);
|
||||
|
||||
/**
|
||||
* Total Energy Rate Control Feedforward
|
||||
* Maps the total energy rate setpoint to the throttle setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_FF, 0.7f);
|
||||
|
||||
/**
|
||||
* Total Energy Rate Control P
|
||||
* Maps the total energy rate error to the throttle setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_P, 0.1f);
|
||||
|
||||
/**
|
||||
* Total Energy Rate Control I
|
||||
* Maps the integrated total energy rate to the throttle setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_I, 0.25f);
|
||||
|
||||
/**
|
||||
* Total Energy Rate Control Offset (Cruise throttle sp)
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_OFF, 0.7f);
|
||||
|
||||
/**
|
||||
* Energy Distribution Rate Control Feedforward
|
||||
* Maps the energy distribution rate setpoint to the pitch setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_FF, 0.4f);
|
||||
|
||||
/**
|
||||
* Energy Distribution Rate Control P
|
||||
* Maps the energy distribution rate error to the pitch setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_P, 0.03f);
|
||||
|
||||
/**
|
||||
* Energy Distribution Rate Control I
|
||||
* Maps the integrated energy distribution rate error to the pitch setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_I, 0.03f);
|
||||
|
||||
/**
|
||||
* Total Energy Distribution Offset (Cruise pitch sp)
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_OFF, 0.0f);
|
||||
|
||||
/**
|
||||
* Minimal Throttle Setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Maximal Throttle Setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Minimal Pitch Setpoint in Degrees
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
|
||||
|
||||
/**
|
||||
* Maximal Pitch Setpoint in Degrees
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
|
||||
|
||||
/**
|
||||
* Lowpass (cutoff freq.) for altitude
|
||||
*
|
||||
* @unit Hz
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
|
||||
|
||||
/**
|
||||
* Lowpass (cutoff freq.) for the flight path angle
|
||||
*
|
||||
* @unit Hz
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_LP, 1.0f);
|
||||
|
||||
/**
|
||||
* P gain for the altitude control
|
||||
* Maps the altitude error to the flight path angle setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_P, 0.3f);
|
||||
|
||||
/**
|
||||
* D gain for the altitude control
|
||||
* Maps the change of altitude error to the flight path angle setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Lowpass for FPA error derivative calculation (see MT_FPA_D)
|
||||
*
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_D_LP, 1.0f);
|
||||
|
||||
/**
|
||||
* Minimal flight path angle setpoint
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_MIN, -20.0f);
|
||||
|
||||
/**
|
||||
* Maximal flight path angle setpoint
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_FPA_MAX, 30.0f);
|
||||
|
||||
/**
|
||||
* Lowpass (cutoff freq.) for airspeed
|
||||
*
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_A_LP, 0.5f);
|
||||
|
||||
/**
|
||||
* Airspeed derivative calculation lowpass
|
||||
*
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_AD_LP, 0.5f);
|
||||
|
||||
/**
|
||||
* P gain for the airspeed control
|
||||
* Maps the airspeed error to the acceleration setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ACC_P, 0.3f);
|
||||
|
||||
/**
|
||||
* D gain for the airspeed control
|
||||
* Maps the change of airspeed error to the acceleration setpoint
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ACC_D, 0.0f);
|
||||
|
||||
/**
|
||||
* Lowpass for ACC error derivative calculation (see MT_ACC_D)
|
||||
*
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ACC_D_LP, 0.5f);
|
||||
|
||||
/**
|
||||
* Minimal acceleration (air)
|
||||
*
|
||||
* @unit m/s/s
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ACC_MIN, -40.0f);
|
||||
|
||||
/**
|
||||
* Maximal acceleration (air)
|
||||
*
|
||||
* @unit m/s/s
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_ACC_MAX, 40.0f);
|
||||
|
||||
/**
|
||||
* Minimal throttle during takeoff
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_TKF_THR_MIN, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximal throttle during takeoff
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_TKF_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Minimal pitch during takeoff
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Maximal pitch during takeoff
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_TKF_PIT_MAX, 45.0f);
|
||||
|
||||
/**
|
||||
* Minimal throttle in underspeed mode
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_USP_THR_MIN, 1.0f);
|
||||
|
||||
/**
|
||||
* Maximal throttle in underspeed mode
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_USP_THR_MAX, 1.0f);
|
||||
|
||||
/**
|
||||
* Minimal pitch in underspeed mode
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_USP_PIT_MIN, -45.0f);
|
||||
|
||||
/**
|
||||
* Maximal pitch in underspeed mode
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_USP_PIT_MAX, 0.0f);
|
||||
|
||||
/**
|
||||
* Minimal throttle in landing mode (only last phase of landing)
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_LND_THR_MIN, 0.0f);
|
||||
|
||||
/**
|
||||
* Maximal throttle in landing mode (only last phase of landing)
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 1.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_LND_THR_MAX, 0.0f);
|
||||
|
||||
/**
|
||||
* Minimal pitch in landing mode
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_LND_PIT_MIN, -5.0f);
|
||||
|
||||
/**
|
||||
* Maximal pitch in landing mode
|
||||
*
|
||||
* @min -90.0
|
||||
* @max 90.0
|
||||
* @unit deg
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_LND_PIT_MAX, 15.0f);
|
||||
|
||||
/**
|
||||
* Integrator Limit for Total Energy Rate Control
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_THR_I_MAX, 10.0f);
|
||||
|
||||
/**
|
||||
* Integrator Limit for Energy Distribution Rate Control
|
||||
*
|
||||
* @min 0.0
|
||||
* @max 10.0
|
||||
* @group mTECS
|
||||
*/
|
||||
PARAM_DEFINE_FLOAT(MT_PIT_I_MAX, 10.0f);
|
||||
Reference in New Issue
Block a user