diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_roll_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 8 |
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); } |