Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-08 10:50:19 +08:00
Code Issues Actions 23 Packages Projects Releases Wiki Activity

IO driver: Fixx lockdown flag handling

Browse Source
This commit is contained in:
Lorenz Meier
2016-02-23 13:05:51 +01:00
parent 5dfd9dd68b
commit 738d3cb123
2 changed files with 13 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/px4io/CMakeLists.txt
+1 -1
View File
@@ -33,7 +33,7 @@
px4_add_module(
MODULE drivers__px4io
MAIN px4io
STACK 1200
STACK 1800
COMPILE_FLAGS
-Os
SRCS
src/drivers/px4io/px4io.cpp
+12 -9
View File
@@ -273,7 +273,8 @@ private:
/* cached IO state */
uint16_t _status; ///< Various IO status flags
uint16_t _alarms; ///< Various IO alarms
uint16_t _last_written_arming; ///< the last written arming state reg
uint16_t _last_written_arming_s; ///< the last written arming state reg
uint16_t _last_written_arming_c; ///< the last written arming state reg
/* subscribed topics */
int _t_actuator_controls_0; ///< actuator controls group 0 topic
@@ -510,7 +511,8 @@ PX4IO::PX4IO(device::Device *interface) :
_perf_sample_latency(perf_alloc(PC_ELAPSED, "io latency")),
_status(0),
_alarms(0),
_last_written_arming(0),
_last_written_arming_s(0),
_last_written_arming_c(0),
_t_actuator_controls_0(-1),
_t_actuator_controls_1(-1),
_t_actuator_controls_2(-1),
@@ -836,7 +838,8 @@ PX4IO::init()
PX4IO_P_SETUP_ARMING_FMU_ARMED |
PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK |
PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK |
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0);
PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE |
PX4IO_P_SETUP_ARMING_LOCKDOWN, 0);
if (_rc_handling_disabled) {
ret = io_disable_rc_handling();
@@ -1331,9 +1334,11 @@ PX4IO::io_set_arming_state()
if (armed.lockdown && !_lockdown_override) {
set |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = true;
} else {
} else if (!armed.lockdown && _lockdown_override) {
clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
_lockdown_override = false;
}
/* Do not set failsafe if circuit breaker is enabled */
@@ -1369,11 +1374,9 @@ PX4IO::io_set_arming_state()
}
}
uint16_t new_arming_state = set;
new_arming_state &= ~(clear);
if (_last_written_arming != new_arming_state) {
_last_written_arming = new_arming_state;
if (_last_written_arming_s != set || _last_written_arming_c != clear) {
_last_written_arming_s = set;
_last_written_arming_c = clear;
return io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, clear, set);
}