From affa3af4e6415d1f68dd6242d0ded6e5ed8a1b1d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Oct 2012 13:39:26 +0200 Subject: Clean 250 Hz updates in filter, partial updates enabled --- .../attitude_estimator_ekf_main.c | 57 ++++++++++++++++------ 1 file changed, 41 insertions(+), 16 deletions(-) (limited to 'apps/attitude_estimator_ekf') diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 295cc4b54..6368c5157 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -161,7 +161,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) thread_should_exit = false; attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf", SCHED_RR, - SCHED_PRIORITY_DEFAULT, + SCHED_PRIORITY_MAX - 5, 20000, attitude_estimator_ekf_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); @@ -221,7 +221,7 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* subscribe to raw data */ int sub_raw = orb_subscribe(ORB_ID(sensor_combined)); /* rate-limit raw data updates to 200Hz */ - orb_set_interval(sub_raw, 5); + orb_set_interval(sub_raw, 4); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -236,6 +236,10 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) struct debug_key_value_s dbg = { .key = "", .value = 0.0f }; orb_advert_t pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); + /* keep track of sensor updates */ + uint32_t sensor_last_count[3] = {0, 0, 0}; + uint64_t sensor_last_timestamp[3] = {0, 0, 0}; + float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; /* process noise covariance */ float q[12]; @@ -264,17 +268,38 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; + uint8_t update_vect[3] = {0, 0, 0}; /* Fill in gyro measurements */ + if (sensor_last_count[0] != raw.gyro_counter) { + update_vect[0] = 1; + sensor_last_count[0] = raw.gyro_counter; + sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]); + sensor_last_timestamp[0] = raw.timestamp; + } + z_k[0] = raw.gyro_rad_s[0]; z_k[1] = raw.gyro_rad_s[1]; z_k[2] = raw.gyro_rad_s[2]; - /* scale from 14 bit to m/s2 */ + /* update accelerometer measurements */ + if (sensor_last_count[1] != raw.accelerometer_counter) { + update_vect[1] = 1; + sensor_last_count[1] = raw.accelerometer_counter; + sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]); + sensor_last_timestamp[1] = raw.timestamp; + } z_k[3] = raw.accelerometer_m_s2[0]; z_k[4] = raw.accelerometer_m_s2[1]; z_k[5] = raw.accelerometer_m_s2[2]; + /* update magnetometer measurements */ + if (sensor_last_count[2] != raw.magnetometer_counter) { + update_vect[2] = 1; + sensor_last_count[2] = raw.magnetometer_counter; + sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]); + sensor_last_timestamp[2] = raw.timestamp; + } z_k[6] = raw.magnetometer_ga[0]; z_k[7] = raw.magnetometer_ga[1]; z_k[8] = raw.magnetometer_ga[2]; @@ -293,7 +318,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) overloadcounter++; } - uint8_t update_vect[3] = {1, 1, 1}; int32_t z_k_sizes = 9; // float u[4] = {0.0f, 0.0f, 0.0f, 0.0f}; @@ -361,21 +385,22 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) uint64_t timing_diff = hrt_absolute_time() - timing_start; // /* print rotation matrix every 200th time */ - // if (printcounter % 200 == 0) { - // // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", - // // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], - // // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], - // // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); + if (printcounter % 200 == 0) { + // printf("x apo:\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n%8.4f\t%8.4f\t%8.4f\n", + // x_aposteriori[0], x_aposteriori[1], x_aposteriori[2], + // x_aposteriori[3], x_aposteriori[4], x_aposteriori[5], + // x_aposteriori[6], x_aposteriori[7], x_aposteriori[8]); - // // } + // } - // printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); - // printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); - // // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), - // // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), - // // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); - // } + printf("EKF attitude iteration: %d, runtime: %d us, dt: %d us (%d Hz)\n", loopcounter, (int)timing_diff, (int)(dt * 1000000.0f), (int)(1.0f / dt)); + printf("roll: %8.4f\tpitch: %8.4f\tyaw:%8.4f\n", (double)euler[0], (double)euler[1], (double)euler[2]); + printf("update rates gyro: %8.4f\taccel: %8.4f\tmag:%8.4f\n", (double)sensor_update_hz[0], (double)sensor_update_hz[1], (double)sensor_update_hz[2]); + // printf("\n%d\t%d\t%d\n%d\t%d\t%d\n%d\t%d\t%d\n", (int)(Rot_matrix[0] * 100), (int)(Rot_matrix[1] * 100), (int)(Rot_matrix[2] * 100), + // (int)(Rot_matrix[3] * 100), (int)(Rot_matrix[4] * 100), (int)(Rot_matrix[5] * 100), + // (int)(Rot_matrix[6] * 100), (int)(Rot_matrix[7] * 100), (int)(Rot_matrix[8] * 100)); + } // int i = printcounter % 9; -- cgit v1.2.3