From 324322cb29720dd78b6eb534bb679532d5ed83f2 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Mon, 7 Jul 2014 02:10:09 +0400
Subject: [PATCH] UAVCAN ESC perf counters

---
 src/modules/uavcan/esc_controller.cpp | 28 +++++++++++++++++++++------
 src/modules/uavcan/esc_controller.hpp |  8 ++++++++
 2 files changed, 30 insertions(+), 6 deletions(-)

diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp
index f603d94482..406eba88c4 100644
--- a/src/modules/uavcan/esc_controller.cpp
+++ b/src/modules/uavcan/esc_controller.cpp
@@ -48,6 +48,12 @@ UavcanEscController::UavcanEscController(uavcan::INode &node) :
 {
 }
 
+UavcanEscController::~UavcanEscController()
+{
+	perf_free(_perfcnt_invalid_input);
+	perf_free(_perfcnt_scaling_error);
+}
+
 int UavcanEscController::init()
 {
 	// ESC status subscription
@@ -67,8 +73,10 @@ int UavcanEscController::init()
 
 void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 {
-	assert(outputs != nullptr);
-	assert(num_outputs <= MAX_ESCS);
+	if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
+		perf_count(_perfcnt_invalid_input);
+		return;
+	}
 
 	/*
 	 * Rate limiting - we don't want to congest the bus
@@ -89,13 +97,21 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 		for (unsigned i = 0; i < num_outputs; i++) {
 
 			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
-			if (scaled < 1.0F)
+			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
+			}
 
-			assert(scaled >= uavcan::equipment::esc::RawCommand::CMD_MIN);
-			assert(scaled <= uavcan::equipment::esc::RawCommand::CMD_MAX);
+			if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
+				perf_count(_perfcnt_scaling_error);
+			} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
+				perf_count(_perfcnt_scaling_error);
+			} else {
+				; // Correct value
+			}
 
-			msg.cmd.push_back(scaled);
+			msg.cmd.push_back(static_cast<unsigned>(scaled));
 		}
 	}
 
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp
index 0ed0c59b56..559ede561e 100644
--- a/src/modules/uavcan/esc_controller.hpp
+++ b/src/modules/uavcan/esc_controller.hpp
@@ -47,11 +47,13 @@
 #include <uavcan/uavcan.hpp>
 #include <uavcan/equipment/esc/RawCommand.hpp>
 #include <uavcan/equipment/esc/Status.hpp>
+#include <systemlib/perf_counter.h>
 
 class UavcanEscController
 {
 public:
 	UavcanEscController(uavcan::INode& node);
+	~UavcanEscController();
 
 	int init();
 
@@ -96,4 +98,10 @@ private:
 	 */
 	bool 				_armed = false;
 	uavcan::equipment::esc::Status	_states[MAX_ESCS];
+
+	/*
+	 * Perf counters
+	 */
+	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
+	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
 };