aboutsummaryrefslogtreecommitdiff
path: root/src/modules
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules')
-rw-r--r--src/modules/commander/commander.cpp34
-rw-r--r--src/modules/commander/state_machine_helper.cpp26
-rw-r--r--src/modules/mc_att_control/mc_att_control_main.cpp17
-rw-r--r--src/modules/systemlib/circuit_breaker.c93
-rw-r--r--src/modules/systemlib/circuit_breaker.h55
-rw-r--r--src/modules/systemlib/module.mk5
-rw-r--r--src/modules/uORB/topics/vehicle_status.h4
7 files changed, 228 insertions, 6 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bb42889ea..507f18e24 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
#include <fcntl.h>
#include <errno.h>
#include <systemlib/err.h>
+#include <systemlib/circuit_breaker.h>
#include <debug.h>
#include <sys/prctl.h>
#include <sys/stat.h>
@@ -76,6 +77,7 @@
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/safety.h>
+#include <uORB/topics/system_power.h>
#include <uORB/topics/mission_result.h>
#include <uORB/topics/telemetry_status.h>
@@ -700,6 +702,12 @@ 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;
+
+ // CIRCUIT BREAKERS
+ status.circuit_breaker_engaged_power_check = false;
+
/* publish initial state */
status_pub = orb_advertise(ORB_ID(vehicle_status), &status);
@@ -846,6 +854,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 */
@@ -903,6 +916,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 */
@@ -945,6 +961,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 74885abf6..1997729d4 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -135,6 +135,32 @@ arming_state_transition(struct vehicle_status_s *status, /// current
valid_transition = false;
}
+ // 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.");
+ }
+
+ 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/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);
+ }
}
}
}
diff --git a/src/modules/systemlib/circuit_breaker.c b/src/modules/systemlib/circuit_breaker.c
new file mode 100644
index 000000000..b59531d69
--- /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 <systemlib/param/param.h>
+#include <systemlib/circuit_breaker.h>
+
+/**
+ * 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 000000000..86439e2a5
--- /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 <stdbool.h>
+
+__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 3953b757d..147903aa0 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
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 5dc46dacb..56590047f 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -191,6 +191,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 */
@@ -220,6 +222,8 @@ struct vehicle_status_s {
uint16_t errors_count2;
uint16_t errors_count3;
uint16_t errors_count4;
+
+ bool circuit_breaker_engaged_power_check;
};
/**
generated by cgit v1.2.3 (git 2.39.1) at 2024-08-22 01:43:21 +0000