aboutsummaryrefslogtreecommitdiff
path: root/apps
diff options
context:
space:
mode:
authorjgoppert <james.goppert@gmail.com>2013-03-18 15:09:15 -0400
committerjgoppert <james.goppert@gmail.com>2013-03-18 15:09:15 -0400
commit3c6fbeb1a063130fc845e24fe69c68bf5bf2b128 (patch)
tree90c975198660e16975fdb9573bfa3f31f93e56eb /apps
parent3ea44e4a2921f0bfb21bc2abfdfeb40869b5cb0c (diff)
downloadpx4-firmware-3c6fbeb1a063130fc845e24fe69c68bf5bf2b128.tar.gz
px4-firmware-3c6fbeb1a063130fc845e24fe69c68bf5bf2b128.tar.bz2
px4-firmware-3c6fbeb1a063130fc845e24fe69c68bf5bf2b128.zip
Added seatbelt fixedwing controller.
Diffstat (limited to 'apps')
-rw-r--r--apps/controllib/fixedwing.cpp228
-rw-r--r--apps/controllib/fixedwing.hpp146
-rw-r--r--apps/examples/control_demo/params.c5
3 files changed, 134 insertions, 245 deletions
diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp
index 7f5711c47..3f9a70c5f 100644
--- a/apps/controllib/fixedwing.cpp
+++ b/apps/controllib/fixedwing.cpp
@@ -86,113 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
_yawDamper.update(rCmd, r);
}
-BlockHeadingHold::BlockHeadingHold(SuperBlock *parent, const char *name) :
- SuperBlock(parent, name),
- _psi2Phi(this, "PSI2PHI"),
- _phi2P(this, "PHI2P"),
- _phiLimit(this, "PHI_LIM")
-{
-}
-
-BlockHeadingHold::~BlockHeadingHold() {};
-
-float BlockHeadingHold::update(float psiCmd, float phi, float psi, float p)
-{
- float psiError = _wrap_pi(psiCmd - psi);
- float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
- return _phi2P.update(phiCmd - phi);
-}
-
-BlockVelocityHoldBackside::BlockVelocityHoldBackside(SuperBlock *parent, const char *name) :
- SuperBlock(parent, name),
- _v2Theta(this, "V2THE"),
- _theta2Q(this, "THE2Q"),
- _theLimit(this, "THE"),
- _vLimit(this, "V")
-{
-}
-
-BlockVelocityHoldBackside::~BlockVelocityHoldBackside() {};
-
-float BlockVelocityHoldBackside::update(float vCmd, float v, float theta, float q)
-{
- // negative sign because nose over to increase speed
- float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
- return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockVelocityHoldFrontside::BlockVelocityHoldFrontside(SuperBlock *parent, const char *name) :
- SuperBlock(parent, name),
- _v2Thr(this, "V2THR")
-{
-}
-
-BlockVelocityHoldFrontside::~BlockVelocityHoldFrontside() {};
-
-float BlockVelocityHoldFrontside::update(float vCmd, float v)
-{
- return _v2Thr.update(vCmd - v);
-}
-
-BlockAltitudeHoldBackside::BlockAltitudeHoldBackside(SuperBlock *parent, const char *name) :
- SuperBlock(parent, name),
- _h2Thr(this, "H2THR"),
- _throttle(0)
-{
-}
-
-BlockAltitudeHoldBackside::~BlockAltitudeHoldBackside() {};
-
-void BlockAltitudeHoldBackside::update(float hCmd, float h)
-{
- _throttle = _h2Thr.update(hCmd - h);
-}
-
-BlockAltitudeHoldFrontside::BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name) :
- SuperBlock(parent, name),
- _h2Theta(this, "H2THE"),
- _theta2Q(this, "THE2Q")
-{
-}
-
-BlockAltitudeHoldFrontside::~BlockAltitudeHoldFrontside() {};
-
-float BlockAltitudeHoldFrontside::update(float hCmd, float h, float theta, float q)
-{
- float thetaCmd = _h2Theta.update(hCmd - h);
- return _theta2Q.update(thetaCmd - theta);
-}
-
-BlockBacksideAutopilot::BlockBacksideAutopilot(SuperBlock *parent,
- const char *name,
- BlockStabilization *stabilization) :
- SuperBlock(parent, name),
- _stabilization(stabilization),
- _headingHold(this, ""),
- _velocityHold(this, ""),
- _altitudeHold(this, ""),
- _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) */
-{
-}
-
-BlockBacksideAutopilot::~BlockBacksideAutopilot() {};
-
-void BlockBacksideAutopilot::update(float hCmd, float vCmd, float rCmd, float psiCmd,
- float h, float v,
- float phi, float theta, float psi,
- float p, float q, float r)
-{
- _altitudeHold.update(hCmd, h);
- _stabilization->update(
- _headingHold.update(psiCmd, phi, psi, p),
- _velocityHold.update(vCmd, v, theta, q),
- rCmd,
- p, q, r);
-}
-
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
SuperBlock(parent, name),
_xtYawLimit(this, "XT2YAW"),
@@ -251,9 +144,33 @@ BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
BlockUorbEnabledAutopilot(parent, name),
_stabilization(this, ""), // no name needed, already unique
- _backsideAutopilot(this, "", &_stabilization),
+
+ // heading hold
+ _psi2Phi(this, "PSI2PHI"),
+ _phi2P(this, "PHI2P"),
+ _phiLimit(this, "PHI_LIM"),
+
+ // velocity hold
+ _v2Theta(this, "V2THE"),
+ _theta2Q(this, "THE2Q"),
+ _theLimit(this, "THE"),
+ _vLimit(this, "V"),
+
+ // altitude/roc hold
+ _h2Thr(this, "H2THR"),
+ _roc2Thr(this, "H2THR"),
+
+ // 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) */
+
_vCmd(this, "V_CMD"),
+ _rocMax(this, "ROC_MAX"),
_attPoll(),
_lastPosCmd(),
_timeStamp(0)
@@ -305,7 +222,7 @@ void BlockMultiModeBacksideAutopilot::update()
// handle autopilot modes
if (_status.state_machine == SYSTEM_STATE_AUTO ||
- _status.state_machine == SYSTEM_STATE_STABILIZED) {
+ _status.state_machine == SYSTEM_STATE_STABILIZED) {
// update guidance
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
@@ -313,19 +230,32 @@ void BlockMultiModeBacksideAutopilot::update()
// 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);
- // commands
+ // altitude hold
+ float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+
+ // heading hold
+ float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
+ float phiCmd = _phiLimit.update(_psi2Phi.update(psiError));
+ float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+ // velocity hold
+ // negative sign because nose over to increase speed
+ float thetaCmd = _theLimit.update(-_v2Theta.update(
+ _vLimit.update(_vCmd.get()) - v));
+ float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+ // yaw rate cmd
float rCmd = 0;
- _backsideAutopilot.update(
- _posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
- _pos.alt, v,
- _att.roll, _att.pitch, _att.yaw,
- _att.rollspeed, _att.pitchspeed, _att.yawspeed
- );
- _actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
- _actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
- _actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
- _actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
+ // stabilization
+ _stabilization.update(pCmd, qCmd, rCmd,
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+ // output
+ _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+ _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+ _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+ _actuators.control[CH_THR] = dThrottle + _trimThr.get();
// XXX limit throttle to manual setting (safety) for now.
// If it turns out to be confusing, it can be removed later once
@@ -336,14 +266,12 @@ void BlockMultiModeBacksideAutopilot::update()
if (!_status.flag_hil_enabled) {
/* limit to value of manual throttle */
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
- _actuators.control[CH_THR] : _manual.throttle;
+ _actuators.control[CH_THR] : _manual.throttle;
}
-
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
-
_actuators.control[CH_AIL] = _manual.roll;
_actuators.control[CH_ELV] = _manual.pitch;
_actuators.control[CH_RDR] = _manual.yaw;
@@ -351,8 +279,60 @@ 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);
+
+ // throttle 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.throttle - _pos.vz);
+
+ // roll channel -> bank angle
+ float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
+ float pCmd = _phi2P.update(phiCmd - _att.roll);
+
+ // pitch channel -> velocity
+ // negative sign because nose over to increase speed
+ float vCmd = _manual.pitch * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin();
+ float thetaCmd = _theLimit.update(-_v2Theta.update(_vLimit.update(vCmd) - v));
+ float qCmd = _theta2Q.update(thetaCmd - _att.pitch);
+
+ // yaw rate cmd
+ float rCmd = 0;
+
+ // stabilization
+ _stabilization.update(pCmd, qCmd, rCmd,
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+
+ // output
+ _actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
+ _actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
+ _actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
+
+ // currenlty using manual throttle
+ // XXX if you enable this watch out, vz might be very noisy
+ //_actuators.control[CH_THR] = dThrottle + _trimThr.get();
+ _actuators.control[CH_THR] = _manual.throttle;
+
+ // XXX limit throttle to manual setting (safety) for now.
+ // If it turns out to be confusing, it can be removed later once
+ // a first binary release can be targeted.
+ // This is not a hack, but a design choice.
+
+ /* do not limit in HIL */
+ if (!_status.flag_hil_enabled) {
+ /* limit to value of manual throttle */
+ _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
+ _actuators.control[CH_THR] : _manual.throttle;
+ }
+ }
+
+ // body rates controller, disabled for now
+ else if (0 /*_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS*/) {
+
_stabilization.update(_manual.roll, _manual.pitch, _manual.yaw,
- _att.rollspeed, _att.pitchspeed, _att.yawspeed);
+ _att.rollspeed, _att.pitchspeed, _att.yawspeed);
_actuators.control[CH_AIL] = _stabilization.getAileron();
_actuators.control[CH_ELV] = _stabilization.getElevator();
diff --git a/apps/controllib/fixedwing.hpp b/apps/controllib/fixedwing.hpp
index ea742641c..281cbb4cb 100644
--- a/apps/controllib/fixedwing.hpp
+++ b/apps/controllib/fixedwing.hpp
@@ -233,26 +233,6 @@ public:
};
/**
- * Heading hold autopilot block.
- * Aircraft Control and Simulation, Stevens and Lewis
- * Heading hold, pg. 348
- */
-class __EXPORT BlockHeadingHold : public SuperBlock
-{
-private:
- BlockP _psi2Phi;
- BlockP _phi2P;
- BlockLimitSym _phiLimit;
-public:
- BlockHeadingHold(SuperBlock *parent, const char *name);
- virtual ~BlockHeadingHold();
- /**
- * returns pCmd
- */
- float update(float psiCmd, float phi, float psi, float p);
-};
-
-/**
* Frontside/ Backside Control Systems
*
* Frontside :
@@ -270,106 +250,6 @@ public:
*/
/**
- * Backside velocity hold autopilot block.
- * v -> theta -> q -> elevator
- */
-class __EXPORT BlockVelocityHoldBackside : public SuperBlock
-{
-private:
- BlockPID _v2Theta;
- BlockPID _theta2Q;
- BlockLimit _theLimit;
- BlockLimit _vLimit;
-public:
- BlockVelocityHoldBackside(SuperBlock *parent, const char *name);
- virtual ~BlockVelocityHoldBackside();
- /**
- * returns qCmd
- */
- float update(float vCmd, float v, float theta, float q);
-};
-
-/**
- * Frontside velocity hold autopilot block.
- * v -> throttle
- */
-class __EXPORT BlockVelocityHoldFrontside : public SuperBlock
-{
-private:
- BlockPID _v2Thr;
-public:
- BlockVelocityHoldFrontside(SuperBlock *parent, const char *name);
- virtual ~BlockVelocityHoldFrontside();
- /**
- * returns throttle
- */
- float update(float vCmd, float v);
-};
-
-/**
- * Backside altitude hold autopilot block.
- * h -> throttle
- */
-class __EXPORT BlockAltitudeHoldBackside : public SuperBlock
-{
-private:
- BlockPID _h2Thr;
- float _throttle;
-public:
- BlockAltitudeHoldBackside(SuperBlock *parent, const char *name);
- virtual ~BlockAltitudeHoldBackside();
- void update(float hCmd, float h);
- float getThrottle() { return _throttle; }
-};
-
-/**
- * Frontside altitude hold autopilot block.
- * h -> theta > q -> elevator
- */
-class __EXPORT BlockAltitudeHoldFrontside : public SuperBlock
-{
-private:
- BlockPID _h2Theta;
- BlockPID _theta2Q;
-public:
- BlockAltitudeHoldFrontside(SuperBlock *parent, const char *name);
- virtual ~BlockAltitudeHoldFrontside();
- /**
- * return qCmd
- */
- float update(float hCmd, float h, float theta, float q);
-};
-
-/**
- * Backside autopilot
- */
-class __EXPORT BlockBacksideAutopilot : public SuperBlock
-{
-private:
- BlockStabilization *_stabilization;
- BlockHeadingHold _headingHold;
- BlockVelocityHoldBackside _velocityHold;
- BlockAltitudeHoldBackside _altitudeHold;
- BlockParam<float> _trimAil;
- BlockParam<float> _trimElv;
- BlockParam<float> _trimRdr;
- BlockParam<float> _trimThr;
-public:
- BlockBacksideAutopilot(SuperBlock *parent,
- const char *name,
- BlockStabilization *stabilization);
- virtual ~BlockBacksideAutopilot();
- void update(float hCmd, float vCmd, float rCmd, float psiCmd,
- float h, float v,
- float phi, float theta, float psi,
- float p, float q, float r);
- float getRudder() { return _stabilization->getRudder() + _trimRdr.get(); }
- float getAileron() { return _stabilization->getAileron() + _trimAil.get(); }
- float getElevator() { return _stabilization->getElevator() + _trimElv.get(); }
- float getThrottle() { return _altitudeHold.getThrottle() + _trimThr.get(); }
-};
-
-/**
* Waypoint Guidance block
*/
class __EXPORT BlockWaypointGuidance : public SuperBlock
@@ -416,10 +296,34 @@ public:
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
{
private:
+ // stabilization
BlockStabilization _stabilization;
- BlockBacksideAutopilot _backsideAutopilot;
+
+ // heading hold
+ BlockP _psi2Phi;
+ BlockP _phi2P;
+ BlockLimitSym _phiLimit;
+
+ // velocity hold
+ BlockPID _v2Theta;
+ BlockPID _theta2Q;
+ BlockLimit _theLimit;
+ BlockLimit _vLimit;
+
+ // altitude/ roc hold
+ BlockPID _h2Thr;
+ BlockPID _roc2Thr;
+
+ // guidance
BlockWaypointGuidance _guide;
+
+ // block params
+ BlockParam<float> _trimAil;
+ BlockParam<float> _trimElv;
+ BlockParam<float> _trimRdr;
+ BlockParam<float> _trimThr;
BlockParam<float> _vCmd;
+ BlockParam<float> _rocMax;
struct pollfd _attPoll;
vehicle_global_position_setpoint_s _lastPosCmd;
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c
index 83e01e63c..8db61d4d4 100644
--- a/apps/examples/control_demo/params.c
+++ b/apps/examples/control_demo/params.c
@@ -56,4 +56,9 @@ PARAM_DEFINE_FLOAT(FWB_V_MIN, 10.0f); // minimum commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_CMD, 12.0f); // commanded velocity
PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity
+// rate of climb
+// this is what rate of climb is commanded (in m/s)
+// when the throttle stick is fully defelcted in simple mode
+PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
+
PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.8f); // trim throttle (0,1)