diff options
author | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-21 21:49:00 +0200 |
---|---|---|
committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-05-21 21:49:00 +0200 |
commit | b9b81beb17eb449921f11f46bc419056dce03852 (patch) | |
tree | aeb4453b3db34fcac50bc08bae2a65ecab667f69 /src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | |
parent | 05de7fb7a08a4786b12ab3c9eeda040f70b01228 (diff) | |
download | px4-firmware-b9b81beb17eb449921f11f46bc419056dce03852.tar.gz px4-firmware-b9b81beb17eb449921f11f46bc419056dce03852.tar.bz2 px4-firmware-b9b81beb17eb449921f11f46bc419056dce03852.zip |
fw att: add performance counter
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 8 |
1 files changed, 8 insertions, 0 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index a3f5199b1..0a909d02f 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -63,12 +63,19 @@ ECL_PitchController::ECL_PitchController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f) { + perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"); +} + +ECL_PitchController::~ECL_PitchController() +{ + perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { + perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -131,6 +138,7 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_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); } |