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)

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;
 
 };