diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-09-24 08:52:28 +0200 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-09-24 08:52:28 +0200 |
commit | 0553771f4fd6fbdba43669a8f17185ed61f96a51 (patch) | |
tree | af21ce23d5092857d51a56dbce36b5f6046b4522 /src/lib | |
parent | 475f9a594b1088e8cb34b2d7c6f550de5a0193d7 (diff) | |
download | px4-firmware-0553771f4fd6fbdba43669a8f17185ed61f96a51.tar.gz px4-firmware-0553771f4fd6fbdba43669a8f17185ed61f96a51.tar.bz2 px4-firmware-0553771f4fd6fbdba43669a8f17185ed61f96a51.zip |
Adapted for sharded library use with ROS. Problems to solve: error library from PX4 does not work yet. math functions such as isfinite need to be shared as well. performance library needs to be shared as well (commented for now)
Diffstat (limited to 'src/lib')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 15 |
1 files changed, 11 insertions, 4 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 926a8db2a..0019ef94d 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -45,7 +45,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_PitchController::ECL_PitchController() : _last_run(0), @@ -61,8 +68,8 @@ ECL_PitchController::ECL_PitchController() : _integrator(0.0f), _rate_error(0.0f), _rate_setpoint(0.0f), - _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) + _bodyrate_setpoint(0.0f) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input")) { } @@ -75,7 +82,7 @@ float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, fl { /* Do not calculate control signal with bad inputs */ if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) { - perf_count(_nonfinite_input_perf); + //perf_count(_nonfinite_input_perf); warnx("not controlling pitch"); return _rate_setpoint; } @@ -138,7 +145,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); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } |