From 921c391db4c6da676f49b0889c8871f205508d53 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 3 Oct 2012 14:00:04 +0200 Subject: Commit finished attitude estim cleanup --- apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 7 +++++++ apps/uORB/topics/vehicle_attitude.h | 1 + 2 files changed, 8 insertions(+) diff --git a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c index 6368c5157..10405758f 100644 --- a/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c +++ b/apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c @@ -427,6 +427,13 @@ int attitude_estimator_ekf_thread_main(int argc, char *argv[]) att.pitchspeed = x_aposteriori[1]; att.yawspeed = x_aposteriori[2]; + /* copy offsets */ + memcpy(&att.rate_offsets, &(x_aposteriori[3]), sizeof(att.rate_offsets)); + + /* copy rotation matrix */ + memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix)); + att.R_valid = true; + // Broadcast orb_publish(ORB_ID(vehicle_attitude), pub_att, &att); } diff --git a/apps/uORB/topics/vehicle_attitude.h b/apps/uORB/topics/vehicle_attitude.h index e365a6557..009379920 100644 --- a/apps/uORB/topics/vehicle_attitude.h +++ b/apps/uORB/topics/vehicle_attitude.h @@ -74,6 +74,7 @@ struct vehicle_attitude_s { float rollspeed; /**< Roll angular speed (rad/s, Tait-Bryan, NED) LOGME */ float pitchspeed; /**< Pitch angular speed (rad/s, Tait-Bryan, NED) LOGME */ float yawspeed; /**< Yaw angular speed (rad/s, Tait-Bryan, NED) LOGME */ + float rate_offsets[3];/**< Offsets of the body angular rates from zero */ float R[3][3]; /**< Rotation matrix body to world, (Tait-Bryan, NED) */ float q[4]; /**< Quaternion (NED) */ bool R_valid; /**< Rotation matrix valid */ -- cgit v1.2.3