diff options
author | Julian Oes <julian@oes.ch> | 2014-05-26 20:19:11 +0200 |
---|---|---|
committer | Julian Oes <julian@oes.ch> | 2014-05-26 20:19:11 +0200 |
commit | 063caba36bd2fe26eb4bfa8e546e9551ccc05519 (patch) | |
tree | d8ea5015111793800d945fbc33505088cf5fe12d /src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | |
parent | 68352cb923d366b66bb68c8d946c4960b6f7ff1a (diff) | |
parent | 36495cdb62e21b30a5a1851ec802c9f6a40c1171 (diff) | |
download | px4-firmware-063caba36bd2fe26eb4bfa8e546e9551ccc05519.tar.gz px4-firmware-063caba36bd2fe26eb4bfa8e546e9551ccc05519.tar.bz2 px4-firmware-063caba36bd2fe26eb4bfa8e546e9551ccc05519.zip |
Merge branch 'master' into navigator_rewrite
Conflicts:
src/drivers/gps/gps.cpp
src/drivers/gps/mtk.cpp
src/modules/commander/commander.cpp
src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
src/modules/navigator/mission.cpp
src/modules/navigator/mission.h
src/modules/navigator/navigator_main.cpp
src/modules/navigator/navigator_state.h
src/modules/position_estimator_inav/position_estimator_inav_main.c
Diffstat (limited to 'src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp')
-rw-r--r-- | src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp | 20 |
1 files changed, 20 insertions, 0 deletions
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp index 255776765..e53ffc644 100644 --- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp +++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp @@ -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(); |