fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 22:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

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)

Browse Source
This commit is contained in:
Roman Bapst
2014-09-24 08:52:28 +02:00
parent 475f9a594b
commit 0553771f4f
1 changed files with 11 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+11 -4
View File
@@ -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);
}