fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +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 09:11:44 +02:00
parent 5aefe11975
commit 77c823d3cd
5 changed files with 31 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
View File

@ -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)

15
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
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_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);
}

5
src/lib/ecl/attitude_fw/ecl_roll_controller.h
View File

@ -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

15
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
View File

@ -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 */

5
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
View File

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