mc_rate_control: minor cleanup

- only publish thrust setpoint on update
 - don't store full copies of msgs

msg/rate_ctrl_status.msg

@@ -1,4 +1,5 @@
-uint64 timestamp		# time since system start (microseconds)
+uint64 timestamp        # time since system start (microseconds)
+uint64 timestamp_sample # timestamp of the data sample on which this message is based (microseconds)
 
 # rate controller integrator status
 float32 rollspeed_integ

msg/vehicle_rates_setpoint.msg

@@ -1,4 +1,5 @@
 uint64 timestamp	# time since system start (microseconds)
+uint64 timestamp_sample                 # the timestamp of the raw data (microseconds)
 
 # body angular rates in NED frame
 float32 roll		# [rad/s] roll rate setpoint

src/modules/mc_rate_control/CMakeLists.txt

@@ -42,7 +42,6 @@ px4_add_module(
 		MulticopterRateControl.cpp
 		MulticopterRateControl.hpp
 	DEPENDS
-		circuit_breaker
 		mathlib
 		RateControl
 		px4_work_queue

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -46,12 +46,10 @@ using math::radians;
 MulticopterRateControl::MulticopterRateControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::rate_ctrl),
-	_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0)),
-	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
+	_actuator_controls_0_pub(vtol ? ORB_ID(actuator_controls_virtual_mc) : ORB_ID(actuator_controls_0))
 {
-	_vehicle_status.vehicle_type = vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
-
 	parameters_updated();
+
 	_controller_status_pub.advertise();
 }
 
@@ -60,8 +58,7 @@ MulticopterRateControl::~MulticopterRateControl()
 	perf_free(_loop_perf);
 }
 
