diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index 50ac73deca..edeea89c7d 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/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; diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 5adb4088e8..335275b159 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -53,6 +53,7 @@ #include #include #include +#include #include #include #include @@ -112,6 +113,7 @@ private: uORB::SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; uORB::Publication _actuators_0_pub; + uORB::Publication _actuator_controls_status_pub; uORB::Publication _attitude_sp_pub; uORB::Publication _rate_sp_pub{ORB_ID(vehicle_rates_setpoint)}; uORB::PublicationMulti _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) _param_fw_acro_x_max, (ParamFloat) _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. */