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

Merge remote-tracking branch 'upstream/master' into geo

Browse Source
This commit is contained in:
Thomas Gubler
2014-05-30 16:09:44 +02:00
parent e0042ec12c 90d8f7726d
commit dc336a2ea4
62 changed files with 1484 additions and 1060 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+20 -1
View File
@@ -63,11 +63,22 @@ ECL_PitchController::ECL_PitchController() :
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
}
ECL_PitchController::~ECL_PitchController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
{
/* Do not calculate control signal with bad inputs */
if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling pitch");
return _rate_setpoint;
}
/* flying inverted (wings upside down) ? */
bool inverted = false;
@@ -123,6 +134,14 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
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);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+4
View File
@@ -51,12 +51,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_PitchController //XXX: create controller superclass
{
public:
ECL_PitchController();
~ECL_PitchController();
float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
@@ -126,6 +129,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_PITCH_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+21 -2
View File
@@ -61,10 +61,21 @@ ECL_RollController::ECL_RollController() :
_rate_setpoint(0.0f),
_bodyrate_setpoint(0.0f)
{
perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
}
ECL_RollController::~ECL_RollController()
{
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);
return _rate_setpoint;
}
/* Calculate error */
float roll_error = roll_setpoint - roll;
@@ -86,6 +97,14 @@ float ECL_RollController::control_bodyrate(float pitch,
float yaw_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
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);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
@@ -122,8 +141,8 @@ float ECL_RollController::control_bodyrate(float pitch,
float id = _rate_error * dt;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
src/lib/ecl/attitude_fw/ecl_roll_controller.h
+4
View File
@@ -51,12 +51,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_RollController //XXX: create controller superclass
{
public:
ECL_RollController();
~ECL_RollController();
float control_attitude(float roll_setpoint, float roll);
float control_bodyrate(float pitch,
@@ -117,6 +120,7 @@ private:
float _rate_error;
float _rate_setpoint;
float _bodyrate_setpoint;
perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_ROLL_CONTROLLER_H
src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+20
View File
@@ -60,12 +60,25 @@ ECL_YawController::ECL_YawController() :
_bodyrate_setpoint(0.0f),
_coordinated_min_speed(1.0f)
{
perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
}
ECL_YawController::~ECL_YawController()
{
perf_free(_nonfinite_input_perf);
}
float ECL_YawController::control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint)
{
/* Do not calculate control signal with bad inputs */
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);
return _rate_setpoint;
}
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
@@ -103,6 +116,13 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
float pitch_rate_setpoint,
float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
{
/* Do not calculate control signal with bad inputs */
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);
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = ecl_elapsed_time(&_last_run);
_last_run = ecl_absolute_time();
src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+4
View File
@@ -50,12 +50,15 @@
#include <stdbool.h>
#include <stdint.h>
#include <systemlib/perf_counter.h>
class __EXPORT ECL_YawController //XXX: create controller superclass
{
public:
ECL_YawController();
~ECL_YawController();
float control_attitude(float roll, float pitch,
float speed_body_u, float speed_body_v, float speed_body_w,
float roll_rate_setpoint, float pitch_rate_setpoint);
@@ -118,6 +121,7 @@ private:
float _rate_setpoint;
float _bodyrate_setpoint;
float _coordinated_min_speed;
perf_counter_t _nonfinite_input_perf;
};