-bool
-MulticopterRateControl::init()
+bool MulticopterRateControl::init()
 {
 	if (!_vehicle_angular_velocity_sub.registerCallback()) {
 		PX4_ERR("callback registration failed");
@@ -71,8 +68,7 @@ MulticopterRateControl::init()
 	return true;
 }
 
-void
-MulticopterRateControl::parameters_updated()
+void MulticopterRateControl::parameters_updated()
 {
 	// rate control parameters
 	// The controller gain K is used to convert the parallel (P + I/s + sD) form
@@ -89,15 +85,9 @@ MulticopterRateControl::parameters_updated()
 
 	_rate_control.setFeedForwardGain(
 		Vector3f(_param_mc_rollrate_ff.get(), _param_mc_pitchrate_ff.get(), _param_mc_yawrate_ff.get()));
-
-
-	// manual rate control acro mode rate limits
-	_acro_rate_max = Vector3f(radians(_param_mc_acro_r_max.get()), radians(_param_mc_acro_p_max.get()),
-				  radians(_param_mc_acro_y_max.get()));
 }
 
-void
-MulticopterRateControl::Run()
+void MulticopterRateControl::Run()
 {
 	if (should_exit()) {
 		_vehicle_angular_velocity_sub.unregisterCallback();
@@ -105,8 +95,6 @@ MulticopterRateControl::Run()
 		return;
 	}
 
-	perf_begin(_loop_perf);
-
 	// Check if parameters have changed
 	if (_parameter_update_sub.updated()) {
 		// clear update
@@ -117,60 +105,76 @@ MulticopterRateControl::Run()
 		parameters_updated();
 	}
 
-	/* run controller on gyro changes */
-	vehicle_angular_velocity_s angular_velocity;
+	perf_begin(_loop_perf);
 
-	if (_vehicle_angular_velocity_sub.update(&angular_velocity)) {
+	{
+		vehicle_control_mode_s vehicle_control_mode;
+
+		if (_vehicle_control_mode_sub.update(&vehicle_control_mode)) {
+			_rate_control_enabled = vehicle_control_mode.flag_control_rates_enabled;
+			_attitude_control_enabled = vehicle_control_mode.flag_control_attitude_enabled;
+			_manual_enabled = vehicle_control_mode.flag_control_manual_enabled;
+		}
+	}
+
+	// run controller on gyro changes
+	vehicle_angular_velocity_s angular_velocity;
+	vehicle_angular_acceleration_s angular_acceleration;
+
+	if (_rate_control_enabled && _vehicle_angular_velocity_sub.update(&angular_velocity)) {
 
 		// grab corresponding vehicle_angular_acceleration immediately after vehicle_angular_velocity copy
-		vehicle_angular_acceleration_s v_angular_acceleration{};
-		_vehicle_angular_acceleration_sub.copy(&v_angular_acceleration);
+		if (!_vehicle_angular_acceleration_sub.copy(&angular_acceleration)) {
+			Vector3f().copyTo(angular_acceleration.xyz);
+		}
 
-		const hrt_abstime now = angular_velocity.timestamp_sample;
-
-		// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
-		const float dt = math::constrain(((now - _last_run) * 1e-6f), 0.000125f, 0.02f);
-		_last_run = now;
-
-		const Vector3f angular_accel{v_angular_acceleration.xyz};
-		const Vector3f rates{angular_velocity.xyz};
-
-		/* check for updates in other topics */
-		_vehicle_control_mode_sub.update(&_vehicle_control_mode);
-
-		if (_vehicle_land_detected_sub.updated()) {
+		{
 			vehicle_land_detected_s vehicle_land_detected;
 
-			if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
-				_landed = vehicle_land_detected.landed;
-				_maybe_landed = vehicle_land_detected.maybe_landed;
+			if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) {
+				_landed = vehicle_land_detected.landed || vehicle_land_detected.maybe_landed;
 			}
 		}
 
-		_vehicle_status_sub.update(&_vehicle_status);
+		{
+			vehicle_status_s vehicle_status;
 
-		if (_landing_gear_sub.updated()) {
-			landing_gear_s landing_gear;
+			if (_vehicle_status_sub.update(&vehicle_status)) {
+				_armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
+				_rotary_wing = (vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING);
+				_vtol = vehicle_status.is_vtol;
+			}
+		}
 
-			if (_landing_gear_sub.copy(&landing_gear)) {
-				if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
-					if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && (_landed || _maybe_landed)) {
-						mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t");
-						events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"),
-						{events::Log::Error, events::LogInternal::Info},
-						"Landed, unable to retract landing gear");
+		{
+			// update saturation status from control allocation feedback
+			control_allocator_status_s control_allocator_status;
 
-					} else {
-						_landing_gear = landing_gear.landing_gear;
+			if (_control_allocator_status_sub.update(&control_allocator_status)) {
+				if (!control_allocator_status.torque_setpoint_achieved) {
+
+					Vector<bool, 3> saturation_positive{};
+					Vector<bool, 3> saturation_negative{};
+
+					for (size_t i = 0; i < 3; i++) {
+						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
+							saturation_positive(i) = true;
+
+						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
+							saturation_negative(i) = true;
+						}
 					}
+
+					// TODO: send the unallocated value directly for better anti-windup
+					_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
+
+				} else {
+					_rate_control.clearSaturationStatus();
 				}
 			}
 		}
 
-		// use rates setpoint topic
-		vehicle_rates_setpoint_s vehicle_rates_setpoint{};
-
-		if (_vehicle_control_mode.flag_control_manual_enabled && !_vehicle_control_mode.flag_control_attitude_enabled) {
+		if (_manual_enabled && !_attitude_control_enabled) {
 			// generate the rate setpoint from sticks
 			manual_control_setpoint_s manual_control_setpoint;
 
@@ -181,138 +185,147 @@ MulticopterRateControl::Run()
 					math::superexpo(-manual_control_setpoint.x, _param_mc_acro_expo.get(), _param_mc_acro_supexpo.get()),
 					math::superexpo(manual_control_setpoint.r, _param_mc_acro_expo_y.get(), _param_mc_acro_supexpoy.get())};
 
-				_rates_setpoint = man_rate_sp.emult(_acro_rate_max);
-				_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.0f, 1.0f);
-				_thrust_setpoint(0) = _thrust_setpoint(1) = 0.f;
+				// manual rate control acro mode rate limits
+				const Vector3f acro_rate_max(radians(_param_mc_acro_r_max.get()),
+							     radians(_param_mc_acro_p_max.get()),
+							     radians(_param_mc_acro_y_max.get()));
+
+				_rates_setpoint = man_rate_sp.emult(acro_rate_max);
+
+				_thrust_setpoint(0) = 0.f;
+				_thrust_setpoint(1) = 0.f;
+				_thrust_setpoint(2) = -math::constrain(manual_control_setpoint.z, 0.f, 1.f);
 
 				// publish rate setpoint
-				vehicle_rates_setpoint.roll = _rates_setpoint(0);
+				vehicle_rates_setpoint_s vehicle_rates_setpoint;
+				vehicle_rates_setpoint.timestamp_sample = manual_control_setpoint.timestamp_sample;
+				vehicle_rates_setpoint.roll  = _rates_setpoint(0);
 				vehicle_rates_setpoint.pitch = _rates_setpoint(1);
-				vehicle_rates_setpoint.yaw = _rates_setpoint(2);
+				vehicle_rates_setpoint.yaw   = _rates_setpoint(2);
 				_thrust_setpoint.copyTo(vehicle_rates_setpoint.thrust_body);
 				vehicle_rates_setpoint.timestamp = hrt_absolute_time();
-
 				_vehicle_rates_setpoint_pub.publish(vehicle_rates_setpoint);
+
+
+				publishThrustSetpoint(manual_control_setpoint.timestamp);
 			}
 
-		} else if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
-			if (_vehicle_rates_setpoint_sub.copy(&vehicle_rates_setpoint)) {
-				_rates_setpoint(0) = PX4_ISFINITE(vehicle_rates_setpoint.roll)  ? vehicle_rates_setpoint.roll  : rates(0);
-				_rates_setpoint(1) = PX4_ISFINITE(vehicle_rates_setpoint.pitch) ? vehicle_rates_setpoint.pitch : rates(1);
-				_rates_setpoint(2) = PX4_ISFINITE(vehicle_rates_setpoint.yaw)   ? vehicle_rates_setpoint.yaw   : rates(2);
+		} else {
+			// use rates setpoint topic
+			vehicle_rates_setpoint_s vehicle_rates_setpoint;
+
+			if (_vehicle_rates_setpoint_sub.update(&vehicle_rates_setpoint)) {
+				_rates_setpoint(0) = vehicle_rates_setpoint.roll;
+				_rates_setpoint(1) = vehicle_rates_setpoint.pitch;
+				_rates_setpoint(2) = vehicle_rates_setpoint.yaw;
 				_thrust_setpoint = Vector3f(vehicle_rates_setpoint.thrust_body);
+
+				publishThrustSetpoint(vehicle_rates_setpoint.timestamp);
 			}
 		}
 
-		// run the rate controller
-		if (_vehicle_control_mode.flag_control_rates_enabled) {
+		// Guard against too small (< 0.125ms) and too large (> 20ms) dt's.
+		const float dt = math::constrain(((angular_velocity.timestamp_sample - _last_run) * 1e-6f), 0.000125f, 0.02f);
+		_last_run = angular_velocity.timestamp_sample;
 
-			// reset integral if disarmed
-			if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
-				_rate_control.resetIntegral();
-			}
-
-			// update saturation status from control allocation feedback
-			control_allocator_status_s control_allocator_status;
-
-			if (_control_allocator_status_sub.update(&control_allocator_status)) {
-				Vector<bool, 3> saturation_positive;
-				Vector<bool, 3> saturation_negative;
-
-				if (!control_allocator_status.torque_setpoint_achieved) {
-					for (size_t i = 0; i < 3; i++) {
-						if (control_allocator_status.unallocated_torque[i] > FLT_EPSILON) {
-							saturation_positive(i) = true;
-
-						} else if (control_allocator_status.unallocated_torque[i] < -FLT_EPSILON) {
-							saturation_negative(i) = true;
-						}
-					}
-				}
-
-				// TODO: send the unallocated value directly for better anti-windup
-				_rate_control.setSaturationStatus(saturation_positive, saturation_negative);
-			}
-
-			// run rate controller
-			const Vector3f att_control = _rate_control.update(rates, _rates_setpoint, angular_accel, dt, _maybe_landed || _landed);
-
-			// publish rate controller status
-			rate_ctrl_status_s rate_ctrl_status{};
-			_rate_control.getRateControlStatus(rate_ctrl_status);
-			rate_ctrl_status.timestamp = hrt_absolute_time();
-			_controller_status_pub.publish(rate_ctrl_status);
-
-			// publish actuator controls
-			actuator_controls_s actuators{};
-			actuators.control[actuator_controls_s::INDEX_ROLL] = PX4_ISFINITE(att_control(0)) ? att_control(0) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_PITCH] = PX4_ISFINITE(att_control(1)) ? att_control(1) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_YAW] = PX4_ISFINITE(att_control(2)) ? att_control(2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_THROTTLE] = PX4_ISFINITE(_thrust_setpoint(2)) ? -_thrust_setpoint(
-						2) : 0.0f;
-			actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
-			actuators.timestamp_sample = angular_velocity.timestamp_sample;
-
-			if (!_vehicle_status.is_vtol) {
-				publishTorqueSetpoint(att_control, angular_velocity.timestamp_sample);
-				publishThrustSetpoint(angular_velocity.timestamp_sample);
-			}
-
-			// scale effort by battery status if enabled
-			if (_param_mc_bat_scale_en.get()) {
-				if (_battery_status_sub.updated()) {
-					battery_status_s battery_status;
-
-					if (_battery_status_sub.copy(&battery_status) && battery_status.connected && battery_status.scale > 0.f) {
-						_battery_status_scale = battery_status.scale;
-					}
-				}
-
-				if (_battery_status_scale > 0.0f) {
-					for (int i = 0; i < 4; i++) {
-						actuators.control[i] *= _battery_status_scale;
-					}
-				}
-			}
-
-			actuators.timestamp = hrt_absolute_time();
-			_actuator_controls_0_pub.publish(actuators);
-
-			updateActuatorControlsStatus(actuators, dt);
-
-		} else if (_vehicle_control_mode.flag_control_termination_enabled) {
-			if (!_vehicle_status.is_vtol) {
-				// publish actuator controls
-				actuator_controls_s actuators{};
-				actuators.timestamp = hrt_absolute_time();
-				_actuator_controls_0_pub.publish(actuators);
-			}
+		// reset integral if disarmed
+		if (!_armed || !_rotary_wing) {
+			_rate_control.resetIntegral();
 		}
+
+		// run rate controller
+		const Vector3f torque_sp{_rate_control.update(Vector3f(angular_velocity.xyz), _rates_setpoint, Vector3f(angular_acceleration.xyz), dt, _landed)};
+
+		publishTorqueSetpoint(angular_velocity.timestamp_sample, torque_sp);
+		publishRateControllerStatus(angular_velocity.timestamp_sample);
+
+		publishActuatorControls(angular_velocity.timestamp_sample, torque_sp, dt);
 	}
 
 	perf_end(_loop_perf);
 }
 
