diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 6904bfce54..95efeb23ad 100644 --- a/msg/CMakeLists.txt +++ b/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 diff --git a/msg/actuator_controls_status.msg b/msg/actuator_controls_status.msg new file mode 100644 index 0000000000..4e3bf83450 --- /dev/null +++ b/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 diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 3de518198b..bd8f88f118 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/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"); diff --git a/src/modules/mc_rate_control/MulticopterRateControl.cpp b/src/modules/mc_rate_control/MulticopterRateControl.cpp index 4e907fc4d2..17074ca8a5 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.cpp +++ b/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; diff --git a/src/modules/mc_rate_control/MulticopterRateControl.hpp b/src/modules/mc_rate_control/MulticopterRateControl.hpp index 531b1e435b..410e5dc1f4 100644 --- a/src/modules/mc_rate_control/MulticopterRateControl.hpp +++ b/src/modules/mc_rate_control/MulticopterRateControl.hpp @@ -47,6 +47,7 @@ #include #include #include +#include #include #include #include @@ -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 _actuators_0_pub; - uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; /**< controller status publication */ - uORB::Publication _v_rates_sp_pub{ORB_ID(vehicle_rates_setpoint)}; /**< rate setpoint publication */ + uORB::Publication _actuator_controls_status_0_pub{ORB_ID(actuator_controls_status_0)}; + uORB::PublicationMulti _controller_status_pub{ORB_ID(rate_ctrl_status)}; + uORB::Publication _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) _param_mc_rollrate_p, (ParamFloat) _param_mc_rollrate_i,