aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp13
-rw-r--r--src/lib/ecl/attitude_fw/ecl_roll_controller.cpp15
-rw-r--r--src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp12
-rw-r--r--src/modules/fw_att_control/fw_att_control_main.cpp42
4 files changed, 72 insertions, 10 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 9584924cc..a3f5199b1 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -67,7 +67,11 @@ ECL_PitchController::ECL_PitchController() :
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))) {
+ warnx("not controlling pitch");
+ return _rate_setpoint;
+ }
/* flying inverted (wings upside down) ? */
bool inverted = false;
@@ -123,6 +127,13 @@ 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))) {
+ 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();
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 2e86c72dc..6ad00049d 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -65,6 +65,10 @@ ECL_RollController::ECL_RollController() :
float ECL_RollController::control_attitude(float roll_setpoint, float roll)
{
+ /* Do not calculate control signal with bad inputs */
+ if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+ return _rate_setpoint;
+ }
/* Calculate error */
float roll_error = roll_setpoint - roll;
@@ -86,6 +90,13 @@ 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))) {
+ 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 +133,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);
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 255776765..d43e0314e 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -66,6 +66,12 @@ 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))) {
+ return _rate_setpoint;
+ }
// static int counter = 0;
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_rate_setpoint = 0.0f;
@@ -103,6 +109,12 @@ 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))) {
+ 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();
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 6943239e5..59854e734 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -592,6 +592,8 @@ FixedwingAttitudeControl::task_main()
while (!_task_should_exit) {
+ static int loop_counter = 0;
+
/* wait for up to 500ms for data */
int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
@@ -755,7 +757,9 @@ FixedwingAttitudeControl::task_main()
speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
} else {
- warnx("Did not get a valid R\n");
+ if (loop_counter % 10 == 0) {
+ warnx("Did not get a valid R\n");
+ }
}
/* Run attitude controllers */
@@ -773,7 +777,10 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
if (!isfinite(roll_u)) {
- warnx("roll_u %.4f", (double)roll_u);
+ _roll_ctrl.reset_integrator();
+ if (loop_counter % 10 == 0) {
+ warnx("roll_u %.4f", (double)roll_u);
+ }
}
float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -782,8 +789,21 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
if (!isfinite(pitch_u)) {
- warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
- (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
+ _pitch_ctrl.reset_integrator();
+ if (loop_counter % 10 == 0) {
+ warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+ " airspeed %.4f, airspeed_scaling %.4f,"
+ " roll_sp %.4f, pitch_sp %.4f,"
+ " _roll_ctrl.get_desired_rate() %.4f,"
+ " _pitch_ctrl.get_desired_rate() %.4f"
+ " att_sp.roll_body %.4f",
+ (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
+ (double)airspeed, (double)airspeed_scaling,
+ (double)roll_sp, (double)pitch_sp,
+ (double)_roll_ctrl.get_desired_rate(),
+ (double)_pitch_ctrl.get_desired_rate(),
+ (double)_att_sp.roll_body);
+ }
}
float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -792,16 +812,23 @@ FixedwingAttitudeControl::task_main()
_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
if (!isfinite(yaw_u)) {
- warnx("yaw_u %.4f", (double)yaw_u);
+ _yaw_ctrl.reset_integrator();
+ if (loop_counter % 10 == 0) {
+ warnx("yaw_u %.4f", (double)yaw_u);
+ }
}
/* throttle passed through */
_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
if (!isfinite(throttle_sp)) {
- warnx("throttle_sp %.4f", (double)throttle_sp);
+ if (loop_counter % 10 == 0) {
+ warnx("throttle_sp %.4f", (double)throttle_sp);
+ }
}
} else {
- warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+ if (loop_counter % 10 == 0) {
+ warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+ }
}
/*
@@ -865,6 +892,7 @@ FixedwingAttitudeControl::task_main()
}
+ loop_counter++;
perf_end(_loop_perf);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-19 01:21:18 +0000