From 01eed6e946407ca6299a179e3d517ec2631ee9c5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 14 Nov 2012 10:41:44 +0100 Subject: Added perf counter, cleaned up --- .../attitude_estimator_ekf_main.c | 43 ++++------------------ 1 file changed, 8 insertions(+), 35 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 b25e61229..6533390bc 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -63,6 +63,7 @@ #include #include +#include #include #include "codegen/attitudeKalmanfilter_initialize.h" @@ -255,6 +256,9 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f }; unsigned offset_count = 0; + /* register the perf counter */ + perf_counter_t ekf_loop_perf = perf_alloc(PC_ELAPSED, "attitude_estimator_ekf"); + /* Main loop*/ while (!thread_should_exit) { @@ -306,6 +310,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } } else { + perf_begin(ekf_loop_perf); + /* Calculate data time difference in seconds */ dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; @@ -400,41 +406,6 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ continue; } - - // 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]); - - - // } - - //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; - - // // for (int i = 0; i < 9; i++) { - // char name[10]; - // sprintf(name, "xapo #%d", i); - // memcpy(dbg.key, name, sizeof(dbg.key)); - // dbg.value = x_aposteriori[i]; - // if (pub_dbg < 0) { - // pub_dbg = orb_advertise(ORB_ID(debug_key_value), &dbg); - // } else { - // orb_publish(ORB_ID(debug_key_value), pub_dbg, &dbg); - // } - - printcounter++; if (last_data > 0 && raw.timestamp - last_data > 12000) printf("[attitude estimator ekf] sensor data missed! (%llu)\n", raw.timestamp - last_data); @@ -463,6 +434,8 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) } else { warnx("NaN in roll/pitch/yaw estimate!"); } + + perf_end(ekf_loop_perf); } } } -- cgit v1.2.3