diff options
| -rw-r--r-- | apps/attitude_estimator_ekf/attitude_estimator_ekf_main.c | 7 | ||||
| -rw-r--r-- | apps/uORB/topics/vehicle_attitude.h | 1 |
2 files changed, 8 insertions, 0 deletions
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 */ |