From 3c6fbeb1a063130fc845e24fe69c68bf5bf2b128 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 18 Mar 2013 15:09:15 -0400 Subject: Added seatbelt fixedwing controller. --- apps/controllib/fixedwing.cpp | 228 ++++++++++++++++-------------------- apps/controllib/fixedwing.hpp | 146 ++++------------------- apps/examples/control_demo/params.c | 5 + 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 @@ -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 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) -- cgit v1.2.3 From 90f9b154eb895aaf321819b00948653b0de5d407 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 18 Mar 2013 15:18:08 -0400 Subject: Flipped simple mode pitch/throttle channel input. --- apps/controllib/fixedwing.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 3f9a70c5f..bbf87533e 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -282,19 +282,19 @@ 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); - // throttle channel -> rate of climb + // 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.throttle - _pos.vz); + //_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); - // pitch channel -> velocity + // throttle channel -> velocity // negative sign because nose over to increase speed - float vCmd = _manual.pitch * (_vLimit.getMax() - _vLimit.getMin()) + _vLimit.getMin(); + 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); -- cgit v1.2.3 From 9c9873bcc112a604d6c43888bf121b27d101317b Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 18 Mar 2013 16:18:58 -0400 Subject: Fixed PID roc2Thr param names/ added them to controldemo. --- apps/controllib/fixedwing.cpp | 2 +- apps/examples/control_demo/params.c | 9 ++++++++- 2 files changed, 9 insertions(+), 2 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index bbf87533e..1cc28b9dd 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -158,7 +158,7 @@ BlockMultiModeBacksideAutopilot::BlockMultiModeBacksideAutopilot(SuperBlock *par // altitude/roc hold _h2Thr(this, "H2THR"), - _roc2Thr(this, "H2THR"), + _roc2Thr(this, "ROC2THR"), // guidance block _guide(this, ""), diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 8db61d4d4..527709629 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -58,7 +58,14 @@ 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 +// 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); // altitude 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) -- cgit v1.2.3 From b9e53b4ab4ff8184d8f67569c9d001347f25ffd1 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 18 Mar 2013 16:22:50 -0400 Subject: Fixed a comment. --- apps/examples/control_demo/params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 527709629..428b779b1 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -62,7 +62,7 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 16.0f); // maximum commanded velocity PARAM_DEFINE_FLOAT(FWB_ROC_MAX, 1.0f); // rate of climb -> thr -PARAM_DEFINE_FLOAT(FWB_ROC2THR_P, 0.01f); // altitude to throttle PID +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); -- cgit v1.2.3 From 8025a54d77c3f839864b90a4fa6305387e3d4bc0 Mon Sep 17 00:00:00 2001 From: jgoppert Date: Mon, 18 Mar 2013 16:44:53 -0400 Subject: Added pressure alt measurement to kalman demo. --- apps/examples/kalman_demo/KalmanNav.cpp | 24 ++++++++++++++++-------- apps/examples/kalman_demo/KalmanNav.hpp | 1 + apps/examples/kalman_demo/params.c | 1 + 3 files changed, 18 insertions(+), 8 deletions(-) diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp index 955e77b3e..6df454a55 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 7709ac697..c2bf18115 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 3bfe01261..91e80a5ea 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); -- cgit v1.2.3 From 70a85739ccf2df6f032093ee9b0f03666d5a241c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 21 Mar 2013 10:14:34 +0100 Subject: Added startup scripts useful when running USB consoles, made MAVLink aware that /dev/console is a hint for running on USB (magic strings, magic strings) --- ROMFS/Makefile | 2 ++ ROMFS/scripts/rc.hil | 57 ++++++++++++++++++++++++++++++++++++++++++++++++++ ROMFS/scripts/rc.usb | 14 +++++++++++++ apps/mavlink/mavlink.c | 3 ++- 4 files changed, 75 insertions(+), 1 deletion(-) create mode 100644 ROMFS/scripts/rc.hil create mode 100644 ROMFS/scripts/rc.usb diff --git a/ROMFS/Makefile b/ROMFS/Makefile index 15167bf47..ed39ab825 100644 --- a/ROMFS/Makefile +++ b/ROMFS/Makefile @@ -22,6 +22,8 @@ ROMFS_FSSPEC := $(SRCROOT)/scripts/rcS~init.d/rcS \ $(SRCROOT)/scripts/rc.PX4IO~init.d/rc.PX4IO \ $(SRCROOT)/scripts/rc.PX4IOAR~init.d/rc.PX4IOAR \ $(SRCROOT)/scripts/rc.FMU_quad_x~init.d/rc.FMU_quad_x \ + $(SRCROOT)/scripts/rc.usb~init.d/rc.usb \ + $(SRCROOT)/scripts/rc.hil~init.d/rc.hil \ $(SRCROOT)/mixers/FMU_pass.mix~mixers/FMU_pass.mix \ $(SRCROOT)/mixers/FMU_Q.mix~mixers/FMU_Q.mix \ $(SRCROOT)/mixers/FMU_X5.mix~mixers/FMU_X5.mix \ diff --git a/ROMFS/scripts/rc.hil b/ROMFS/scripts/rc.hil new file mode 100644 index 000000000..3b37ac26b --- /dev/null +++ b/ROMFS/scripts/rc.hil @@ -0,0 +1,57 @@ +#!nsh +# +# USB HIL start +# + +echo "[HIL] starting.." + +uorb start + +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/console + +# Create a fake HIL /dev/pwm_output interface +hil mode_pwm + +# +# Load microSD params +# +echo "[init] loading microSD params" +param select /fs/microsd/parameters +if [ -f /fs/microsd/parameters ] +then + param load /fs/microsd/parameters +fi + +# +# Force some key parameters to sane values +# MAV_TYPE 1 = fixed wing, 2 = quadrotor, 13 = hexarotor +# see https://pixhawk.ethz.ch/mavlink/ +# +param set MAV_TYPE 1 + +# +# Start the commander (depends on orb, mavlink) +# +commander start + +# +# Start the sensors (depends on orb, px4io) +# +sh /etc/init.d/rc.sensors + +# +# Start the attitude estimator (depends on orb) +# +kalman_demo start + +# +# Load mixer and start controllers (depends on px4io) +# +mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix +control_demo start + +echo "[HIL] setup done, running" + +# Exit shell to make it available to MAVLink +exit diff --git a/ROMFS/scripts/rc.usb b/ROMFS/scripts/rc.usb new file mode 100644 index 000000000..9031b38a2 --- /dev/null +++ b/ROMFS/scripts/rc.usb @@ -0,0 +1,14 @@ +#!nsh +# +# USB MAVLink start +# + +echo "[testing] doing production test.." + +# Tell MAVLink that this link is "fast" +mavlink start -b 230400 -d /dev/console + +# Exit shell to make it available to MAVLink +exit + +echo "[testing] testing done" diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b958d5f96..644b779af 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -442,7 +442,8 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf int termios_state; *is_usb = false; - if (strcmp(uart_name, "/dev/ttyACM0") != OK) { + /* make some wild guesses including that USB serial is indicated by either /dev/ttyACM0 or /dev/console */ + if (strcmp(uart_name, "/dev/ttyACM0") != OK && strcmp(uart_name, "/dev/console") != OK) { /* Back up the original uart configuration to restore it after exit */ if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { fprintf(stderr, "[mavlink] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state); -- cgit v1.2.3 From 4ae4a29a17b152c87f59f06b8b50761b7a3610c7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 22 Mar 2013 13:17:04 +0100 Subject: Fixed copy & paste documentation lies --- ROMFS/scripts/rc.usb | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/scripts/rc.usb b/ROMFS/scripts/rc.usb index 9031b38a2..31af3991a 100644 --- a/ROMFS/scripts/rc.usb +++ b/ROMFS/scripts/rc.usb @@ -3,12 +3,12 @@ # USB MAVLink start # -echo "[testing] doing production test.." +echo "Starting MAVLink on this USB console" # Tell MAVLink that this link is "fast" mavlink start -b 230400 -d /dev/console +echo "MAVLink started, exiting shell.." + # Exit shell to make it available to MAVLink exit - -echo "[testing] testing done" -- cgit v1.2.3 From 6eba7a9e41ca3e5fac9c3b5feb74af9f37774987 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Mar 2013 12:47:47 +0100 Subject: Fix gyro measurement noise variance --- apps/examples/kalman_demo/params.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 91e80a5ea..50642f067 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,7 +1,7 @@ #include /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 1.0f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f); PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f); @@ -14,4 +14,3 @@ PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); - -- cgit v1.2.3 From 9dbd2695d3b476e8ed0a2001b027329e8600bd29 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 28 Mar 2013 12:48:28 +0100 Subject: Hotfix missing yaw deadzone default (leads to continuous turns since zero speed is never commanded) --- apps/sensors/sensor_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index a1ef9d136..c850e3a1e 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -88,7 +88,7 @@ PARAM_DEFINE_FLOAT(RC4_MIN, 1000); PARAM_DEFINE_FLOAT(RC4_TRIM, 1500); PARAM_DEFINE_FLOAT(RC4_MAX, 2000); PARAM_DEFINE_FLOAT(RC4_REV, 1.0f); -PARAM_DEFINE_FLOAT(RC4_DZ, 0.0f); +PARAM_DEFINE_FLOAT(RC4_DZ, 30.0f); PARAM_DEFINE_FLOAT(RC5_MIN, 1000); PARAM_DEFINE_FLOAT(RC5_TRIM, 1500); -- cgit v1.2.3