diff options
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp')
| -rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 15 |
1 files changed, 11 insertions, 4 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b8065..5b023fa8f 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -44,7 +44,14 @@ #include <geo/geo.h> #include <ecl/ecl.h> #include <mathlib/mathlib.h> + +#ifdef CONFIG_ARCH_ARM #include <systemlib/err.h> +#else +#include<ros_error.h> +#include <cmath> +#define isfinite std::isfinite +#endif ECL_YawController::ECL_YawController() : _last_run(0), @@ -59,13 +66,13 @@ ECL_YawController::ECL_YawController() : _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), _coordinated_min_speed(1.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control yaw nonfinite input")) { } ECL_YawController::~ECL_YawController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_YawController::control_attitude(float roll, float pitch, @@ -76,7 +83,7 @@ float ECL_YawController::control_attitude(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) && isfinite(speed_body_w) && isfinite(roll_rate_setpoint) && isfinite(pitch_rate_setpoint))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } // static int counter = 0; @@ -120,7 +127,7 @@ float ECL_YawController::control_bodyrate(float roll, float pitch, if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) && isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) && isfinite(airspeed_max) && isfinite(scaler))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } /* get the usual dt estimate */ |