diff --git a/msg/rate_ctrl_status.msg b/msg/rate_ctrl_status.msg index a3f033ec9e..0e28ede022 100644 --- a/msg/rate_ctrl_status.msg +++ b/msg/rate_ctrl_status.msg @@ -1,5 +1,11 @@ -float32 roll_integ # roll rate integrator -float32 pitch_integ # pitch rate integrator -float32 yaw_integ # yaw rate integrator -float32 additional_integ1 # FW: wheel rate integrator +# rates used by the controller +float32 rollspeed # Bias corrected angular velocity about X body axis in rad/s +float32 pitchspeed # Bias corrected angular velocity about Y body axis in rad/s +float32 yawspeed # Bias corrected angular velocity about Z body axis in rad/s + +# rate controller integrator status +float32 rollspeed_integ +float32 pitchspeed_integ +float32 yawspeed_integ +float32 additional_integ1 # FW: wheel rate integrator (optional) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 17ce9eebbb..47ff935281 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -1179,9 +1179,12 @@ FixedwingAttitudeControl::task_main() rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.roll_integ = _roll_ctrl.get_integrator(); - rate_ctrl_status.pitch_integ = _pitch_ctrl.get_integrator(); - rate_ctrl_status.yaw_integ = _yaw_ctrl.get_integrator(); + rate_ctrl_status.rollspeed = _att.rollspeed; + rate_ctrl_status.pitchspeed = _att.pitchspeed; + rate_ctrl_status.yawspeed = _att.yawspeed; + rate_ctrl_status.rollspeed_integ = _roll_ctrl.get_integrator(); + rate_ctrl_status.pitchspeed_integ = _pitch_ctrl.get_integrator(); + rate_ctrl_status.yawspeed_integ = _yaw_ctrl.get_integrator(); rate_ctrl_status.additional_integ1 = _wheel_ctrl.get_integrator(); int instance; diff --git a/src/modules/logger/logger.cpp b/src/modules/logger/logger.cpp index 03d79c00d9..8066bc4448 100644 --- a/src/modules/logger/logger.cpp +++ b/src/modules/logger/logger.cpp @@ -599,7 +599,7 @@ void Logger::add_default_topics() add_topic("mission_result"); add_topic("optical_flow", 50); add_topic("position_setpoint_triplet", 200); - add_topic("rate_ctrl_status", 200); + add_topic("rate_ctrl_status", 30); add_topic("safety"); add_topic("sensor_combined", 100); add_topic("sensor_preflight", 200); @@ -629,6 +629,7 @@ void Logger::add_high_rate_topics() add_topic("actuator_controls_0"); add_topic("actuator_outputs"); add_topic("manual_control_setpoint"); + add_topic("rate_ctrl_status"); add_topic("sensor_combined"); add_topic("vehicle_attitude"); add_topic("vehicle_attitude_setpoint"); diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index da330e0c3c..fefa3802c9 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -1257,9 +1257,12 @@ MulticopterAttitudeControl::task_main() /* publish controller status */ rate_ctrl_status_s rate_ctrl_status; rate_ctrl_status.timestamp = hrt_absolute_time(); - rate_ctrl_status.roll_integ = _rates_int(0); - rate_ctrl_status.pitch_integ = _rates_int(1); - rate_ctrl_status.yaw_integ = _rates_int(2); + rate_ctrl_status.rollspeed = _rates_prev(0); + rate_ctrl_status.pitchspeed = _rates_prev(1); + rate_ctrl_status.yawspeed = _rates_prev(2); + rate_ctrl_status.rollspeed_integ = _rates_int(0); + rate_ctrl_status.pitchspeed_integ = _rates_int(1); + rate_ctrl_status.yawspeed_integ = _rates_int(2); int instance; orb_publish_auto(ORB_ID(rate_ctrl_status), &_controller_status_pub, &rate_ctrl_status, &instance, ORB_PRIO_DEFAULT); diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b378f2df29..faf4470af6 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -2082,9 +2082,9 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- MULTIROTOR ATTITUDE CONTROLLER STATUS --- */ if (copy_if_updated(ORB_ID(rate_ctrl_status), &subs.rate_ctrl_status_sub, &buf.rate_ctrl_status)) { log_msg.msg_type = LOG_MACS_MSG; - log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.roll_integ; - log_msg.body.log_MACS.pitch_rate_integ = buf.rate_ctrl_status.pitch_integ; - log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yaw_integ; + log_msg.body.log_MACS.roll_rate_integ = buf.rate_ctrl_status.rollspeed_integ; + log_msg.body.log_MACS.pitch_rate_integ = buf.rate_ctrl_status.pitchspeed_integ; + log_msg.body.log_MACS.yaw_rate_integ = buf.rate_ctrl_status.yawspeed_integ; LOGBUFFER_WRITE_AND_COUNT(MACS); } }