diff --git a/msg/CMakeLists.txt b/msg/CMakeLists.txt index 24f70e12a6..637543d242 100644 --- a/msg/CMakeLists.txt +++ b/msg/CMakeLists.txt @@ -166,6 +166,7 @@ set(msg_files ParameterSetValueRequest.msg ParameterSetValueResponse.msg ParameterUpdate.msg + Performance.msg Ping.msg PositionControllerLandingStatus.msg PositionControllerStatus.msg diff --git a/msg/Performance.msg b/msg/Performance.msg new file mode 100644 index 0000000000..e65625d51f --- /dev/null +++ b/msg/Performance.msg @@ -0,0 +1,2 @@ +uint64 timestamp # time since system start (microseconds) +uint64 delta_time \ No newline at end of file diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 48363eeb1e..9a4e9bbf31 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -308,6 +308,12 @@ ControlAllocator::Run() perf_begin(_loop_perf); + performance_s performance; + performance.timestamp = hrt_absolute_time(); + performance.delta_time = performance.timestamp - _last_perf_timestamp; + _performance_pub.publish(performance); + _last_perf_timestamp = performance.timestamp; + #ifndef ENABLE_LOCKSTEP_SCHEDULER // Backup schedule would interfere with lockstep // Push backup schedule ScheduleDelayed(50_ms); diff --git a/src/modules/control_allocator/ControlAllocator.hpp b/src/modules/control_allocator/ControlAllocator.hpp index 303316f1ef..a4e357752a 100644 --- a/src/modules/control_allocator/ControlAllocator.hpp +++ b/src/modules/control_allocator/ControlAllocator.hpp @@ -77,6 +77,7 @@ #include #include #include +#include #include class ControlAllocator : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem @@ -184,6 +185,7 @@ private: uORB::Publication _actuator_motors_pub{ORB_ID(actuator_motors)}; uORB::Publication _actuator_servos_pub{ORB_ID(actuator_servos)}; uORB::Publication _actuator_servos_trim_pub{ORB_ID(actuator_servos_trim)}; + uORB::Publication _performance_pub{ORB_ID(performance)}; uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; @@ -205,6 +207,7 @@ private: hrt_abstime _last_run{0}; hrt_abstime _timestamp_sample{0}; hrt_abstime _last_status_pub{0}; + hrt_abstime _last_perf_timestamp{0}; ParamHandles _param_handles{}; Params _params{}; diff --git a/src/modules/logger/logged_topics.cpp b/src/modules/logger/logged_topics.cpp index 8bd55e62fe..0949a93fb8 100644 --- a/src/modules/logger/logged_topics.cpp +++ b/src/modules/logger/logged_topics.cpp @@ -97,6 +97,7 @@ void LoggedTopics::add_default_topics() add_topic("offboard_control_mode", 100); add_topic("onboard_computer_status", 10); add_topic("parameter_update"); + add_topic("performance"); add_topic("position_controller_status", 500); add_topic("position_controller_landing_status", 100); add_topic("goto_setpoint", 200);