diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index a094772b37..f56e3ad243 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -54,7 +54,7 @@ #include #include #include - #include +#include #include #include #include @@ -527,22 +527,27 @@ void AttitudeEstimatorQ::task_main() } struct control_state_s ctrl_state = {}; + ctrl_state.timestamp = sensors.timestamp; /* Attitude quaternions for control state */ ctrl_state.q[0] = _q(0); + ctrl_state.q[1] = _q(1); + ctrl_state.q[2] = _q(2); + ctrl_state.q[3] = _q(3); /* Attitude rates for control state */ ctrl_state.roll_rate = _lp_roll_rate.apply(_rates(0)); + ctrl_state.pitch_rate = _lp_pitch_rate.apply(_rates(1)); + ctrl_state.roll_rate = _rates(2); /* Publish to control state topic */ - if (_ctrl_state_pub == nullptr) - { + if (_ctrl_state_pub == nullptr) { _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); } else {