fw att: add performance counter

1 files changed, 8 insertions, 0 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 6ad00049d..82903ef5a 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -61,12 +61,19 @@ ECL_RollController::ECL_RollController() :
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f)
 {
+	perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
+}
+
+ECL_RollController::~ECL_RollController()
+{
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+		perf_count(_nonfinite_input_perf);
 		return _rate_setpoint;
 	}
 
@@ -94,6 +101,7 @@ float ECL_RollController::control_bodyrate(float pitch,
 	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
 			isfinite(airspeed_min) && isfinite(airspeed_max) &&
 			isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
 		return math::constrain(_last_output, -1.0f, 1.0f);
 	}