mc_rate: compute control energy and publish to status msg

msg/CMakeLists.txt

@@ -39,6 +39,7 @@ include(px4_list_make_absolute)
 set(msg_files
 	actuator_armed.msg
 	actuator_controls.msg
+	actuator_controls_status.msg
 	actuator_outputs.msg
 	adc_report.msg
 	airspeed.msg

msg/actuator_controls_status.msg

@@ -0,0 +1,10 @@
+uint64 timestamp			# time since system start (microseconds)
+
+uint8 INDEX_ROLL = 0
+uint8 INDEX_PITCH = 1
+uint8 INDEX_YAW = 2
+uint8 INDEX_THROTTLE = 3
+
+float32[4] control_power
+
+# TOPICS actuator_controls_status actuator_controls_status_0 actuator_controls_status_1

src/modules/logger/logged_topics.cpp

@@ -52,6 +52,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("actuator_controls_3", 100);
 	add_topic("actuator_controls_4", 100);
 	add_topic("actuator_controls_5", 100);
+	add_topic("actuator_controls_status_0", 300);
 	add_topic("airspeed", 1000);
 	add_topic("airspeed_validated", 200);
 	add_topic("camera_capture");

src/modules/mc_rate_control/MulticopterRateControl.cpp

@@ -256,6 +256,8 @@ MulticopterRateControl::Run()
 			actuators.timestamp = hrt_absolute_time();
 			_actuators_0_pub.publish(actuators);
 
+			updateActuatorControlsStatus(actuators, dt);
+
 		} else if (_v_control_mode.flag_control_termination_enabled) {
 			if (!_vehicle_status.is_vtol) {
 				// publish actuator controls
@@ -269,6 +271,29 @@ MulticopterRateControl::Run()
 	perf_end(_loop_perf);
 }
 
+void MulticopterRateControl::updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt)
+{
+	for (int i = 0; i < 4; i++) {
+		_control_energy[i] += actuators.control[i] * actuators.control[i] * dt;
+	}
+
+	_energy_integration_time += dt;
+
+	if (_energy_integration_time > 500e-3f) {
+
+		actuator_controls_status_s status;
+		status.timestamp = actuators.timestamp;
+
+		for (int i = 0; i < 4; i++) {
+			status.control_power[i] = _control_energy[i] / _energy_integration_time;
+			_control_energy[i] = 0.f;
+		}
+
+		_actuator_controls_status_0_pub.publish(status);
+		_energy_integration_time = 0.f;
+	}
+}
+
 int MulticopterRateControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;

src/modules/mc_rate_control/MulticopterRateControl.hpp

@@ -47,6 +47,7 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_controls_status.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/landing_gear.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -87,6 +88,8 @@ private:
 	 */
 	void		parameters_updated();
 
+	void updateActuatorControlsStatus(const actuator_controls_s &actuators, float dt);
+
 	RateControl _rate_control; ///< class for rate control calculations
 
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status)};
@@ -104,8 +107,9 @@ private:
 	uORB::SubscriptionCallbackWorkItem _vehicle_angular_velocity_sub{this, ORB_ID(vehicle_angular_velocity)};
 
 	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
-	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status)};	/**< controller status publication */
-	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};			/**< rate setpoint publication */
+	uORB::Publication<actuator_controls_status_s>	_actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)};
+	uORB::PublicationMulti<rate_ctrl_status_s>	_controller_status_pub{ORB_ID(rate_ctrl_status)};
+	uORB::Publication<vehicle_rates_setpoint_s>	_v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 
 	vehicle_control_mode_s		_v_control_mode{};
 	vehicle_status_s		_vehicle_status{};
@@ -126,6 +130,9 @@ private:
 
 	int8_t _landing_gear{landing_gear_s::GEAR_DOWN};
 
+	float _energy_integration_time{0.0f};
+	float _control_energy[4] {};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::MC_ROLLRATE_P>) _param_mc_rollrate_p,
 		(ParamFloat<px4::params::MC_ROLLRATE_I>) _param_mc_rollrate_i,