From a9b848cae31b849825e3a9faaec9658287fd964e Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Fri, 12 Aug 2022 15:06:57 +0200 Subject: [PATCH] FW attitue controller: fix publishing of rate controller status Signed-off-by: Silvan Fuhrer --- msg/rate_ctrl_status.msg | 2 +- src/modules/fw_att_control/FixedwingAttitudeControl.cpp | 9 +++------ 2 files changed, 4 insertions(+), 7 deletions(-) diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index 145bf25a5c..3f5644699d 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -4,4 +4,4 @@ uint64 timestamp # time since system start (microseconds) float32 rollspeed_integ float32 pitchspeed_integ float32 yawspeed_integ -float32 additional_integ1 # FW: wheel rate integrator (optional) +float32 wheel_rate_integ # FW only and optional diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 33417a77eb..d0aed1c485 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -623,16 +623,13 @@ void FixedwingAttitudeControl::Run() _rates_sp.thrust_body[0] : 0.0f; } + // publish rate controller status rate_ctrl_status_s rate_ctrl_status{}; + _rate_control.getRateControlStatus(rate_ctrl_status); rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); - rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); if (wheel_control) { - rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); - - } else { - rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); + rate_ctrl_status.wheel_rate_integ = _wheel_ctrl.get_integrator(); } _rate_ctrl_status_pub.publish(rate_ctrl_status);