From 77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Wed, 24 Sep 2014 09:11:44 +0200 Subject: [PATCH] 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) --- src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp | 2 +- src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 ++++- src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 15 +++++++++++---- src/lib/ecl/attitude_fw/ecl_yaw_controller.h | 5 ++++- 5 files changed, 31 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp index 0019ef94d9..b840206d58 100644 --- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp @@ -75,7 +75,7 @@ ECL_PitchController::ECL_PitchController() : ECL_PitchController::~ECL_PitchController() { - perf_free(_nonfinite_input_perf); + //perf_free(_nonfinite_input_perf); } float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed) diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp index 94bd26f03d..1b9925f632 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp @@ -45,7 +45,14 @@ #include #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#define isfinite std::isfinite +#endif ECL_RollController::ECL_RollController() : _last_run(0), @@ -60,20 +67,20 @@ ECL_RollController::ECL_RollController() : _rate_error(0.0f), _rate_setpoint(0.0f), _bodyrate_setpoint(0.0f), - _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) + //_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input")) { } ECL_RollController::~ECL_RollController() { - perf_free(_nonfinite_input_perf); + //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); + //perf_count(_nonfinite_input_perf); return _rate_setpoint; } @@ -101,7 +108,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); + //perf_count(_nonfinite_input_perf); return math::constrain(_last_output, -1.0f, 1.0f); } diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h index 0799dbe03b..84e6e9fe4f 100644 --- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h @@ -51,7 +51,10 @@ #include #include + +#ifdef CONIG_ARCH_ARM #include +#endif class __EXPORT ECL_RollController //XXX: create controller superclass { @@ -120,7 +123,7 @@ private: float _rate_error; float _rate_setpoint; float _bodyrate_setpoint; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; }; #endif // ECL_ROLL_CONTROLLER_H diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index fe03b80651..5b023fa8f3 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 #include #include + +#ifdef CONFIG_ARCH_ARM #include +#else +#include +#include +#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 */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index a360c14b89..61657e95be 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h @@ -50,7 +50,10 @@ #include #include + +#ifdef CONFIG_ARCH_ARM #include +#endif class __EXPORT ECL_YawController //XXX: create controller superclass { @@ -121,7 +124,7 @@ private: float _rate_setpoint; float _bodyrate_setpoint; float _coordinated_min_speed; - perf_counter_t _nonfinite_input_perf; + //perf_counter_t _nonfinite_input_perf; };