diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 7f5711c47d..1cc28b9dd9 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, "ROC2THR"), + + // 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); + + // 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, - _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 ea742641c6..281cbb4cb6 100644 --- a/apps/controllib/fixedwing.hpp +++ b/apps/controllib/fixedwing.hpp @@ -232,26 +232,6 @@ public: 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 * @@ -269,106 +249,6 @@ public: * 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 _trimAil; - BlockParam _trimElv; - BlockParam _trimRdr; - BlockParam _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 */ @@ -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 _trimAil; + BlockParam _trimElv; + BlockParam _trimRdr; + BlockParam _trimThr; BlockParam _vCmd; + BlockParam _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 83e01e63c8..428b779b17 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -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_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) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 955e77b3e4..6df454a555 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -61,8 +61,8 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : HAtt(6, 9), RAtt(6, 6), // position measurement ekf matrices - HPos(5, 9), - RPos(5, 5), + HPos(6, 9), + RPos(6, 6), // attitude representations C_nb(), q(), @@ -95,6 +95,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : _rGpsVel(this, "R_GPS_VEL"), _rGpsPos(this, "R_GPS_POS"), _rGpsAlt(this, "R_GPS_ALT"), + _rPressAlt(this, "R_PRESS_ALT"), _rAccel(this, "R_ACCEL"), _magDip(this, "ENV_MAG_DIP"), _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(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; HPos(4, 8) = 1.0f; + HPos(5, 8) = 1.0f; // initialize all parameters updateParams(); @@ -192,7 +194,7 @@ void KalmanNav::update() gpsUpdate && _gps.fix_type > 2 //&& _gps.counter_pos_valid > 10 - ) { + ) { vN = _gps.vel_n_m_s; vE = _gps.vel_e_m_s; vD = _gps.vel_d_m_s; @@ -630,12 +632,13 @@ int KalmanNav::correctPos() using namespace math; // residual - Vector y(5); + Vector y(6); y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; 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(4) = double(_gps.alt) / 1.0e3 - alt; + y(5) = double(_sensors.baro_alt_meter) - alt; // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -685,7 +688,8 @@ int KalmanNav::correctPos() double(y(1) / sqrtf(RPos(1, 1))), double(y(2) / sqrtf(RPos(2, 2))), 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; @@ -740,7 +744,8 @@ void KalmanNav::updateParams() float noiseVel = _rGpsVel.get(); float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseAlt = _rGpsAlt.get(); + float noiseGpsAlt = _rGpsAlt.get(); + float noisePressAlt = _rPressAlt.get(); // bound noise to prevent singularities if (noiseVel < noiseMin) noiseVel = noiseMin; @@ -749,13 +754,16 @@ void KalmanNav::updateParams() 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(1, 1) = noiseVel * noiseVel; // ve RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat 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 // be called if lat changes significantly } diff --git a/apps/examples/kalman_demo/KalmanNav.hpp b/apps/examples/kalman_demo/KalmanNav.hpp index 7709ac697c..c2bf18115d 100644 --- a/apps/examples/kalman_demo/KalmanNav.hpp +++ b/apps/examples/kalman_demo/KalmanNav.hpp @@ -159,6 +159,7 @@ protected: control::BlockParam _rGpsVel; /**< gps velocity measurement noise */ control::BlockParam _rGpsPos; /**< gps position measurement noise */ control::BlockParam _rGpsAlt; /**< gps altitude measurement noise */ + control::BlockParam _rPressAlt; /**< press altitude measurement noise */ control::BlockParam _rAccel; /**< accelerometer measurement noise */ control::BlockParam _magDip; /**< magnetic inclination with level */ control::BlockParam _magDec; /**< magnetic declination, clockwise rotation */ diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 3bfe012612..91e80a5eac 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -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_POS, 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_FAULT_POS, 10.0f); PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f);