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

Merged with master

Browse Source
This commit is contained in:
Lorenz Meier
2013-05-09 15:52:36 +02:00
parent b944962a73 83ce24072f
commit 3152dae3dc
11 changed files with 1225 additions and 156 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/controllib/fixedwing.cpp
+41 -23
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) :
@@ -156,21 +156,21 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par
_theLimit(this, "THE"),
_vLimit(this, "V"),
// altitude/roc hold
// altitude/climb rate hold
_h2Thr(this, "H2THR"),
_roc2Thr(this, "ROC2THR"),
_cr2Thr(this, "CR2THR"),
// 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"),
_crMax(this, "CR_MAX"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
@@ -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,13 +291,18 @@ 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
// from +1 -> -1 m/s for rate of climb
//float dThrottle = _roc2Thr.update(
//_rocMax.get()*_manual.pitch - _pos.vz);
//float dThrottle = _cr2Thr.update(
//_crMax.get()*_manual.pitch - _pos.vz);
// roll channel -> bank angle
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
@@ -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
src/modules/controllib/fixedwing.hpp
+7 -5
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(); }
@@ -310,9 +311,9 @@ private:
BlockLimit _theLimit;
BlockLimit _vLimit;
// altitude/ roc hold
// altitude/ climb rate hold
BlockPID _h2Thr;
BlockPID _roc2Thr;
BlockPID _cr2Thr;
// guidance
BlockWaypointGuidance _guide;
@@ -322,8 +323,9 @@ private:
BlockParam<float> _trimElv;
BlockParam<float> _trimRdr;
BlockParam<float> _trimThr;
BlockParam<float> _trimV;
BlockParam<float> _vCmd;
BlockParam<float> _rocMax;
BlockParam<float> _crMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;