-void MulticopterRateControl::publishTorqueSetpoint(const Vector3f &torque_sp, const hrt_abstime &timestamp_sample)
-{
-	vehicle_torque_setpoint_s vehicle_torque_setpoint{};
-	vehicle_torque_setpoint.timestamp = hrt_absolute_time();
-	vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
-	vehicle_torque_setpoint.xyz[0] = (PX4_ISFINITE(torque_sp(0))) ? torque_sp(0) : 0.0f;
-	vehicle_torque_setpoint.xyz[1] = (PX4_ISFINITE(torque_sp(1))) ? torque_sp(1) : 0.0f;
-	vehicle_torque_setpoint.xyz[2] = (PX4_ISFINITE(torque_sp(2))) ? torque_sp(2) : 0.0f;
-
-	_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
-}
-
 void MulticopterRateControl::publishThrustSetpoint(const hrt_abstime &timestamp_sample)
 {
-	vehicle_thrust_setpoint_s vehicle_thrust_setpoint{};
-	vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
-	vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
-	_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+	if (!_vtol) {
+		vehicle_thrust_setpoint_s vehicle_thrust_setpoint;
+		vehicle_thrust_setpoint.timestamp_sample = timestamp_sample;
+		_thrust_setpoint.copyTo(vehicle_thrust_setpoint.xyz);
+		vehicle_thrust_setpoint.timestamp = hrt_absolute_time();
 
-	_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
+		_vehicle_thrust_setpoint_pub.publish(vehicle_thrust_setpoint);
+	}
+}
+
+void MulticopterRateControl::publishTorqueSetpoint(const hrt_abstime &timestamp_sample, const Vector3f &torque_sp)
+{
+	if (!_vtol) {
+		vehicle_torque_setpoint_s vehicle_torque_setpoint;
+		vehicle_torque_setpoint.timestamp_sample = timestamp_sample;
+		torque_sp.copyTo(vehicle_torque_setpoint.xyz);
+		vehicle_torque_setpoint.timestamp = hrt_absolute_time();
+
+		_vehicle_torque_setpoint_pub.publish(vehicle_torque_setpoint);
+	}
+}
+
+void MulticopterRateControl::publishRateControllerStatus(const hrt_abstime &timestamp_sample)
+{
+	rate_ctrl_status_s rate_ctrl_status;
+	rate_ctrl_status.timestamp_sample  = timestamp_sample;
+	rate_ctrl_status.rollspeed_integ   = _rate_control.getIntegral()(0);
+	rate_ctrl_status.pitchspeed_integ  = _rate_control.getIntegral()(1);
+	rate_ctrl_status.yawspeed_integ    = _rate_control.getIntegral()(2);
+	rate_ctrl_status.additional_integ1 = 0;
+	rate_ctrl_status.timestamp         = hrt_absolute_time();
+	_controller_status_pub.publish(rate_ctrl_status);
+}
+
+void MulticopterRateControl::publishActuatorControls(const hrt_abstime &timestamp_sample, const Vector3f &torque_sp,
+		float dt)
+{
+	// scale effort by battery status if enabled
+	if (_param_mc_bat_scale_en.get() && _battery_status_sub.updated()) {
+		battery_status_s battery_status;
+
+		if (_battery_status_sub.copy(&battery_status)) {
+			if (battery_status.connected && (battery_status.scale > 0.f)) {
+				_battery_status_scale = battery_status.scale;
+			}
+		}
+	}
+
+	{
+		// landing gear passed through actuator controls
+		landing_gear_s landing_gear;
+
+		if (_landing_gear_sub.update(&landing_gear)) {
+			if (landing_gear.landing_gear != landing_gear_s::GEAR_KEEP) {
+				if (landing_gear.landing_gear == landing_gear_s::GEAR_UP && _landed) {
+					mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear\t");
+					events::send(events::ID("mc_rate_control_not_retract_landing_gear_landed"),
+					{events::Log::Error, events::LogInternal::Info},
+					"Landed, unable to retract landing gear");
+
+				} else {
+					_landing_gear = landing_gear.landing_gear;
+				}
+			}
+		}
+	}
+
+	// publish actuator controls
+	actuator_controls_s actuators{};
+	actuators.timestamp_sample = timestamp_sample;
+	actuators.control[actuator_controls_s::INDEX_ROLL]  = torque_sp(0) * _battery_status_scale;
+	actuators.control[actuator_controls_s::INDEX_PITCH] = torque_sp(1) * _battery_status_scale;
+	actuators.control[actuator_controls_s::INDEX_YAW]   = torque_sp(2) * _battery_status_scale;
+	actuators.control[actuator_controls_s::INDEX_THROTTLE] = -_thrust_setpoint(2) * _battery_status_scale;
+	actuators.control[actuator_controls_s::INDEX_LANDING_GEAR] = _landing_gear;
+	actuators.timestamp = hrt_absolute_time();
+	_actuator_controls_0_pub.publish(actuators);
+
+	updateActuatorControlsStatus(actuators, dt);
 }
 
 void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -91,12 +91,14 @@ private:
 	 */
 	void parameters_updated();
 
