From bdec17a9d4daf3d3e707c9d769ed0189caf8e6d8 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 24 Aug 2022 17:39:43 -0400
Subject: [PATCH] drivers/pwm_out_sim: add perf counters (sync with
 drivers/pwm_out)

---
 src/modules/simulation/pwm_out_sim/PWMSim.cpp | 58 ++++++++++---------
 src/modules/simulation/pwm_out_sim/PWMSim.hpp |  9 +--
 2 files changed, 37 insertions(+), 30 deletions(-)

diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.cpp b/src/modules/simulation/pwm_out_sim/PWMSim.cpp
index 3ae575ba0a..c11a5eb365 100644
--- a/src/modules/simulation/pwm_out_sim/PWMSim.cpp
+++ b/src/modules/simulation/pwm_out_sim/PWMSim.cpp
@@ -52,33 +52,14 @@ PWMSim::PWMSim(bool hil_mode_enabled) :
 	_mixing_output.setIgnoreLockdown(hil_mode_enabled);
 }
 
-void
-PWMSim::Run()
+PWMSim::~PWMSim()
 {
-	if (should_exit()) {
-		ScheduleClear();
-		_mixing_output.unregister();
-
-		exit_and_cleanup();
-		return;
-	}
-
-	_mixing_output.update();
-
-	// check for parameter updates
-	if (_parameter_update_sub.updated()) {
-		parameter_update_s pupdate;
-		_parameter_update_sub.copy(&pupdate);
-		updateParams();
-	}
-
-	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
-	_mixing_output.updateSubscriptions(true);
+	perf_free(_cycle_perf);
+	perf_free(_interval_perf);
 }
 
-bool
-PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
-		      unsigned num_control_groups_updated)
+bool PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigned num_outputs,
+			   unsigned num_control_groups_updated)
 {
 	// Only publish once we receive actuator_controls (important for lock-step to work correctly)
 	if (num_control_groups_updated > 0) {
@@ -116,8 +97,30 @@ PWMSim::updateOutputs(bool stop_motors, uint16_t outputs[MAX_ACTUATORS], unsigne
 	return false;
 }
 
-int
-PWMSim::task_spawn(int argc, char *argv[])
+void PWMSim::Run()
+{
+	if (should_exit()) {
+		ScheduleClear();
+		_mixing_output.unregister();
+
+		exit_and_cleanup();
+		return;
+	}
+
+	_mixing_output.update();
+
+	// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+		updateParams();
+	}
+
+	// check at end of cycle (updateSubscriptions() can potentially change to a different WorkQueue thread)
+	_mixing_output.updateSubscriptions(true);
+}
+
+int PWMSim::task_spawn(int argc, char *argv[])
 {
 	bool hil_mode = false;
 
@@ -156,7 +159,10 @@ int PWMSim::custom_command(int argc, char *argv[])
 
 int PWMSim::print_status()
 {
+	perf_print_counter(_cycle_perf);
+	perf_print_counter(_interval_perf);
 	_mixing_output.printStatus();
+
 	return 0;
 }
 
diff --git a/src/modules/simulation/pwm_out_sim/PWMSim.hpp b/src/modules/simulation/pwm_out_sim/PWMSim.hpp
index a35b092db3..b16a9fac5e 100644
--- a/src/modules/simulation/pwm_out_sim/PWMSim.hpp
+++ b/src/modules/simulation/pwm_out_sim/PWMSim.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012-2019 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2022 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,8 +42,6 @@
 #include <px4_platform_common/tasks.h>
 #include <px4_platform_common/time.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_outputs.h>
-#include <uORB/Publication.hpp>
 #include <uORB/Subscription.hpp>
 
 #if defined(CONFIG_ARCH_BOARD_PX4_SITL)
@@ -58,6 +56,7 @@ class PWMSim : public ModuleBase<PWMSim>, public OutputModuleInterface
 {
 public:
 	PWMSim(bool hil_mode_enabled);
+	~PWMSim() override;
 
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
@@ -75,7 +74,6 @@ public:
 			   unsigned num_outputs, unsigned num_control_groups_updated) override;
 
 private:
-
 	void Run() override;
 
 	static constexpr uint16_t PWM_SIM_DISARMED_MAGIC = 900;
@@ -87,4 +85,7 @@ private:
 	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
 
 	uORB::Publication<actuator_outputs_s> _actuator_outputs_sim_pub{ORB_ID(actuator_outputs_sim)};
+
+	perf_counter_t	_cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
+	perf_counter_t	_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")};
 };