diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-19 22:43:00 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2012-09-19 22:43:00 +0200 |
commit | dbd6cbea60ade756b10c693b905fb3a85329e185 (patch) | |
tree | 7b6a1810ed3d031a07ac6d74aeeed743c3e7624d /apps | |
parent | efcf146b6d22600341b55283b39f8b0a846dee09 (diff) | |
download | px4-firmware-dbd6cbea60ade756b10c693b905fb3a85329e185.tar.gz px4-firmware-dbd6cbea60ade756b10c693b905fb3a85329e185.tar.bz2 px4-firmware-dbd6cbea60ade756b10c693b905fb3a85329e185.zip |
Minor cleanups, correct sensor scaling
Diffstat (limited to 'apps')
-rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 74 |
1 files changed, 17 insertions, 57 deletions
diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6c18b044e..58d415d86 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -1,8 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2008-2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2012 PX4 Development Team. All rights reserved. * Author: Tobias Naegeli <naegelit@student.ethz.ch> - * Laurens Mackay <mackayl@student.ethz.ch> * Lorenz Meier <lm@inf.ethz.ch> * * Redistribution and use in source and binary forms, with or without @@ -35,14 +34,11 @@ ****************************************************************************/ /* - * @file Extended Kalman Filter for Attitude Estimation + * @file attitude_estimator_ekf_main.c + * + * Extended Kalman Filter for Attitude Estimation. */ - -/**************************************************************************** - * Included Files - ****************************************************************************/ - #include <nuttx/config.h> #include <unistd.h> #include <stdlib.h> @@ -114,9 +110,6 @@ static float Rot_matrix[9] = {1.f, 0, 0, 0, 0, 1.f }; /**< init: identity matrix */ -// static float x_aposteriori_k[12] = {0}; -// static float P_aposteriori_k[144] = {0}; - /* * [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) */ @@ -171,7 +164,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) /* */ /* update successful, copy data on every 2nd run and execute filter */ - } else if (loopcounter & 0x01) { + } else { orb_copy(ORB_ID(sensor_combined), sub_raw, &raw); @@ -179,24 +172,19 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) dt = (raw.timestamp - last_measurement) / 1000000.0f; last_measurement = raw.timestamp; - // XXX Read out accel range via SPI on init, assuming 4G range at 14 bit res here - float range_g = 4.0f; - float mag_offset[3] = {0}; /* scale from 14 bit to m/s2 */ - z_k[3] = ((raw.accelerometer_raw[0] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[4] = ((raw.accelerometer_raw[1] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); - z_k[5] = ((raw.accelerometer_raw[2] * range_g) / 8192.0f) / 9.81f; // = accel * (1 / 32768.0f / 8.0f * 9.81f); + z_k[3] = raw.accelerometer_m_s2[0]; + z_k[4] = raw.accelerometer_raw[1]; + z_k[5] = raw.accelerometer_raw[2]; - // XXX Read out mag range via I2C on init, assuming 0.88 Ga and 12 bit res here - z_k[0] = (raw.magnetometer_raw[0] - mag_offset[0]) * 0.01f; - z_k[1] = (raw.magnetometer_raw[1] - mag_offset[1]) * 0.01f; - z_k[2] = (raw.magnetometer_raw[2] - mag_offset[2]) * 0.01f; + z_k[0] = raw.magnetometer_ga[0]; + z_k[1] = raw.magnetometer_ga[1]; + z_k[2] = raw.magnetometer_ga[2]; /* Fill in gyro measurements */ - z_k[6] = raw.gyro_raw[0] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[7] = raw.gyro_raw[1] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - z_k[8] = raw.gyro_raw[2] * 0.00026631611f /* = gyro * (500.0f / 180.0f * pi / 32768.0f ) */; - + z_k[6] = raw.gyro_rad_s[0]; + z_k[7] = raw.gyro_rad_s[1]; + z_k[8] = raw.gyro_rad_s[2]; uint64_t now = hrt_absolute_time(); unsigned int time_elapsed = now - last_run; @@ -212,19 +200,6 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) overloadcounter++; } -// now = hrt_absolute_time(); - /* filter values */ - /* - * function [Rot_matrix,x_aposteriori,P_aposteriori] = attitudeKalmanfilter(dt,z_k,x_aposteriori_k,P_aposteriori_k,knownConst) - */ - - //extern void attitudeKalmanfilter(real32_T dt, const int8_T updVect[9], const real32_T z_k_data[9], - // const int32_T z_k_sizes[1], const real32_T u[4], - // const real32_T x_aposteriori_k[9], const real32_T P_aposteriori_k[81], - // const real32_T knownConst[20], real32_T eulerAngles[3], - // real32_T Rot_matrix[9], real32_T x_aposteriori[9], - // real32_T P_aposteriori[81]); - int8_t update_vect[9] = {1, 1, 1, 1, 1, 1, 1, 1, 1}; float euler[3]; int32_t z_k_sizes = 9; @@ -249,35 +224,20 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) printcounter++; -// time_elapsed = hrt_absolute_time() - now; -// if (blubb == 20) -// { -// printf("Estimator: %lu\n", time_elapsed); -// blubb = 0; -// } -// blubb++; - if (last_data > 0 && raw.timestamp - last_data > 8000) printf("sensor data missed! (%llu)\n", raw.timestamp - last_data); -// printf("%llu -> %llu = %llu\n", last_data, raw.timestamp, raw.timestamp - last_data); last_data = raw.timestamp; /* send out */ att.timestamp = raw.timestamp; - // att.roll = euler.x; - // att.pitch = euler.y; - // att.yaw = euler.z + F_M_PI; - - // if (att.yaw > 2 * F_M_PI) { - // att.yaw -= 2 * F_M_PI; - // } + att.roll = euler.x; + att.pitch = euler.y; + att.yaw = euler.z; // att.rollspeed = rates.x; // att.pitchspeed = rates.y; // att.yawspeed = rates.z; - // memcpy()R - // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } |