From c7bf00562d685b513e1720f558fee5840aca151b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 08:22:54 +0200 Subject: [PATCH 1/7] commander: Enforce (in presence of power sensing) that a) system is not purely servo rail powered and b) power rail voltage is higher than 4.5V on the main avionics rail --- src/modules/commander/commander.cpp | 27 +++++++++++++++++++ .../commander/state_machine_helper.cpp | 22 +++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 ++ 3 files changed, 51 insertions(+) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5c0628a166..55c74fdff0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -76,6 +76,7 @@ #include #include #include +#include #include #include @@ -717,6 +718,9 @@ int commander_thread_main(int argc, char *argv[]) status.counter++; status.timestamp = hrt_absolute_time(); + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = -1.0f; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -853,6 +857,11 @@ int commander_thread_main(int argc, char *argv[]) struct position_setpoint_triplet_s pos_sp_triplet; memset(&pos_sp_triplet, 0, sizeof(pos_sp_triplet)); + /* Subscribe to system power */ + int system_power_sub = orb_subscribe(ORB_ID(system_power)); + struct system_power_s system_power; + memset(&system_power, 0, sizeof(system_power)); + control_status_leds(&status, &armed, true); /* now initialized */ @@ -933,6 +942,24 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); } + orb_check(system_power_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(system_power), system_power_sub, &system_power); + + if (hrt_elapsed_time(&system_power.timestamp) < 200000) { + if (system_power.servo_valid && + !system_power.brick_valid && + !system_power.usb_connected) { + /* flying only on servo rail, this is unsafe */ + status.condition_power_input_valid = false; + } else { + status.condition_power_input_valid = true; + status.avionics_power_rail_voltage = system_power.voltage5V_v; + } + } + } + check_valid(diff_pres.timestamp, DIFFPRESS_TIMEOUT, true, &(status.condition_airspeed_valid), &status_changed); /* update safety topic */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 87309b238a..818974648e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,6 +144,28 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } + // Fail transition if power is not good + if (!status->condition_power_input_valid) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; + } + + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { + + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } + + valid_transition = false; + } + } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { new_arming_state = ARMING_STATE_STANDBY_ERROR; } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ea20a317a6..2ed0d6ad45 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -179,6 +179,8 @@ struct vehicle_status_s { bool condition_local_altitude_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ + bool condition_power_input_valid; /**< set if input power is valid */ + float avionics_power_rail_voltage; /**< voltage of the avionics power rail */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception lost */ From 032fe389a5630a22c9da4642d79b3fae05a213f5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:11 +0200 Subject: [PATCH 2/7] systemlib: Add circuit breakers --- src/modules/systemlib/circuit_breaker.c | 93 +++++++++++++++++++++++++ src/modules/systemlib/circuit_breaker.h | 55 +++++++++++++++ src/modules/systemlib/module.mk | 5 +- 3 files changed, 151 insertions(+), 2 deletions(-) create mode 100644 src/modules/systemlib/circuit_breaker.c create mode 100644 src/modules/systemlib/circuit_breaker.h diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c new file mode 100644 index 0000000000..b59531d69b --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.c @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.c + * + * Circuit breaker parameters. + * Analog to real aviation circuit breakers these parameters + * allow to disable subsystems. They are not supported as standard + * operation procedure and are only provided for development purposes. + * To ensure they are not activated accidentally, the associated + * parameter needs to set to the key (magic). + */ + +#include +#include + +/** + * Circuit breaker for power supply check + * + * Setting this parameter to 894281 will disable the power valid + * checks in the commander. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 894281 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_SUPPLY_CHK, 0); + +/** + * Circuit breaker for rate controller output + * + * Setting this parameter to 140253 will disable the rate + * controller uORB publication. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 140253 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_RATE_CTRL, 0); + +/** + * Circuit breaker for IO safety + * + * Setting this parameter to 894281 will disable IO safety. + * WARNING: ENABLING THIS CIRCUIT BREAKER IS AT OWN RISK + * + * @min 0 + * @max 22027 + * @group Circuit Breaker + */ +PARAM_DEFINE_INT32(CBRK_IO_SAFETY, 0); + +bool circuit_breaker_enabled(const char* breaker, int32_t magic) +{ + int32_t val; + (void)param_get(param_find(breaker), val); + + return (val == magic); +} + diff --git a/src/modules/systemlib/circuit_breaker.h b/src/modules/systemlib/circuit_breaker.h new file mode 100644 index 0000000000..86439e2a58 --- /dev/null +++ b/src/modules/systemlib/circuit_breaker.h @@ -0,0 +1,55 @@ +/**************************************************************************** + * + * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/* + * @file circuit_breaker.h + * + * Circuit breaker functionality. + */ + +#ifndef CIRCUIT_BREAKER_H_ +#define CIRCUIT_BREAKER_H_ + +#define CBRK_SUPPLY_CHK_KEY 894281 +#define CBRK_RATE_CTRL_KEY 140253 +#define CBRK_IO_SAFETY_KEY 22027 + +#include + +__BEGIN_DECLS + +__EXPORT bool circuit_breaker_enabled(const char* breaker, int32_t magic); + +__END_DECLS + +#endif /* CIRCUIT_BREAKER_H_ */ diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk index 3953b757d2..147903aa03 100644 --- a/src/modules/systemlib/module.mk +++ b/src/modules/systemlib/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2012 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -52,5 +52,6 @@ SRCS = err.c \ rc_check.c \ otp.c \ board_serial.c \ - pwm_limit/pwm_limit.c + pwm_limit/pwm_limit.c \ + circuit_breaker.c From 064329dd099819b0f0b8a9a7bc0233440fdf6df1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:34:32 +0200 Subject: [PATCH 3/7] commander: put circuit breaker into effect --- src/modules/commander/commander.cpp | 7 ++++ .../commander/state_machine_helper.cpp | 32 +++++++++++-------- 2 files changed, 25 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 55c74fdff0..71067ac4f4 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include @@ -721,6 +722,9 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = true; status.avionics_power_rail_voltage = -1.0f; + // CIRCUIT BREAKERS + status.circuit_breaker_engaged_power_check = false; + /* publish initial state */ status_pub = orb_advertise(ORB_ID(vehicle_status), &status); @@ -907,6 +911,9 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); + + status.circuit_breaker_engaged_power_check = circuit_breaker_enabled("CBRK_SUPPLY_CHK", CBRK_SUPPLY_CHK_KEY); + status_changed = true; /* re-check RC calibration */ diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 818974648e..84f4d03af8 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -144,26 +144,30 @@ arming_state_transition(struct vehicle_status_s *status, /// current valid_transition = false; } - // Fail transition if power is not good - if (!status->condition_power_input_valid) { + // Perform power checks only if circuit breaker is not + // engaged for these checks + if (!status->circuit_breaker_engaged_power_check) { + // Fail transition if power is not good + if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); + } + + valid_transition = false; } - valid_transition = false; - } + // Fail transition if power levels on the avionics rail + // are insufficient + if ((status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.5f)) { - // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { + if (mavlink_fd) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + } - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); + valid_transition = false; } - - valid_transition = false; } } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) { From 55ad5588a88bc4957583067beeffe20aa2fcd442 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:16 +0200 Subject: [PATCH 4/7] system status topic: Add support for circuit breaker --- src/modules/uORB/topics/vehicle_status.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2ed0d6ad45..85962883d1 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -208,6 +208,8 @@ struct vehicle_status_s { uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; + + bool circuit_breaker_engaged_power_check; }; /** From 77365188ada556c5953f6f026566d4912e8c6e76 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:35:37 +0200 Subject: [PATCH 5/7] mc_att_control: Support circuit breakers --- .../mc_att_control/mc_att_control_main.cpp | 17 +++++++++++++---- 1 file 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 20e016da34..19c10198c2 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 #include #include +#include #include #include @@ -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); + } } } } From 8ea5fd20c1c268c6baf145606186592bf5bd3699 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 May 2014 20:36:05 +0200 Subject: [PATCH 6/7] px4io driver: Add support for circuit breakers --- src/drivers/px4io/px4io.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 972f451485..c6acfe6be8 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -72,6 +72,7 @@ #include #include #include +#include #include #include @@ -1010,6 +1011,19 @@ PX4IO::task_main() } } + int32_t safety_param_val; + param_t safety_param = param_find("RC_FAILS_THR"); + + if (safety_param != PARAM_INVALID) { + + param_get(safety_param, &safety_param_val); + + if (safety_param_val == PX4IO_FORCE_SAFETY_MAGIC) { + /* disable IO safety if circuit breaker asked for it */ + (void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, safety_param_val); + } + } + } } From 799f568ab48ca3446b24f41290589f7f3c39ef0f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 30 Jun 2014 14:33:56 +0200 Subject: [PATCH 7/7] commander: Minor checks / improvements to power enforce patch --- src/modules/commander/commander.cpp | 4 +++- src/modules/commander/state_machine_helper.cpp | 16 +++++----------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 507f18e247..a15712a91e 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -974,8 +974,10 @@ int commander_thread_main(int argc, char *argv[]) status.condition_power_input_valid = false; } else { status.condition_power_input_valid = true; - status.avionics_power_rail_voltage = system_power.voltage5V_v; } + + /* copy avionics voltage */ + status.avionics_power_rail_voltage = system_power.voltage5V_v; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 1997729d48..7a61d481be 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -141,22 +141,16 @@ arming_state_transition(struct vehicle_status_s *status, /// current // Fail transition if power is not good if (!status->condition_power_input_valid) { - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); - } - + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Connect power module."); valid_transition = false; } // Fail transition if power levels on the avionics rail - // are insufficient - if ((status->avionics_power_rail_voltage > 0.0f) && - (status->avionics_power_rail_voltage < 4.5f)) { - - if (mavlink_fd) { - mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); - } + // are measured but are insufficient + if (status->condition_power_input_valid && (status->avionics_power_rail_voltage > 0.0f) && + (status->avionics_power_rail_voltage < 4.9f)) { + mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Avionics power low: %6.2f V.", status->avionics_power_rail_voltage); valid_transition = false; } }