+	void publishActuatorControls(const hrt_abstime &timestamp_sample, const matrix::Vector3f &torque_sp, float dt);
+	void publishRateControllerStatus(const hrt_abstime &timestamp_sample);
+	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
+	void publishTorqueSetpoint(const hrt_abstime &timestamp_sample, const matrix::Vector3f &torque_sp);
+
 	void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
 
-	void publishTorqueSetpoint(const matrix::Vector3f &torque_sp, const hrt_abstime &timestamp_sample);
-	void publishThrustSetpoint(const hrt_abstime &timestamp_sample);
-
-	RateControl _rate_control; ///< class for rate control calculations
+	RateControl _rate_control{}; ///< class for rate control calculations
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
 	uORB::Subscription _control_allocator_status_sub{ORB_ID(control_allocator_status)};
@@ -121,28 +123,31 @@ private:
 
 	orb_advert_t _mavlink_log_pub{nullptr};
 
-	vehicle_control_mode_s	_vehicle_control_mode{};
-	vehicle_status_s	_vehicle_status{};
-
+	bool _armed{false};
 	bool _landed{true};
-	bool _maybe_landed{true};
+
+	bool _rotary_wing{true};
+	bool _vtol{false};
+
+	bool _rate_control_enabled{false};
+	bool _attitude_control_enabled{false};
+	bool _manual_enabled{false};
 
 	hrt_abstime _last_run{0};
 
