diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index c1f8380e6a..8fa41e1502 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -306,9 +306,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) q[0] = 1e1f; q[1] = 1e1f; q[2] = 1e1f; - q[3] = 1e-6f; - q[4] = 1e-6f; - q[5] = 1e-6f; + /* process noise gyro offset covariance */ + q[3] = 1e-4f; + q[4] = 1e-4f; + q[5] = 1e-4f; q[6] = 1e-1f; q[7] = 1e-1f; q[8] = 1e-1f; @@ -347,6 +348,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) continue; } + dt = 0.004f; + uint64_t timing_start = hrt_absolute_time(); // attitudeKalmanfilter(dt, update_vect, z_k, &z_k_sizes, u, x_aposteriori_k, P_aposteriori_k, knownConst, euler, // Rot_matrix, x_aposteriori, P_aposteriori); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 117387f369..3b1d2d0735 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -1708,9 +1708,9 @@ int mavlink_thread_main(int argc, char *argv[]) set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_HIGHRES_IMU, 20); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_RAW_IMU, 20); /* 50 Hz / 20 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_ATTITUDE, 30); /* 20 Hz / 50 ms */ - set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 50); + set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_NAMED_VALUE_FLOAT, 10); set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, 50); /* 2 Hz */ set_mavlink_interval_limit(&mavlink_subs, MAVLINK_MSG_ID_MANUAL_CONTROL, 100);