forked from Archive/PX4-Autopilot
Merge branch 'seatbelt_controllib' of https://github.com/jgoppert/Firmware into seatbelt
This commit is contained in:
commit
f43a74c6ca
|
@ -86,113 +86,6 @@ void BlockStabilization::update(float pCmd, float qCmd, float rCmd,
|
||||||
_yawDamper.update(rCmd, r);
|
_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) :
|
BlockWaypointGuidance::BlockWaypointGuidance(SuperBlock *parent, const char *name) :
|
||||||
SuperBlock(parent, name),
|
SuperBlock(parent, name),
|
||||||
_xtYawLimit(this, "XT2YAW"),
|
_xtYawLimit(this, "XT2YAW"),
|
||||||
|
@ -251,9 +144,33 @@ BlockUorbEnabledAutopilot::~BlockUorbEnabledAutopilot() {};
|
||||||
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
|
BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *parent, const char *name) :
|
||||||
BlockUorbEnabledAutopilot(parent, name),
|
BlockUorbEnabledAutopilot(parent, name),
|
||||||
_stabilization(this, ""), // no name needed, already unique
|
_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, "ROC2THR"),
|
||||||
|
|
||||||
|
// guidance block
|
||||||
_guide(this, ""),
|
_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"),
|
_vCmd(this, "V_CMD"),
|
||||||
|
_rocMax(this, "ROC_MAX"),
|
||||||
_attPoll(),
|
_attPoll(),
|
||||||
_lastPosCmd(),
|
_lastPosCmd(),
|
||||||
_timeStamp(0)
|
_timeStamp(0)
|
||||||
|
@ -305,7 +222,7 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
|
||||||
// handle autopilot modes
|
// handle autopilot modes
|
||||||
if (_status.state_machine == SYSTEM_STATE_AUTO ||
|
if (_status.state_machine == SYSTEM_STATE_AUTO ||
|
||||||
_status.state_machine == SYSTEM_STATE_STABILIZED) {
|
_status.state_machine == SYSTEM_STATE_STABILIZED) {
|
||||||
|
|
||||||
// update guidance
|
// update guidance
|
||||||
_guide.update(_pos, _att, _posCmd, _lastPosCmd);
|
_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
|
// 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);
|
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;
|
float rCmd = 0;
|
||||||
|
|
||||||
_backsideAutopilot.update(
|
// stabilization
|
||||||
_posCmd.altitude, _vCmd.get(), rCmd, _guide.getPsiCmd(),
|
_stabilization.update(pCmd, qCmd, rCmd,
|
||||||
_pos.alt, v,
|
_att.rollspeed, _att.pitchspeed, _att.yawspeed);
|
||||||
_att.roll, _att.pitch, _att.yaw,
|
|
||||||
_att.rollspeed, _att.pitchspeed, _att.yawspeed
|
// output
|
||||||
);
|
_actuators.control[CH_AIL] = _stabilization.getAileron() + _trimAil.get();
|
||||||
_actuators.control[CH_AIL] = _backsideAutopilot.getAileron();
|
_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
|
||||||
_actuators.control[CH_ELV] = _backsideAutopilot.getElevator();
|
_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
|
||||||
_actuators.control[CH_RDR] = _backsideAutopilot.getRudder();
|
_actuators.control[CH_THR] = dThrottle + _trimThr.get();
|
||||||
_actuators.control[CH_THR] = _backsideAutopilot.getThrottle();
|
|
||||||
|
|
||||||
// XXX limit throttle to manual setting (safety) for now.
|
// XXX limit throttle to manual setting (safety) for now.
|
||||||
// If it turns out to be confusing, it can be removed later once
|
// 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) {
|
if (!_status.flag_hil_enabled) {
|
||||||
/* limit to value of manual throttle */
|
/* limit to value of manual throttle */
|
||||||
_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _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) {
|
} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
|
||||||
|
|
||||||
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
|
||||||
|
|
||||||
_actuators.control[CH_AIL] = _manual.roll;
|
_actuators.control[CH_AIL] = _manual.roll;
|
||||||
_actuators.control[CH_ELV] = _manual.pitch;
|
_actuators.control[CH_ELV] = _manual.pitch;
|
||||||
_actuators.control[CH_RDR] = _manual.yaw;
|
_actuators.control[CH_RDR] = _manual.yaw;
|
||||||
|
@ -351,8 +279,60 @@ void BlockMultiModeBacksideAutopilot::update()
|
||||||
|
|
||||||
} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
|
} 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);
|
||||||
|
|
||||||
|
// 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);
|
||||||
|
|
||||||
|
// roll channel -> bank angle
|
||||||
|
float phiCmd = _phiLimit.update(_manual.roll * _phiLimit.getMax());
|
||||||
|
float pCmd = _phi2P.update(phiCmd - _att.roll);
|
||||||
|
|
||||||
|
// 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 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,
|
_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_AIL] = _stabilization.getAileron();
|
||||||
_actuators.control[CH_ELV] = _stabilization.getElevator();
|
_actuators.control[CH_ELV] = _stabilization.getElevator();
|
||||||
|
|
|
@ -232,26 +232,6 @@ public:
|
||||||
float getRudder() { return _yawDamper.getRudder(); }
|
float getRudder() { return _yawDamper.getRudder(); }
|
||||||
};
|
};
|
||||||
|
|
||||||
/**
|
|
||||||
* 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/ Backside Control Systems
|
||||||
*
|
*
|
||||||
|
@ -269,106 +249,6 @@ public:
|
||||||
* than frontside at high speeds.
|
* than frontside at high speeds.
|
||||||
*/
|
*/
|
||||||
|
|
||||||
/**
|
|
||||||
* 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
|
* Waypoint Guidance block
|
||||||
*/
|
*/
|
||||||
|
@ -416,10 +296,34 @@ public:
|
||||||
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
class __EXPORT BlockMultiModeBacksideAutopilot : public BlockUorbEnabledAutopilot
|
||||||
{
|
{
|
||||||
private:
|
private:
|
||||||
|
// stabilization
|
||||||
BlockStabilization _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;
|
BlockWaypointGuidance _guide;
|
||||||
|
|
||||||
|
// block params
|
||||||
|
BlockParam<float> _trimAil;
|
||||||
|
BlockParam<float> _trimElv;
|
||||||
|
BlockParam<float> _trimRdr;
|
||||||
|
BlockParam<float> _trimThr;
|
||||||
BlockParam<float> _vCmd;
|
BlockParam<float> _vCmd;
|
||||||
|
BlockParam<float> _rocMax;
|
||||||
|
|
||||||
struct pollfd _attPoll;
|
struct pollfd _attPoll;
|
||||||
vehicle_global_position_setpoint_s _lastPosCmd;
|
vehicle_global_position_setpoint_s _lastPosCmd;
|
||||||
|
|
|
@ -56,4 +56,16 @@ 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_CMD, 12.0f); // commanded velocity
|
||||||
PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum 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 pitch stick is fully defelcted in simple mode
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f);
|
||||||
|
|
||||||
|
// rate of climb -> thr
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // rate of climb to throttle PID
|
||||||
|
PARAM_DEFINE_FLOAT(FWB_ROC2THR_I, 0.0f);
|
||||||
|
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)
|
||||||
|
|
|
@ -61,8 +61,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||||
HAtt(6, 9),
|
HAtt(6, 9),
|
||||||
RAtt(6, 6),
|
RAtt(6, 6),
|
||||||
// position measurement ekf matrices
|
// position measurement ekf matrices
|
||||||
HPos(5, 9),
|
HPos(6, 9),
|
||||||
RPos(5, 5),
|
RPos(6, 6),
|
||||||
// attitude representations
|
// attitude representations
|
||||||
C_nb(),
|
C_nb(),
|
||||||
q(),
|
q(),
|
||||||
|
@ -95,6 +95,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||||
_rGpsVel(this, "R_GPS_VEL"),
|
_rGpsVel(this, "R_GPS_VEL"),
|
||||||
_rGpsPos(this, "R_GPS_POS"),
|
_rGpsPos(this, "R_GPS_POS"),
|
||||||
_rGpsAlt(this, "R_GPS_ALT"),
|
_rGpsAlt(this, "R_GPS_ALT"),
|
||||||
|
_rPressAlt(this, "R_PRESS_ALT"),
|
||||||
_rAccel(this, "R_ACCEL"),
|
_rAccel(this, "R_ACCEL"),
|
||||||
_magDip(this, "ENV_MAG_DIP"),
|
_magDip(this, "ENV_MAG_DIP"),
|
||||||
_magDec(this, "ENV_MAG_DEC"),
|
_magDec(this, "ENV_MAG_DEC"),
|
||||||
|
@ -134,6 +135,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
|
||||||
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
|
HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F;
|
||||||
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
|
HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F;
|
||||||
HPos(4, 8) = 1.0f;
|
HPos(4, 8) = 1.0f;
|
||||||
|
HPos(5, 8) = 1.0f;
|
||||||
|
|
||||||
// initialize all parameters
|
// initialize all parameters
|
||||||
updateParams();
|
updateParams();
|
||||||
|
@ -192,7 +194,7 @@ void KalmanNav::update()
|
||||||
gpsUpdate &&
|
gpsUpdate &&
|
||||||
_gps.fix_type > 2
|
_gps.fix_type > 2
|
||||||
//&& _gps.counter_pos_valid > 10
|
//&& _gps.counter_pos_valid > 10
|
||||||
) {
|
) {
|
||||||
vN = _gps.vel_n_m_s;
|
vN = _gps.vel_n_m_s;
|
||||||
vE = _gps.vel_e_m_s;
|
vE = _gps.vel_e_m_s;
|
||||||
vD = _gps.vel_d_m_s;
|
vD = _gps.vel_d_m_s;
|
||||||
|
@ -630,12 +632,13 @@ int KalmanNav::correctPos()
|
||||||
using namespace math;
|
using namespace math;
|
||||||
|
|
||||||
// residual
|
// residual
|
||||||
Vector y(5);
|
Vector y(6);
|
||||||
y(0) = _gps.vel_n_m_s - vN;
|
y(0) = _gps.vel_n_m_s - vN;
|
||||||
y(1) = _gps.vel_e_m_s - vE;
|
y(1) = _gps.vel_e_m_s - vE;
|
||||||
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
|
||||||
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
|
||||||
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
y(4) = double(_gps.alt) / 1.0e3 - alt;
|
||||||
|
y(5) = double(_sensors.baro_alt_meter) - alt;
|
||||||
|
|
||||||
// compute correction
|
// compute correction
|
||||||
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
// http://en.wikipedia.org/wiki/Extended_Kalman_filter
|
||||||
|
@ -685,7 +688,8 @@ int KalmanNav::correctPos()
|
||||||
double(y(1) / sqrtf(RPos(1, 1))),
|
double(y(1) / sqrtf(RPos(1, 1))),
|
||||||
double(y(2) / sqrtf(RPos(2, 2))),
|
double(y(2) / sqrtf(RPos(2, 2))),
|
||||||
double(y(3) / sqrtf(RPos(3, 3))),
|
double(y(3) / sqrtf(RPos(3, 3))),
|
||||||
double(y(4) / sqrtf(RPos(4, 4))));
|
double(y(4) / sqrtf(RPos(4, 4))),
|
||||||
|
double(y(5) / sqrtf(RPos(5, 5))));
|
||||||
}
|
}
|
||||||
|
|
||||||
return ret_ok;
|
return ret_ok;
|
||||||
|
@ -740,7 +744,8 @@ void KalmanNav::updateParams()
|
||||||
float noiseVel = _rGpsVel.get();
|
float noiseVel = _rGpsVel.get();
|
||||||
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
|
float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R;
|
||||||
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
|
float noiseLonDegE7 = noiseLatDegE7 / cosLSing;
|
||||||
float noiseAlt = _rGpsAlt.get();
|
float noiseGpsAlt = _rGpsAlt.get();
|
||||||
|
float noisePressAlt = _rPressAlt.get();
|
||||||
|
|
||||||
// bound noise to prevent singularities
|
// bound noise to prevent singularities
|
||||||
if (noiseVel < noiseMin) noiseVel = noiseMin;
|
if (noiseVel < noiseMin) noiseVel = noiseMin;
|
||||||
|
@ -749,13 +754,16 @@ void KalmanNav::updateParams()
|
||||||
|
|
||||||
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
|
if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin;
|
||||||
|
|
||||||
if (noiseAlt < noiseMin) noiseAlt = noiseMin;
|
if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin;
|
||||||
|
|
||||||
|
if (noisePressAlt < noiseMin) noisePressAlt = noiseMin;
|
||||||
|
|
||||||
RPos(0, 0) = noiseVel * noiseVel; // vn
|
RPos(0, 0) = noiseVel * noiseVel; // vn
|
||||||
RPos(1, 1) = noiseVel * noiseVel; // ve
|
RPos(1, 1) = noiseVel * noiseVel; // ve
|
||||||
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
|
RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat
|
||||||
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
|
RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon
|
||||||
RPos(4, 4) = noiseAlt * noiseAlt; // h
|
RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h
|
||||||
|
RPos(5, 5) = noisePressAlt * noisePressAlt; // h
|
||||||
// XXX, note that RPos depends on lat, so updateParams should
|
// XXX, note that RPos depends on lat, so updateParams should
|
||||||
// be called if lat changes significantly
|
// be called if lat changes significantly
|
||||||
}
|
}
|
||||||
|
|
|
@ -159,6 +159,7 @@ protected:
|
||||||
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
|
control::BlockParam<float> _rGpsVel; /**< gps velocity measurement noise */
|
||||||
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
|
control::BlockParam<float> _rGpsPos; /**< gps position measurement noise */
|
||||||
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
|
control::BlockParam<float> _rGpsAlt; /**< gps altitude measurement noise */
|
||||||
|
control::BlockParam<float> _rPressAlt; /**< press altitude measurement noise */
|
||||||
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
|
control::BlockParam<float> _rAccel; /**< accelerometer measurement noise */
|
||||||
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
|
control::BlockParam<float> _magDip; /**< magnetic inclination with level */
|
||||||
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
|
control::BlockParam<float> _magDec; /**< magnetic declination, clockwise rotation */
|
||||||
|
|
|
@ -7,6 +7,7 @@ PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
|
||||||
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
|
||||||
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
|
PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
|
||||||
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
|
PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
|
||||||
|
PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
|
||||||
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
|
PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
|
||||||
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
|
PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
|
||||||
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
|
PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);
|
||||||
|
|
Loading…
Reference in New Issue