diff options
author | Roman Bapst <romanbapst@yahoo.de> | 2014-09-24 09:11:44 +0200 |
---|---|---|
committer | Roman Bapst <romanbapst@yahoo.de> | 2014-09-24 09:11:44 +0200 |
commit | 77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5 (patch) | |
tree | 991b0ec7fd12d7d5a8be23a5ae2bd81530abf2d5 /src/lib | |
parent | 5aefe11975c0fa04ec0a921ffb54d7cf8da5c39a (diff) | |
download | px4-firmware-77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5.tar.gz px4-firmware-77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5.tar.bz2 px4-firmware-77c823d3cd0f49014a33632ec9ef3efdd7d3dfa5.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 | 2 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.cpp | 15 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_roll_controller.h | 5 | ||||
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 15 | ||||
-rw-r--r-- | 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 0019ef94d..b840206d5 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 94bd26f03..1b9925f63 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 <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_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 0799dbe03..84e6e9fe4 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 <stdbool.h> #include <stdint.h> + +#ifdef CONIG_ARCH_ARM #include <systemlib/perf_counter.h> +#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 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 */ diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h index a360c14b8..61657e95b 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 <stdbool.h> #include <stdint.h> + +#ifdef CONFIG_ARCH_ARM #include <systemlib/perf_counter.h> +#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; }; |