compute and publish fixed-wing control power

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -44,6 +44,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl(bool vtol) :
 	ModuleParams(nullptr),
 	WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers),
 	_actuators_0_pub(vtol ? ORB_ID(actuator_controls_virtual_fw) : ORB_ID(actuator_controls_0)),
+	_actuator_controls_status_pub(vtol ? ORB_ID(actuator_controls_status_1) : ORB_ID(actuator_controls_status_0)),
 	_attitude_sp_pub(vtol ? ORB_ID(fw_virtual_attitude_setpoint) : ORB_ID(vehicle_attitude_setpoint)),
 	_loop_perf(perf_alloc(PC_ELAPSED, MODULE_NAME": cycle"))
 {
@@ -642,6 +643,8 @@ void FixedwingAttitudeControl::Run()
 		    _vcontrol_mode.flag_control_manual_enabled) {
 			_actuators_0_pub.publish(_actuators);
 		}
+
+		updateActuatorControlsStatus(dt);
 	}
 
 	perf_end(_loop_perf);
@@ -712,6 +715,41 @@ void FixedwingAttitudeControl::control_flaps(const float dt)
 	}
 }
 
+void FixedwingAttitudeControl::updateActuatorControlsStatus(float dt)
+{
+	for (int i = 0; i < 4; i++) {
+		float control_signal;
+
+		if (i <= actuator_controls_status_s::INDEX_YAW) {
+			// We assume that the attitude is actuated by control surfaces
+			// consuming power only when they move
+			control_signal = _actuators.control[i] - _control_prev[i];
+			_control_prev[i] = _actuators.control[i];
+
+		} else {
+			control_signal = _actuators.control[i];
+		}
+
+		_control_energy[i] += control_signal * control_signal * 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_pub.publish(status);
+		_energy_integration_time = 0.f;
+	}
+}
+
 int FixedwingAttitudeControl::task_spawn(int argc, char *argv[])
 {
 	bool vtol = false;

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -53,6 +53,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/airspeed_validated.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/manual_control_setpoint.h>
@@ -112,6 +113,7 @@ private:
 	uORB::SubscriptionData<airspeed_validated_s> _airspeed_validated_sub{ORB_ID(airspeed_validated)};
 
 	uORB::Publication<actuator_controls_s>		_actuators_0_pub;
+	uORB::Publication<actuator_controls_status_s>	_actuator_controls_status_pub;
 	uORB::Publication<vehicle_attitude_setpoint_s>	_attitude_sp_pub;
 	uORB::Publication<vehicle_rates_setpoint_s>	_rate_sp_pub{ORB_ID(vehicle_rates_setpoint)};
 	uORB::PublicationMulti<rate_ctrl_status_s>	_rate_ctrl_status_pub{ORB_ID(rate_ctrl_status)};
@@ -141,6 +143,10 @@ private:
 
 	bool _is_tailsitter{false};
 
+	float _energy_integration_time{0.0f};
+	float _control_energy[4] {};
+	float _control_prev[3] {};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::FW_ACRO_X_MAX>) _param_fw_acro_x_max,
 		(ParamFloat<px4::params::FW_ACRO_Y_MAX>) _param_fw_acro_y_max,
@@ -217,6 +223,8 @@ private:
 
 	void control_flaps(const float dt);
 
+	void updateActuatorControlsStatus(float dt);
+
 	/**
 	 * Update our local parameter cache.
 	 */