diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-23 08:24:41 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-03-23 08:24:41 -0700 |
commit | 0dc96dbd891cf108a4ecc01539f5f710c6dd92e9 (patch) | |
tree | b6f9c60a81740455ddbb9d41a937bdd9617766f0 /apps/examples | |
parent | 8ca044047166a018d43a41969b885184c4442b8d (diff) | |
parent | f43a74c6ca1b8968d289e66dc9f0d138b60377d2 (diff) | |
download | px4-firmware-0dc96dbd891cf108a4ecc01539f5f710c6dd92e9.tar.gz px4-firmware-0dc96dbd891cf108a4ecc01539f5f710c6dd92e9.tar.bz2 px4-firmware-0dc96dbd891cf108a4ecc01539f5f710c6dd92e9.zip |
Merge pull request #234 from PX4/seatbelt
Seatbelt for fixed wing
Diffstat (limited to 'apps/examples')
-rw-r--r-- | apps/examples/control_demo/params.c | 12 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 24 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.hpp | 1 | ||||
-rw-r--r-- | apps/examples/kalman_demo/params.c | 1 |
4 files changed, 30 insertions, 8 deletions
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 83e01e63c..428b779b1 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 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<float> _rGpsVel; /**< gps velocity measurement noise */ control::BlockParam<float> _rGpsPos; /**< gps position 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> _magDip; /**< magnetic inclination with level */ control::BlockParam<float> _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); |