diff options
-rw-r--r-- | apps/examples/control_demo/params.c | 12 | ||||
-rw-r--r-- | apps/examples/kalman_demo/KalmanNav.cpp | 17 | ||||
-rw-r--r-- | apps/examples/kalman_demo/params.c | 16 |
3 files changed, 23 insertions, 22 deletions
diff --git a/apps/examples/control_demo/params.c b/apps/examples/control_demo/params.c index 422e9b90e..3eed83b5c 100644 --- a/apps/examples/control_demo/params.c +++ b/apps/examples/control_demo/params.c @@ -15,16 +15,16 @@ PARAM_DEFINE_FLOAT(FWB_R_HP, 1.0f); // yaw rate high pass // stabilization mode PARAM_DEFINE_FLOAT(FWB_P2AIL, 0.5f); // roll rate 2 aileron -PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.5f); // pitch rate 2 elevator +PARAM_DEFINE_FLOAT(FWB_Q2ELV, 0.2f); // pitch rate 2 elevator PARAM_DEFINE_FLOAT(FWB_R2RDR, 0.2f); // yaw rate 2 rudder // psi -> phi -> p PARAM_DEFINE_FLOAT(FWB_PSI2PHI, 0.5f); // heading 2 roll PARAM_DEFINE_FLOAT(FWB_PHI2P, 1.0f); // roll to roll rate -PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.5f); // roll limit, 28 deg +PARAM_DEFINE_FLOAT(FWB_PHI_LIM_MAX, 0.3f); // roll limit, 28 deg // velocity -> theta -PARAM_DEFINE_FLOAT(FWB_V2THE_P, 0.1f); // velocity to pitch angle PID, prop gain +PARAM_DEFINE_FLOAT(FWB_V2THE_P, 1.0f); // velocity to pitch angle PID, prop gain PARAM_DEFINE_FLOAT(FWB_V2THE_I, 0.0f); // integral gain PARAM_DEFINE_FLOAT(FWB_V2THE_D, 0.0f); // derivative gain PARAM_DEFINE_FLOAT(FWB_V2THE_D_LP, 0.0f); // derivative low-pass @@ -34,7 +34,7 @@ PARAM_DEFINE_FLOAT(FWB_THE_MAX, 0.5f); // the min commanded pitch angle // theta -> q -PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 2.0f); // pitch angle to pitch-rate PID +PARAM_DEFINE_FLOAT(FWB_THE2Q_P, 1.0f); // pitch angle to pitch-rate PID PARAM_DEFINE_FLOAT(FWB_THE2Q_I, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D, 0.0f); PARAM_DEFINE_FLOAT(FWB_THE2Q_D_LP, 0.0f); @@ -49,7 +49,7 @@ PARAM_DEFINE_FLOAT(FWB_H2THR_I_MAX, 0.0f); // crosstrack PARAM_DEFINE_FLOAT(FWB_XT2YAW_MAX, 1.57f); // cross-track to yaw angle limit 90 deg -PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.002f); // cross-track to yaw angle gain +PARAM_DEFINE_FLOAT(FWB_XT2YAW, 0.005f); // cross-track to yaw angle gain // speed command PARAM_DEFINE_FLOAT(FWB_V_MIN, 20.0f); // minimum commanded velocity @@ -60,4 +60,4 @@ PARAM_DEFINE_FLOAT(FWB_V_MAX, 24.0f); // maximum commanded velocity PARAM_DEFINE_FLOAT(FWB_TRIM_AIL, 0.0f); // trim aileron, normalized (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_ELV, 0.005f); // trim elevator (-1,1) PARAM_DEFINE_FLOAT(FWB_TRIM_RDR, 0.0f); // trim rudder (-1,1) -PARAM_DEFINE_FLOAT(FWB_TRIM_THR, 0.81f); // trim throttle (0,1) +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 d93a7bdc6..6acf39be8 100644 --- a/apps/examples/kalman_demo/KalmanNav.cpp +++ b/apps/examples/kalman_demo/KalmanNav.cpp @@ -66,7 +66,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : q(), // subscriptions _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 1000), // limit to 1 Hz + _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz // publications _pos(&getPublications(), ORB_ID(vehicle_global_position)), @@ -193,9 +193,10 @@ void KalmanNav::update() // prediciton step // using sensors timestamp so we can account for packet lag - float dt= (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; + float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; //printf("dt: %15.10f\n", double(dtFast)); _predictTimeStamp = _sensors.timestamp; + // don't predict if time greater than a second if (dt < 1.0f) { predictState(dt); @@ -213,7 +214,7 @@ void KalmanNav::update() } // attitude correction step - if (_sensors.timestamp - _attTimeStamp > 1e6 / 1) { // 1 Hz + if (_sensors.timestamp - _attTimeStamp > 1e6 / 20) { // 20 Hz _attTimeStamp = _sensors.timestamp; correctAtt(); } @@ -228,9 +229,9 @@ void KalmanNav::update() if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz _outTimeStamp = newTimeStamp; printf("nav: %4d Hz, miss #: %4d\n", - _navFrames / 10, _miss/ 10); + _navFrames / 10, _miss / 10); _navFrames = 0; - _miss= 0; + _miss = 0; } } @@ -325,11 +326,11 @@ void KalmanNav::predictState(float dt) float lDot = vE / (cosLSing * R); float rotRate = 2 * omega + lDot; float vNDot = fN - vE * rotRate * sinL + - vD * LDot; + vD * LDot; float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); + vN * LDot + _g.get(); float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; + vDDot * rotRate * cosL; // rectangular integration vN += vNDot * dt; diff --git a/apps/examples/kalman_demo/params.c b/apps/examples/kalman_demo/params.c index 58ebeba3c..77225b4c7 100644 --- a/apps/examples/kalman_demo/params.c +++ b/apps/examples/kalman_demo/params.c @@ -1,16 +1,16 @@ #include <systemlib/param/param.h> /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.1f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.1f); +PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.01f); +PARAM_DEFINE_FLOAT(KF_V_ACCEL, 0.01f); +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_ACCEL, 0.1f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 1.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.806f); +PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); +PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); +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); |