fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-26 09:13:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

fw att: add performance counter

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-21 21:49:00 +02:00
parent 05de7fb7a0
commit b9b81beb17
7 changed files with 53 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+8
View File
@@ -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);
}