diff options
Diffstat (limited to 'src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp')
| -rwxr-xr-x | src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp | 44 |
1 files changed, 14 insertions, 30 deletions
diff --git a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp index e79726613..e49027e47 100755 --- a/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp +++ b/src/modules/attitude_estimator_so3/attitude_estimator_so3_main.cpp @@ -1,8 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2013 PX4 Development Team. All rights reserved. - * Author: Hyon Lim <limhyon@gmail.com> - * Anton Babushkin <anton.babushkin@me.com> + * Copyright (c) 2013 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,6 +34,9 @@ /* * @file attitude_estimator_so3_main.cpp * + * @author Hyon Lim <limhyon@gmail.com> + * @author Anton Babushkin <anton.babushkin@me.com> + * * Implementation of nonlinear complementary filters on the SO(3). * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer. * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix. @@ -131,7 +132,7 @@ usage(const char *reason) * Makefile does only apply to this management task. * * The actual stack size should be set in the call - * to task_create(). + * to task_spawn_cmd(). */ int attitude_estimator_so3_main(int argc, char *argv[]) { @@ -391,8 +392,6 @@ void NonlinearSO3AHRSupdate(float gx, float gy, float gz, float ax, float ay, fl */ int attitude_estimator_so3_thread_main(int argc, char *argv[]) { - const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds - //! Time constant float dt = 0.005f; @@ -437,15 +436,12 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) orb_advert_t att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); int loopcounter = 0; - int printcounter = 0; thread_running = true; - float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f}; // XXX write this out to perf regs /* keep track of sensor updates */ - uint32_t sensor_last_count[3] = {0, 0, 0}; uint64_t sensor_last_timestamp[3] = {0, 0, 0}; struct attitude_estimator_so3_params so3_comp_params; @@ -513,7 +509,7 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro_offsets[0] /= offset_count; gyro_offsets[1] /= offset_count; gyro_offsets[2] /= offset_count; - warnx("gyro initialized, offsets: %.5f %.5f %.5f", gyro_offsets[0], gyro_offsets[1], gyro_offsets[2]); + warnx("gyro initialized, offsets: %.5f %.5f %.5f", (double)gyro_offsets[0], (double)gyro_offsets[1], (double)gyro_offsets[2]); } } else { @@ -523,13 +519,9 @@ int attitude_estimator_so3_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]); + if (sensor_last_timestamp[0] != raw.timestamp) { sensor_last_timestamp[0] = raw.timestamp; } @@ -538,11 +530,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) gyro[2] = raw.gyro_rad_s[2] - gyro_offsets[2]; /* 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; + if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) { + sensor_last_timestamp[1] = raw.accelerometer_timestamp; } acc[0] = raw.accelerometer_m_s2[0]; @@ -550,11 +539,8 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) acc[2] = 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; + if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) { + sensor_last_timestamp[2] = raw.magnetometer_timestamp; } mag[0] = raw.magnetometer_ga[0]; @@ -572,8 +558,6 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) continue; } - uint64_t timing_start = hrt_absolute_time(); - // NOTE : Accelerometer is reversed. // Because proper mount of PX4 will give you a reversed accelerometer readings. NonlinearSO3AHRSupdate(gyro[0], gyro[1], gyro[2], @@ -612,9 +596,9 @@ int attitude_estimator_so3_thread_main(int argc, char *argv[]) /* due to inputs or numerical failure the output is invalid, skip it */ // Due to inputs or numerical failure the output is invalid warnx("infinite euler angles, rotation matrix:"); - warnx("%.3f %.3f %.3f", Rot_matrix[0], Rot_matrix[1], Rot_matrix[2]); - warnx("%.3f %.3f %.3f", Rot_matrix[3], Rot_matrix[4], Rot_matrix[5]); - warnx("%.3f %.3f %.3f", Rot_matrix[6], Rot_matrix[7], Rot_matrix[8]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[0], (double)Rot_matrix[1], (double)Rot_matrix[2]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[3], (double)Rot_matrix[4], (double)Rot_matrix[5]); + warnx("%.3f %.3f %.3f", (double)Rot_matrix[6], (double)Rot_matrix[7], (double)Rot_matrix[8]); // Don't publish anything continue; } |