fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-06 12:38:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #2976 from PX4/mtecs

Browse Source 
Mtecs: small update and new filter block
This commit is contained in:
Lorenz Meier
2015-10-14 10:17:51 +02:00
parent 5d2b01a8e7 a87776eb81
commit c599cf1256
5 changed files with 119 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/blocks.cpp
+43
View File
@@ -39,6 +39,7 @@
#include <math.h>
#include <stdio.h>
#include <float.h>
#include "blocks.hpp"
@@ -51,6 +52,7 @@ int basicBlocksTest()
blockLimitSymTest();
blockLowPassTest();
blockHighPassTest();
blockLowPass2Test();
blockIntegralTest();
blockIntegralTrapTest();
blockDerivativeTest();
@@ -198,6 +200,47 @@ int blockHighPassTest()
return 0;
}
float BlockLowPass2::update(float input)
{
if (!isfinite(getState())) {
setState(input);
}
if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
_lp.set_cutoff_frequency(_fs, getFCutParam());
}
_state = _lp.apply(input);
return _state;
}
int blockLowPass2Test()
{
printf("Test BlockLowPass2\t\t: ");
BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
// test initial state
ASSERT(equal(10.0f, lowPass.getFCutParam()));
ASSERT(equal(0.0f, lowPass.getState()));
ASSERT(equal(0.0f, lowPass.getDt()));
// set dt
lowPass.setDt(0.1f);
ASSERT(equal(0.1f, lowPass.getDt()));
// set state
lowPass.setState(1.0f);
ASSERT(equal(1.0f, lowPass.getState()));
// test update
ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
// test end condition
for (int i = 0; i < 100; i++) {
lowPass.update(2.0f);
}
ASSERT(equal(2.0f, lowPass.getState()));
ASSERT(equal(2.0f, lowPass.update(2.0f)));
printf("PASS\n");
return 0;
};
float BlockIntegral::update(float input)
{
// trapezoidal integration
src/modules/controllib/blocks.hpp
+32
View File
@@ -45,6 +45,7 @@
#include <stdlib.h>
#include <math.h>
#include <mathlib/math/test/test.hpp>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include "block/Block.hpp"
#include "block/BlockParam.hpp"
@@ -163,6 +164,36 @@ protected:
int __EXPORT blockHighPassTest();
/**
* A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
*/
class __EXPORT BlockLowPass2 : public Block
{
public:
// methods
BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
Block(parent, name),
_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
_fCut(this, ""), // only one parameter, no need to name
_fs(sample_freq),
_lp(_fs, _fCut.get())
{};
virtual ~BlockLowPass2() {};
float update(float input);
// accessors
float getState() { return _state; }
float getFCutParam() { return _fCut.get(); }
void setState(float state) { _state = _lp.reset(state); }
protected:
// attributes
float _state;
control::BlockParamFloat _fCut;
float _fs;
math::LowPassFilter2p _lp;
};
int __EXPORT blockLowPass2Test();
/**
* A rectangular integrator.
* A limiter is built into the class to bound the
@@ -263,6 +294,7 @@ public:
void setU(float u) {_u = u;}
float getU() {return _u;}
float getLP() {return _lowPass.getFCut();}
float getO() { return _lowPass.getState(); }
protected:
// attributes
float _u; /**< previous input */
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+1
View File
@@ -81,6 +81,7 @@
#include <uORB/topics/sensor_combined.h>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/vehicle_status.h>
#include <uORB/topics/tecs_status.h>
#include <systemlib/param/param.h>
#include <systemlib/err.h>
#include <systemlib/pid/pid.h>
src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+20 -10
View File
@@ -52,15 +52,17 @@ mTecs::mTecs() :
_mTecsEnabled(this, "ENABLED"),
_airspeedMin(this, "FW_AIRSPD_MIN", false),
/* Publications */
#if defined(__PX4_NUTTX)
_status(ORB_ID(tecs_status), &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"),
_altitudeLowpass(this, "ALT_LP"),
_airspeedLowpass(this, "A_LP"),
_flightPathAngleLowpass(this, "FPA_LP", 50),
_altitudeLowpass(this, "ALT_LP", 50),
_airspeedLowpass(this, "A_LP", 50),
_airspeedDerivative(this, "AD"),
_throttleSp(0.0f),
_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
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.altitudeSp = altitudeSp;
_status.altitude_filtered = altitudeFiltered;
#endif // defined(__PX4_NUTTX)
/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
(double)airspeedFiltered, (double)accelerationLongitudinalSp);
}
#if defined(__PX4_NUTTX)
/* Write part of the status message */
_status.airspeedSp = airspeedSp;
_status.airspeed_filtered = airspeedFiltered;
#endif // defined(__PX4_NUTTX)
/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
}
/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
mode = tecs_status_s::TECS_MODE_UNDERSPEED;
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 TECS_MODE_NORMAL */
/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
if (mode == MTECS_MODE_TAKEOFF) {
outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND) {
} else if (mode == MTECS_MODE_LAND) {
// only limit pitch but do not limit throttle
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
outputLimiterPitch = &_BlockOutputLimiterLandPitch;
} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
} else if (mode == MTECS_MODE_UNDERSPEED) {
outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
* is running) */
limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
// #if defined(__PX4_NUTTX)
// /* Write part of the status message */
// _status.flightPathAngleSp = flightPathAngleSp;
// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
// _status.energyDistributionRateSp = energyDistributionRateSp;
// _status.energyDistributionRate = energyDistributionRate;
// _status.mode = mode;
// #endif // defined(__PX4_NUTTX)
/** update control blocks **/
/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
(double)accelerationLongitudinalSp, (double)airspeedDerivative);
}
#if defined(__PX4_NUTTX)
/* publish status message */
_status.update();
#endif // defined(__PX4_NUTTX)
/* clean up */
_firstIterationAfterReset = false;
src/modules/fw_pos_control_l1/mtecs/mTecs.h
+23 -3
View File
@@ -49,11 +49,25 @@
#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
{
@@ -94,6 +108,10 @@ public:
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 */
@@ -101,7 +119,9 @@ protected:
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
@@ -114,9 +134,9 @@ protected:
setpoint */
/* Other calculation Blocks */
control::BlockLowPass _flightPathAngleLowpass; /**< low pass filter for the flight path angle */
control::BlockLowPass _altitudeLowpass; /**< low pass filter for altitude */
control::BlockLowPass _airspeedLowpass; /**< low pass filter for airspeed */
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 */