-	perf_counter_t	_loop_perf;			/**< loop duration performance counter */
-
 	// keep setpoint values between updates
-	matrix::Vector3f _acro_rate_max;		/**< max attitude rates in acro mode */
 	matrix::Vector3f _rates_setpoint{};
-
-	float _battery_status_scale{0.0f};
 	matrix::Vector3f _thrust_setpoint{};
 
+	float _battery_status_scale{1.f};
+
 	float _energy_integration_time{0.0f};
 	float _control_energy[4] {};
 
 	int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
 
+	perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; /**< loop duration performance counter */
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,

src/modules/mc_rate_control/RateControl/RateControl.cpp

@@ -68,6 +68,11 @@ Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, cons
 		updateIntegral(rate_error, dt);
 	}
 
+	if (!math::isFinite(torque)) {
+		_rate_int.zero();
+		return {};
+	}
+
 	return torque;
 }
 
@@ -96,16 +101,6 @@ void RateControl::updateIntegral(Vector3f &rate_error, const float dt)
 		// Perform the integration using a first order method
 		float rate_i = _rate_int(i) + i_factor * _gain_i(i) * rate_error(i) * dt;
 
-		// do not propagate the result if out of range or invalid
-		if (PX4_ISFINITE(rate_i)) {
-			_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
-		}
+		_rate_int(i) = math::constrain(rate_i, -_lim_int(i), _lim_int(i));
 	}
 }
