aboutsummaryrefslogtreecommitdiff
path: root/src/lib/ecl
diff options
context:
space:
mode:
Diffstat (limited to 'src/lib/ecl')
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp123
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.h34
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp94
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.h32
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp129
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.h77
-rw-r--r--src/lib/ecl/ecl.h3
-rw-r--r--src/lib/ecl/l1/ecl_l1_pos_controller.cpp58
-rw-r--r--src/lib/ecl/l1/ecl_l1_pos_controller.h12
9 files changed, 424 insertions, 138 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 2eb58abd6..46db788a6 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -45,38 +45,39 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
ECL_PitchController::ECL_PitchController() :
_last_run(0),
+ _tc(0.1f),
+ _k_p(0.0f),
+ _k_i(0.0f),
+ _k_ff(0.0f),
+ _integrator_max(0.0f),
+ _max_rate_pos(0.0f),
+ _max_rate_neg(0.0f),
+ _roll_ff(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
- _max_deflection_rad(math::radians(45.0f))
+ _bodyrate_setpoint(0.0f),
+ _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control pitch nonfinite input"))
{
}
-float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
- bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+ECL_PitchController::~ECL_PitchController()
{
- /* get the usual dt estimate */
- uint64_t dt_micros = ecl_elapsed_time(&_last_run);
- _last_run = ecl_absolute_time();
- float dt = (float)dt_micros * 1e-6f;
-
- /* lock integral for long intervals */
- if (dt_micros > 500000)
- lock_integrator = true;
-
- float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
- float k_i_rate = _k_i * _tc;
+ perf_free(_nonfinite_input_perf);
+}
- /* input conditioning */
- if (!isfinite(airspeed)) {
- /* airspeed is NaN, +- INF or not available, pick center of band */
- airspeed = 0.5f * (airspeed_min + airspeed_max);
- } else if (airspeed < airspeed_min) {
- airspeed = airspeed_min;
+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) ? */
@@ -100,34 +101,83 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
}
/* calculate the offset in the rate resulting from rolling */
+ //xxx needs explanation and conversion to body angular rates or should be removed
float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
tanf(roll) * sinf(roll)) * _roll_ff;
if (inverted)
turn_offset = -turn_offset;
+ /* Calculate the error */
float pitch_error = pitch_setpoint - pitch;
- /* rate setpoint from current error and time constant */
- _rate_setpoint = pitch_error / _tc;
+
+ /* Apply P controller: rate setpoint from current error and time constant */
+ _rate_setpoint = pitch_error / _tc;
/* add turn offset */
_rate_setpoint += turn_offset;
- _rate_error = _rate_setpoint - pitch_rate;
+ /* limit the rate */ //XXX: move to body angluar rates
+ if (_max_rate_pos > 0.01f && _max_rate_neg > 0.01f) {
+ if (_rate_setpoint > 0.0f) {
+ _rate_setpoint = (_rate_setpoint > _max_rate_pos) ? _max_rate_pos : _rate_setpoint;
+ } else {
+ _rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
+ }
- float ilimit_scaled = _integrator_max * scaler;
+ }
- if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+ return _rate_setpoint;
+}
- float id = _rate_error * k_i_rate * dt * scaler;
+float ECL_PitchController::control_bodyrate(float roll, float pitch,
+ float pitch_rate, float yaw_rate,
+ 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();
+ float dt = (float)dt_micros * 1e-6f;
+
+ /* lock integral for long intervals */
+ if (dt_micros > 500000)
+ lock_integrator = true;
+
+ /* input conditioning */
+ if (!isfinite(airspeed)) {
+ /* airspeed is NaN, +- INF or not available, pick center of band */
+ airspeed = 0.5f * (airspeed_min + airspeed_max);
+ } else if (airspeed < airspeed_min) {
+ airspeed = airspeed_min;
+ }
+
+ /* Transform setpoint to body angular rates */
+ _bodyrate_setpoint = cosf(roll) * _rate_setpoint + cosf(pitch) * sinf(roll) * yaw_rate_setpoint; //jacobian
+
+ /* Transform estimation to body angular rates */
+ float pitch_bodyrate = cosf(roll) * pitch_rate + cosf(pitch) * sinf(roll) * yaw_rate; //jacobian
+
+ _rate_error = _bodyrate_setpoint - pitch_bodyrate;
+
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+ float id = _rate_error * dt;
/*
- * anti-windup: do not allow integrator to increase into the
- * wrong direction if actuator is at limit
+ * anti-windup: do not allow integrator to increase if actuator is at limit
*/
- if (_last_output < -_max_deflection_rad) {
+ if (_last_output < -1.0f) {
/* only allow motion to center: increase value */
id = math::max(id, 0.0f);
- } else if (_last_output > _max_deflection_rad) {
+ } else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -136,11 +186,14 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
}
/* integrator limit */
- _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
- /* store non-limited output */
- _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
-
- return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+ //xxx: until start detection is available: integral part in control signal is limited here
+ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_bodyrate_setpoint * _k_ff +_rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+// warnx("pitch: _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+// warnx("roll: _last_output %.4f", (double)_last_output);
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_PitchController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 1e6cec6a1..39b9f9d03 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal pitch PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -50,14 +51,22 @@
#include <stdbool.h>
#include <stdint.h>
+#include <systemlib/perf_counter.h>
-class __EXPORT ECL_PitchController
+class __EXPORT ECL_PitchController //XXX: create controller superclass
{
public:
ECL_PitchController();
- float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
- bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+ ~ECL_PitchController();
+
+ float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
+
+
+ float control_bodyrate(float roll, float pitch,
+ float pitch_rate, float yaw_rate,
+ float yaw_rate_setpoint,
+ float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -67,21 +76,27 @@ public:
void set_k_p(float k_p) {
_k_p = k_p;
}
+
void set_k_i(float k_i) {
_k_i = k_i;
}
- void set_k_d(float k_d) {
- _k_d = k_d;
+
+ void set_k_ff(float k_ff) {
+ _k_ff = k_ff;
}
+
void set_integrator_max(float max) {
_integrator_max = max;
}
+
void set_max_rate_pos(float max_rate_pos) {
_max_rate_pos = max_rate_pos;
}
+
void set_max_rate_neg(float max_rate_neg) {
_max_rate_neg = max_rate_neg;
}
+
void set_roll_ff(float roll_ff) {
_roll_ff = roll_ff;
}
@@ -94,13 +109,17 @@ public:
return _rate_setpoint;
}
+ float get_desired_bodyrate() {
+ return _bodyrate_setpoint;
+ }
+
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
- float _k_d;
+ float _k_ff;
float _integrator_max;
float _max_rate_pos;
float _max_rate_neg;
@@ -109,7 +128,8 @@ private:
float _integrator;
float _rate_error;
float _rate_setpoint;
- float _max_deflection_rad;
+ float _bodyrate_setpoint;
+ perf_counter_t _nonfinite_input_perf;
};
#endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 0b1ffa7a4..9894a34d7 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -45,22 +45,66 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
ECL_RollController::ECL_RollController() :
_last_run(0),
_tc(0.1f),
+ _k_p(0.0f),
+ _k_i(0.0f),
+ _k_ff(0.0f),
+ _integrator_max(0.0f),
+ _max_rate(0.0f),
_last_output(0.0f),
_integrator(0.0f),
_rate_error(0.0f),
_rate_setpoint(0.0f),
- _max_deflection_rad(math::radians(45.0f))
+ _bodyrate_setpoint(0.0f),
+ _nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control roll nonfinite input"))
{
+}
+ECL_RollController::~ECL_RollController()
+{
+ perf_free(_nonfinite_input_perf);
}
-float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
- float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+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;
+
+ /* Apply P controller */
+ _rate_setpoint = roll_error / _tc;
+
+ /* limit the rate */ //XXX: move to body angluar rates
+ if (_max_rate > 0.01f) {
+ _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+ _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+ }
+
+ return _rate_setpoint;
+}
+
+float ECL_RollController::control_bodyrate(float pitch,
+ float roll_rate, float yaw_rate,
+ 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();
@@ -70,10 +114,8 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
if (dt_micros > 500000)
lock_integrator = true;
- float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
- float k_i_rate = _k_i * _tc;
-
/* input conditioning */
+// warnx("airspeed pre %.4f", (double)airspeed);
if (!isfinite(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (airspeed_min + airspeed_max);
@@ -81,32 +123,27 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
airspeed = airspeed_min;
}
- float roll_error = roll_setpoint - roll;
- _rate_setpoint = roll_error / _tc;
- /* limit the rate */
- if (_max_rate > 0.01f) {
- _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
- _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
- }
-
- _rate_error = _rate_setpoint - roll_rate;
+ /* Transform setpoint to body angular rates */
+ _bodyrate_setpoint = _rate_setpoint - sinf(pitch) * yaw_rate_setpoint; //jacobian
+ /* Transform estimation to body angular rates */
+ float roll_bodyrate = roll_rate - sinf(pitch) * yaw_rate; //jacobian
- float ilimit_scaled = _integrator_max * scaler;
+ /* Calculate body angular rate error */
+ _rate_error = _bodyrate_setpoint - roll_bodyrate; //body angular rate error
- if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
- float id = _rate_error * k_i_rate * dt * scaler;
+ float id = _rate_error * dt;
/*
- * anti-windup: do not allow integrator to increase into the
- * wrong direction if actuator is at limit
- */
- if (_last_output < -_max_deflection_rad) {
+ * 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);
- } else if (_last_output > _max_deflection_rad) {
+ } else if (_last_output > 1.0f) {
/* only allow motion to center: decrease value */
id = math::min(id, 0.0f);
}
@@ -115,11 +152,14 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
}
/* integrator limit */
- _integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
- /* store non-limited output */
- _last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+ //xxx: until start detection is available: integral part in control signal is limited here
+ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+ //warnx("roll: _integrator: %.4f, _integrator_max: %.4f", (double)_integrator, (double)_integrator_max);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
- return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_RollController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 0d4ea9333..0799dbe03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -36,6 +36,7 @@
* Definition of a simple orthogonal roll PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
*
* Acknowledgements:
*
@@ -50,14 +51,21 @@
#include <stdbool.h>
#include <stdint.h>
+#include <systemlib/perf_counter.h>
-class __EXPORT ECL_RollController
+class __EXPORT ECL_RollController //XXX: create controller superclass
{
public:
ECL_RollController();
- float control(float roll_setpoint, float roll, float roll_rate,
- float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+ ~ECL_RollController();
+
+ float control_attitude(float roll_setpoint, float roll);
+
+ float control_bodyrate(float pitch,
+ float roll_rate, float yaw_rate,
+ float yaw_rate_setpoint,
+ float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f), float scaler = 1.0f, bool lock_integrator = false);
void reset_integrator();
@@ -66,18 +74,23 @@ public:
_tc = time_constant;
}
}
+
void set_k_p(float k_p) {
_k_p = k_p;
}
+
void set_k_i(float k_i) {
_k_i = k_i;
}
- void set_k_d(float k_d) {
- _k_d = k_d;
+
+ void set_k_ff(float k_ff) {
+ _k_ff = k_ff;
}
+
void set_integrator_max(float max) {
_integrator_max = max;
}
+
void set_max_rate(float max_rate) {
_max_rate = max_rate;
}
@@ -90,19 +103,24 @@ public:
return _rate_setpoint;
}
+ float get_desired_bodyrate() {
+ return _bodyrate_setpoint;
+ }
+
private:
uint64_t _last_run;
float _tc;
float _k_p;
float _k_i;
- float _k_d;
+ float _k_ff;
float _integrator_max;
float _max_rate;
float _last_output;
float _integrator;
float _rate_error;
float _rate_setpoint;
- float _max_deflection_rad;
+ float _bodyrate_setpoint;
+ 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 b786acf24..fe03b8065 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -44,29 +44,140 @@
#include <geo/geo.h>
#include <ecl/ecl.h>
#include <mathlib/mathlib.h>
+#include <systemlib/err.h>
ECL_YawController::ECL_YawController() :
_last_run(0),
- _last_error(0.0f),
+ _k_p(0.0f),
+ _k_i(0.0f),
+ _k_ff(0.0f),
+ _integrator_max(0.0f),
+ _max_rate(0.0f),
_last_output(0.0f),
- _last_rate_hp_out(0.0f),
- _last_rate_hp_in(0.0f),
- _k_d_last(0.0f),
- _integrator(0.0f)
+ _integrator(0.0f),
+ _rate_error(0.0f),
+ _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"))
{
+}
+
+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;
+ if (sqrtf(speed_body_u * speed_body_u + speed_body_v * speed_body_v + speed_body_w * speed_body_w) > _coordinated_min_speed) {
+ float denumerator = (speed_body_u * cosf(roll) * cosf(pitch) + speed_body_w * sinf(pitch));
+ if(fabsf(denumerator) > FLT_EPSILON) {
+ _rate_setpoint = (speed_body_w * roll_rate_setpoint + 9.81f * sinf(roll) * cosf(pitch) + speed_body_u * pitch_rate_setpoint * sinf(roll)) / denumerator;
+// warnx("yaw: speed_body_u %.f speed_body_w %1.f roll %.1f pitch %.1f denumerator %.1f _rate_setpoint %.1f", speed_body_u, speed_body_w, denumerator, _rate_setpoint);
+ }
+// if(counter % 20 == 0) {
+// warnx("denumerator: %.4f, speed_body_u: %.4f, speed_body_w: %.4f, cosf(roll): %.4f, cosf(pitch): %.4f, sinf(pitch): %.4f", (double)denumerator, (double)speed_body_u, (double)speed_body_w, (double)cosf(roll), (double)cosf(pitch), (double)sinf(pitch));
+// }
+ }
+
+ /* limit the rate */ //XXX: move to body angluar rates
+ if (_max_rate > 0.01f) {
+ _rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+ _rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+ }
+
+
+// counter++;
+
+ if(!isfinite(_rate_setpoint)){
+ warnx("yaw rate sepoint not finite");
+ _rate_setpoint = 0.0f;
+ }
+
+ return _rate_setpoint;
}
-float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
- float airspeed_min, float airspeed_max, float aspeed)
+float ECL_YawController::control_bodyrate(float roll, float pitch,
+ float pitch_rate, float yaw_rate,
+ 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();
+ float dt = (float)dt_micros * 1e-6f;
+
+ /* lock integral for long intervals */
+ if (dt_micros > 500000)
+ lock_integrator = true;
+
+ /* input conditioning */
+ if (!isfinite(airspeed)) {
+ /* airspeed is NaN, +- INF or not available, pick center of band */
+ airspeed = 0.5f * (airspeed_min + airspeed_max);
+ } else if (airspeed < airspeed_min) {
+ airspeed = airspeed_min;
+ }
+
+
+ /* Transform setpoint to body angular rates */
+ _bodyrate_setpoint = -sinf(roll) * pitch_rate_setpoint + cosf(roll)*cosf(pitch) * _rate_setpoint; //jacobian
+
+ /* Transform estimation to body angular rates */
+ float yaw_bodyrate = -sinf(roll) * pitch_rate + cosf(roll)*cosf(pitch) * yaw_rate; //jacobian
+
+ /* Calculate body angular rate error */
+ _rate_error = _bodyrate_setpoint - yaw_bodyrate; //body angular rate error
+
+ if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+ float id = _rate_error * dt;
+
+ /*
+ * 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);
+ } else if (_last_output > 1.0f) {
+ /* only allow motion to center: decrease value */
+ id = math::min(id, 0.0f);
+ }
+
+ _integrator += id;
+ }
+
+ /* integrator limit */
+ //xxx: until start detection is available: integral part in control signal is limited here
+ float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
+
+ /* Apply PI rate controller and store non-limited output */
+ _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler * scaler; //scaler is proportional to 1/airspeed
+ //warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
- float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
- return 0.0f;
+ return math::constrain(_last_output, -1.0f, 1.0f);
}
void ECL_YawController::reset_integrator()
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 66b227918..a360c14b8 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -35,54 +35,93 @@
* @file ecl_yaw_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ *
+ * Acknowledgements:
+ *
+ * The control design is based on a design
+ * by Paul Riseborough and Andrew Tridgell, 2013,
+ * which in turn is based on initial work of
+ * Jonathan Challinger, 2012.
*/
#ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H
#include <stdbool.h>
#include <stdint.h>
+#include <systemlib/perf_counter.h>
-class __EXPORT ECL_YawController
+class __EXPORT ECL_YawController //XXX: create controller superclass
{
public:
ECL_YawController();
- float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
- float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+ ~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);
+
+ float control_bodyrate( float roll, float pitch,
+ float pitch_rate, float yaw_rate,
+ float pitch_rate_setpoint,
+ float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator);
void reset_integrator();
- void set_k_side(float k_a) {
- _k_side = k_a;
+ void set_k_p(float k_p) {
+ _k_p = k_p;
}
+
void set_k_i(float k_i) {
_k_i = k_i;
}
- void set_k_d(float k_d) {
- _k_d = k_d;
- }
- void set_k_roll_ff(float k_roll_ff) {
- _k_roll_ff = k_roll_ff;
+
+ void set_k_ff(float k_ff) {
+ _k_ff = k_ff;
}
+
void set_integrator_max(float max) {
_integrator_max = max;
}
+ void set_max_rate(float max_rate) {
+ _max_rate = max_rate;
+ }
+
+ void set_coordinated_min_speed(float coordinated_min_speed) {
+ _coordinated_min_speed = coordinated_min_speed;
+ }
+
+
+ float get_rate_error() {
+ return _rate_error;
+ }
+
+ float get_desired_rate() {
+ return _rate_setpoint;
+ }
+
+ float get_desired_bodyrate() {
+ return _bodyrate_setpoint;
+ }
+
private:
uint64_t _last_run;
-
- float _k_side;
+ float _k_p;
float _k_i;
- float _k_d;
- float _k_roll_ff;
+ float _k_ff;
float _integrator_max;
-
- float _last_error;
+ float _max_rate;
+ float _roll_ff;
float _last_output;
- float _last_rate_hp_out;
- float _last_rate_hp_in;
- float _k_d_last;
float _integrator;
+ float _rate_error;
+ float _rate_setpoint;
+ float _bodyrate_setpoint;
+ float _coordinated_min_speed;
+ perf_counter_t _nonfinite_input_perf;
};
diff --git a/src/lib/ecl/ecl.h b/src/lib/ecl/ecl.h
index e0f207696..aa3c5000a 100644
--- a/src/lib/ecl/ecl.h
+++ b/src/lib/ecl/ecl.h
@@ -38,7 +38,6 @@
*/
#include <drivers/drv_hrt.h>
-#include <geo/geo.h>
#define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time \ No newline at end of file
+#define ecl_elapsed_time hrt_elapsed_time
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 27d76f959..d1c864d78 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -38,6 +38,8 @@
*
*/
+#include <float.h>
+
#include "ecl_l1_pos_controller.h"
float ECL_L1_Pos_Controller::nav_roll()
@@ -70,7 +72,7 @@ float ECL_L1_Pos_Controller::target_bearing()
float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
{
/* following [2], switching on L1 distance */
- return math::max(wp_radius, _L1_distance);
+ return math::min(wp_radius, _L1_distance);
}
bool ECL_L1_Pos_Controller::reached_loiter_target(void)
@@ -83,8 +85,8 @@ float ECL_L1_Pos_Controller::crosstrack_error(void)
return _crosstrack_error;
}
-void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
- const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+ const math::Vector<2> &ground_speed_vector)
{
/* this follows the logic presented in [1] */
@@ -94,7 +96,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float ltrack_vel;
/* get the direction between the last (visited) and next waypoint */
- _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+ _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_B(0), vector_B(1));
/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
@@ -103,7 +105,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_L1_distance = _L1_ratio * ground_speed;
/* calculate vector from A to B */
- math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+ math::Vector<2> vector_AB = get_local_planar_vector(vector_A, vector_B);
/*
* check if waypoints are on top of each other. If yes,
@@ -116,7 +118,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
vector_AB.normalize();
/* calculate the vector from waypoint A to the aircraft */
- math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
/* calculate crosstrack error (output only) */
_crosstrack_error = vector_AB % vector_A_to_airplane;
@@ -130,7 +132,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float alongTrackDist = vector_A_to_airplane * vector_AB;
/* estimate airplane position WRT to B */
- math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+ math::Vector<2> vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
/* calculate angle of airplane position vector relative to line) */
@@ -143,14 +145,14 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
/* calculate eta to fly to waypoint A */
/* unit vector from waypoint A to current position */
- math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+ math::Vector<2> vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
/* velocity across / orthogonal to line */
xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
/* velocity along line */
ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
- _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+ _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
/*
* If the AB vector and the vector from B to airplane point in the same
@@ -174,7 +176,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
eta = atan2f(xtrack_vel, ltrack_vel);
/* bearing from current position to L1 point */
- _nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+ _nav_bearing = atan2f(-vector_B_to_P_unit(1) , -vector_B_to_P_unit(0));
} else {
@@ -194,7 +196,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
float eta1 = asinf(sine_eta1);
eta = eta1 + eta2;
/* bearing from current position to L1 point */
- _nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+ _nav_bearing = atan2f(vector_AB(1), vector_AB(0)) + eta1;
}
@@ -209,8 +211,8 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
_bearing_error = eta;
}
-void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
- const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+ const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@@ -220,7 +222,7 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
float K_velocity = 2.0f * _L1_damping * omega;
/* update bearing to next waypoint */
- _target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+ _target_bearing = get_bearing_to_next_waypoint(vector_curr_position(0), vector_curr_position(1), vector_A(0), vector_A(1));
/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
@@ -229,10 +231,17 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
_L1_distance = _L1_ratio * ground_speed;
/* calculate the vector from waypoint A to current position */
- math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+ math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
- /* store the normalized vector from waypoint A to current position */
- math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+ math::Vector<2> vector_A_to_airplane_unit;
+
+ /* prevent NaN when normalizing */
+ if (vector_A_to_airplane.length() > FLT_EPSILON) {
+ /* store the normalized vector from waypoint A to current position */
+ vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+ } else {
+ vector_A_to_airplane_unit = vector_A_to_airplane;
+ }
/* calculate eta angle towards the loiter center */
@@ -280,26 +289,26 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, cons
*/
// XXX check switch over
- if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+ if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) ||
(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
_lateral_accel = lateral_accel_sp_center;
_circle_mode = false;
/* angle between requested and current velocity vector */
_bearing_error = eta;
/* bearing from current position to L1 point */
- _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+ _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
} else {
_lateral_accel = lateral_accel_sp_circle;
_circle_mode = true;
_bearing_error = 0.0f;
/* bearing from current position to L1 point */
- _nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+ _nav_bearing = atan2f(-vector_A_to_airplane_unit(1) , -vector_A_to_airplane_unit(0));
}
}
-void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed_vector)
{
/* the complete guidance logic in this section was proposed by [2] */
@@ -352,14 +361,11 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
}
-math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+math::Vector<2> ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const
{
/* this is an approximation for small angles, proposed by [2] */
- math::Vector2f out;
-
- out.setX(math::radians((target.getX() - origin.getX())));
- out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+ math::Vector<2> out(math::radians((target(0) - origin(0))), math::radians((target(1) - origin(1))*cosf(math::radians(origin(0)))));
return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
}
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 7a3c42a92..5c0804a39 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -160,8 +160,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
- void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
- const math::Vector2f &ground_speed);
+ void navigate_waypoints(const math::Vector<2> &vector_A, const math::Vector<2> &vector_B, const math::Vector<2> &vector_curr_position,
+ const math::Vector<2> &ground_speed);
/**
@@ -172,8 +172,8 @@ public:
*
* @return sets _lateral_accel setpoint
*/
- void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
- const math::Vector2f &ground_speed_vector);
+ void navigate_loiter(const math::Vector<2> &vector_A, const math::Vector<2> &vector_curr_position, float radius, int8_t loiter_direction,
+ const math::Vector<2> &ground_speed_vector);
/**
@@ -185,7 +185,7 @@ public:
*
* @return sets _lateral_accel setpoint
*/
- void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+ void navigate_heading(float navigation_heading, float current_heading, const math::Vector<2> &ground_speed);
/**
@@ -260,7 +260,7 @@ private:
* @param wp The point to convert to into the local coordinates, in WGS84 coordinates
* @return The vector in meters pointing from the reference position to the coordinates
*/
- math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+ math::Vector<2> get_local_planar_vector(const math::Vector<2> &origin, const math::Vector<2> &target) const;
};
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-02 21:28:24 +0000