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

Added velocity adjustment to stabilization.

Browse Source
This commit is contained in:
James Goppert 2013-04-16 14:11:56 -04:00 committed by James Goppert
parent 078ae23cfa
commit 100bcefc17
3 changed files with 42 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

54
apps/controllib/fixedwing.cpp
View File

@ -56,9 +56,9 @@ BlockYawDamper::BlockYawDamper(SuperBlock *parent, const char *name) :
BlockYawDamper::~BlockYawDamper() {};
void BlockYawDamper::update(float rCmd, float r)
void BlockYawDamper::update(float rCmd, float r, float outputScale)
{
_rudder = _r2Rdr.update(rCmd -
_rudder = outputScale*_r2Rdr.update(rCmd -
_rWashout.update(_rLowPass.update(r)));
}
@ -77,13 +77,13 @@ BlockStabilization::BlockStabilization(SuperBlock *parent, const char *name) :
BlockStabilization::~BlockStabilization() {};
void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
float p, float q, float r)
float p, float q, float r, float outputScale)
{
_aileron = _p2Ail.update(
_aileron = outputScale*_p2Ail.update(
pCmd - _pLowPass.update(p));
_elevator = _q2Elv.update(
_elevator = outputScale*_q2Elv.update(
qCmd - _qLowPass.update(q));
_yawDamper.update(rCmd, r);
_yawDamper.update(rCmd, r, outputScale);
}
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
@ -163,11 +163,11 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
// guidance block
_guide(this, ""),
// block params
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
_trimThr(this, "TRIM_THR", true), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
_trimAil(this, "TRIM_ROLL", false), /* general roll trim (full name: TRIM_ROLL) */
_trimElv(this, "TRIM_PITCH", false), /* general pitch trim */
_trimRdr(this, "TRIM_YAW", false), /* general yaw trim */
_trimThr(this, "TRIM_THR"), /* FWB_ specific throttle trim (full name: FWB_TRIM_THR) */
_trimV(this, "TRIM_V"), /* FWB_ specific trim velocity (full name : FWB_TRIM_V) */
_vCmd(this, "V_CMD"),
_rocMax(this, "ROC_MAX"),
@ -228,7 +228,15 @@ void BlockMultiModeBacksideAutopilot::update()
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// limit velocity command between min/max velocity
float vCmd = _vLimit.update(_vCmd.get());
// altitude hold
float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
@ -240,16 +248,19 @@ void BlockMultiModeBacksideAutopilot::update()
// velocity hold
// negative sign because nose over to increase speed
float thetaCmd = _theLimit.update(-_v2Theta.update(
_vLimit.update(_vCmd.get()) - v));
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd
float rCmd = 0;
// stabilization
float velocityRatio = _trimV.get()/v;
float outputScale = velocityRatio*velocityRatio;
// this term scales the output based on the dynamic pressure change from trim
_stabilization.update(pCmd, qCmd, rCmd,
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
_att.rollspeed, _att.pitchspeed, _att.yawspeed,
outputScale);
// output
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
@ -280,7 +291,12 @@ void BlockMultiModeBacksideAutopilot::update()
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
// calculate velocity, XXX should be airspeed, but using ground speed for now
float v = sqrtf(_pos.vx * _pos.vx + _pos.vy * _pos.vy + _pos.vz * _pos.vz);
// for the purpose of control we will limit the velocity feedback between
// the min/max velocity
float v = _vLimit.update(sqrtf(
_pos.vx * _pos.vx +
_pos.vy * _pos.vy +
_pos.vz * _pos.vz));
// pitch channel -> rate of climb
// TODO, might want to put a gain on this, otherwise commanding
@ -294,8 +310,10 @@ void BlockMultiModeBacksideAutopilot::update()
// throttle channel -> velocity
// negative sign because nose over to increase speed
float vCmd = _manual.throttle * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
float vCmd = _vLimit.update(_manual.throttle *
(_vLimit.getMax() - _vLimit.getMin()) +
_vLimit.getMin());
float thetaCmd = _theLimit.update(-_v2Theta.update(vCmd - v));
float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
// yaw rate cmd

6
apps/controllib/fixedwing.hpp
View File

@ -193,7 +193,7 @@ public:
* good idea to declare a member to store the temporary
* variable.
*/
void update(float rCmd, float r);
void update(float rCmd, float r, float outputScale = 1.0);
/**
* Rudder output value accessor
@ -226,7 +226,8 @@ public:
BlockStabilization(SuperBlock *parent, const char *name);
virtual ~BlockStabilization();
void update(float pCmd, float qCmd, float rCmd,
float p, float q, float r);
float p, float q, float r,
float outputScale = 1.0);
float getAileron() { return _aileron; }
float getElevator() { return _elevator; }
float getRudder() { return _yawDamper.getRudder(); }
@ -322,6 +323,7 @@ private:
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _rocMax;

3
apps/examples/control_demo/params.c
View File

@ -68,4 +68,5 @@ PARAM_DEFINE_FLOAT(FWB_ROC2THR_D, 0.0f);
PARAM_DEFINE_FLOAT(FWB_ROC2THR_D_LP, 0.0f);
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I_MAX, 0.0f);
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)
PARAM_DEFINE_FLOAT(FWB_TRIM_V, 12.0f); // trim velocity, m/s