-
-void RateControl::getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status)
-{
-	rate_ctrl_status.rollspeed_integ = _rate_int(0);
-	rate_ctrl_status.pitchspeed_integ = _rate_int(1);
-	rate_ctrl_status.yawspeed_integ = _rate_int(2);
-}

src/modules/mc_rate_control/RateControl/RateControl.hpp

@@ -42,7 +42,6 @@
 #include <matrix/matrix/math.hpp>
 
 #include <mathlib/mathlib.h>
-#include <uORB/topics/rate_ctrl_status.h>
 
 class RateControl
 {
@@ -78,6 +77,12 @@ public:
 	void setSaturationStatus(const matrix::Vector<bool, 3> &saturation_positive,
 				 const matrix::Vector<bool, 3> &saturation_negative);
 
+	void clearSaturationStatus()
+	{
+		_control_allocator_saturation_negative.zero();
+		_control_allocator_saturation_positive.zero();
+	}
+
 	/**
 	 * Run one control loop cycle calculation
 	 * @param rate estimation of the current vehicle angular rate
@@ -95,10 +100,9 @@ public:
 	void resetIntegral() { _rate_int.zero(); }
 
 	/**
-	 * Get status message of controller for logging/debugging
-	 * @param rate_ctrl_status status message to fill with internal states
+	 * Get integral for logging/debugging
 	 */
-	void getRateControlStatus(rate_ctrl_status_s &rate_ctrl_status);
+	const matrix::Vector3f &getIntegral() const { return _rate_int; }
 
 private:
 	void updateIntegral(matrix::Vector3f &rate_error, const float dt);
@@ -111,7 +115,7 @@ private:
 	matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
 
 	// States
-	matrix::Vector3f _rate_int; ///< integral term of the rate controller
+	matrix::Vector3f _rate_int{}; ///< integral term of the rate controller
 
 	// Feedback from control allocation
 	matrix::Vector<bool, 3> _control_allocator_saturation_negative;