diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-25 20:35:37 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-05-25 20:35:37 +0200 |
commit | 77365188ada556c5953f6f026566d4912e8c6e76 (patch) | |
tree | 2e1224d5683beb02586111ca3d2f8ae2d768e8bf /src/modules | |
parent | 55ad5588a88bc4957583067beeffe20aa2fcd442 (diff) | |
download | px4-firmware-77365188ada556c5953f6f026566d4912e8c6e76.tar.gz px4-firmware-77365188ada556c5953f6f026566d4912e8c6e76.tar.bz2 px4-firmware-77365188ada556c5953f6f026566d4912e8c6e76.zip |
mc_att_control: Support circuit breakers
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/mc_att_control/mc_att_control_main.cpp | 17 |
1 files changed, 13 insertions, 4 deletions
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 20e016da3..19c10198c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -72,6 +72,7 @@ #include <systemlib/err.h> #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> +#include <systemlib/circuit_breaker.h> #include <lib/mathlib/mathlib.h> #include <lib/geo/geo.h> @@ -123,6 +124,8 @@ private: orb_advert_t _v_rates_sp_pub; /**< rate setpoint publication */ orb_advert_t _actuators_0_pub; /**< attitude actuator controls publication */ + bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */ + struct vehicle_attitude_s _v_att; /**< vehicle attitude */ struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */ struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */ @@ -267,6 +270,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _v_rates_sp_pub(-1), _actuators_0_pub(-1), + _actuators_0_circuit_breaker_enabled(false), + /* performance counters */ _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) @@ -402,6 +407,8 @@ MulticopterAttitudeControl::parameters_update() param_get(_params_handles.acro_yaw_max, &v); _params.acro_rate_max(2) = math::radians(v); + _actuators_0_circuit_breaker_enabled = circuit_breaker_enabled("CBRK_RATE_CTRL", CBRK_RATE_CTRL_KEY); + return OK; } @@ -840,11 +847,13 @@ MulticopterAttitudeControl::task_main() _actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f; _actuators.timestamp = hrt_absolute_time(); - if (_actuators_0_pub > 0) { - orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); + if (!_actuators_0_circuit_breaker_enabled) { + if (_actuators_0_pub > 0) { + orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators); - } else { - _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } else { + _actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators); + } } } } |