diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-26 17:06:19 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-12-26 17:06:19 +0100 |
commit | 407889ea2c472ed5be950307bb5dc27f07f88006 (patch) | |
tree | 0dacd8384fd8a136c27163da1e22a086a0b9d3d5 /src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | |
parent | f3a224e30d8a70418541a6185ce5765b37745a7a (diff) | |
parent | 5c51adf5f79266de2b483c2461babd4d673cfffb (diff) | |
download | px4-firmware-407889ea2c472ed5be950307bb5dc27f07f88006.tar.gz px4-firmware-407889ea2c472ed5be950307bb5dc27f07f88006.tar.bz2 px4-firmware-407889ea2c472ed5be950307bb5dc27f07f88006.zip |
Merged master into indoor branch
Diffstat (limited to 'src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp')
-rwxr-xr-x | src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 75 |
1 files changed, 66 insertions, 9 deletions
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index cd7c56b87..1942fdc62 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -38,6 +38,7 @@ * * @author Tobias Naegeli <naegelit@student.ethz.ch> * @author Lorenz Meier <lm@inf.ethz.ch> + * @author Thomas Gubler <thomasgubler@gmail.com> */ #include <nuttx/config.h> @@ -75,8 +76,7 @@ #ifdef __cplusplus extern "C" { #endif -#include "codegen/attitudeKalmanfilter_initialize.h" -#include "codegen/attitudeKalmanfilter.h" +#include "codegen/AttitudeEKF.h" #include "attitude_estimator_ekf_params.h" #ifdef __cplusplus } @@ -133,9 +133,9 @@ int attitude_estimator_ekf_main(int argc, char *argv[]) attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, - 14000, + 7200, attitude_estimator_ekf_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + (argv) ? (char * const *)&argv[2] : (char * const *)NULL); exit(0); } @@ -207,10 +207,11 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds 0, 0, 1.f }; /**< init: identity matrix */ + float debugOutput[4] = { 0.0f }; int overloadcounter = 19; /* Initialize filter */ - attitudeKalmanfilter_initialize(); + AttitudeEKF_initialize(); /* store start time to guard against too slow update rates */ uint64_t last_run = hrt_absolute_time(); @@ -275,9 +276,10 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* keep track of sensor updates */ uint64_t sensor_last_timestamp[3] = {0, 0, 0}; - struct attitude_estimator_ekf_params ekf_params {}; + struct attitude_estimator_ekf_params ekf_params; + memset(&ekf_params, 0, sizeof(ekf_params)); - struct attitude_estimator_ekf_param_handles ekf_param_handles {}; + struct attitude_estimator_ekf_param_handles ekf_param_handles = { 0 }; /* initialize parameter handles */ parameters_init(&ekf_param_handles); @@ -528,8 +530,25 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds continue; } - attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r, - euler, Rot_matrix, x_aposteriori, P_aposteriori); + /* Call the estimator */ + AttitudeEKF(false, // approx_prediction + (unsigned char)ekf_params.use_moment_inertia, + update_vect, + dt, + z_k, + ekf_params.q[0], // q_rotSpeed, + ekf_params.q[1], // q_rotAcc + ekf_params.q[2], // q_acc + ekf_params.q[3], // q_mag + ekf_params.r[0], // r_gyro + ekf_params.r[1], // r_accel + ekf_params.r[2], // r_mag + ekf_params.moment_inertia_J, + x_aposteriori, + P_aposteriori, + Rot_matrix, + euler, + debugOutput); /* swap values for next iteration, check for fatal inputs */ if (isfinite(euler[0]) && isfinite(euler[1]) && isfinite(euler[2])) { @@ -576,6 +595,44 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memcpy(&att.R[0][0], &R.data[0][0], sizeof(att.R)); att.R_valid = true; + // compute secondary attitude + math::Matrix<3, 3> R_adapted; //modified rotation matrix + R_adapted = R; + + //move z to x + R_adapted(0, 0) = R(0, 2); + R_adapted(1, 0) = R(1, 2); + R_adapted(2, 0) = R(2, 2); + //move x to z + R_adapted(0, 2) = R(0, 0); + R_adapted(1, 2) = R(1, 0); + R_adapted(2, 2) = R(2, 0); + + //change direction of pitch (convert to right handed system) + R_adapted(0, 0) = -R_adapted(0, 0); + R_adapted(1, 0) = -R_adapted(1, 0); + R_adapted(2, 0) = -R_adapted(2, 0); + math::Vector<3> euler_angles_sec; //adapted euler angles for fixed wing operation + euler_angles_sec = R_adapted.to_euler(); + + att.roll_sec = euler_angles_sec(0); + att.pitch_sec = euler_angles_sec(1); + att.yaw_sec = euler_angles_sec(2); + + memcpy(&att.R_sec[0][0], &R_adapted.data[0][0], sizeof(att.R_sec)); + + att.rollspeed_sec = -x_aposteriori[2]; + att.pitchspeed_sec = x_aposteriori[1]; + att.yawspeed_sec = x_aposteriori[0]; + att.rollacc_sec = -x_aposteriori[5]; + att.pitchacc_sec = x_aposteriori[4]; + att.yawacc_sec = x_aposteriori[3]; + + att.g_comp_sec[0] = -raw.accelerometer_m_s2[2] - (-acc(2)); + att.g_comp_sec[1] = raw.accelerometer_m_s2[1] - acc(1); + att.g_comp_sec[2] = raw.accelerometer_m_s2[0] - acc(0); + + if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) { // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); |