From eb4a56294856366bf4892aa5eb667b7805a38d3c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 8 Nov 2015 11:13:03 +0100 Subject: [PATCH] Attitude estimator Q: Move to convenience publication call --- .../attitude_estimator_q_main.cpp | 26 +++++++------------ 1 file changed, 10 insertions(+), 16 deletions(-) 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 3e44579c66..f253362dae 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -552,7 +552,7 @@ void AttitudeEstimatorQ::task_main() _vel_prev_t = 0; } - // Time from previous iteration + /* time from previous iteration */ hrt_abstime now = hrt_absolute_time(); float dt = (last_time > 0) ? ((now - last_time) / 1000000.0f) : 0.00001f; last_time = now; @@ -595,18 +595,15 @@ void AttitudeEstimatorQ::task_main() att.accel_vibration = _voter_accel.get_vibration_factor(hrt_absolute_time()); att.mag_vibration = _voter_mag.get_vibration_factor(hrt_absolute_time()); - if (_att_pub == nullptr) { - _att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att); - - } else { - orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att); - } + /* the instance count is not used here */ + int att_inst; + orb_publish_auto(ORB_ID(vehicle_attitude), &_att_pub, &att, &att_inst, ORB_PRIO_HIGH); struct control_state_s ctrl_state = {}; ctrl_state.timestamp = sensors.timestamp; - /* Attitude quaternions for control state */ + /* attitude quaternions for control state */ ctrl_state.q[0] = _q(0); ctrl_state.q[1] = _q(1); @@ -615,7 +612,7 @@ void AttitudeEstimatorQ::task_main() ctrl_state.q[3] = _q(3); - /* Attitude rates for control state */ + /* 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)); @@ -632,13 +629,10 @@ void AttitudeEstimatorQ::task_main() ctrl_state.airspeed_valid = false; } - /* Publish to control state topic */ - if (_ctrl_state_pub == nullptr) { - _ctrl_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state); - - } else { - orb_publish(ORB_ID(control_state), _ctrl_state_pub, &ctrl_state); - } + /* the instance count is not used here */ + int ctrl_inst; + /* publish to control state topic */ + orb_publish_auto(ORB_ID(control_state), &_ctrl_state_pub, &ctrl_state, &ctrl_inst, ORB_PRIO_HIGH); } }