PX4 Firmware
PX4 Autopilot Software http://px4.io
safety.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2017 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file safety.c
36  * Safety button logic.
37  *
38  * @author Lorenz Meier <lorenz@px4.io>
39  */
40 
41 #include <px4_platform_common/px4_config.h>
42 
43 #include <stdbool.h>
44 
45 #include <drivers/drv_hrt.h>
46 
47 #include "px4io.h"
48 
49 static struct hrt_call arming_call;
50 static struct hrt_call failsafe_call;
51 
52 /*
53  * Count the number of times in a row that we see the arming button
54  * held down.
55  */
56 static unsigned counter = 0;
57 
58 /*
59  * Define the various LED flash sequences for each system state.
60  */
61 #define LED_PATTERN_FMU_OK_TO_ARM 0x0003 /**< slow blinking */
62 #define LED_PATTERN_FMU_REFUSE_TO_ARM 0x5555 /**< fast blinking */
63 #define LED_PATTERN_IO_ARMED 0x5050 /**< long off, then double blink */
64 #define LED_PATTERN_FMU_ARMED 0x5500 /**< long off, then quad blink */
65 #define LED_PATTERN_IO_FMU_ARMED 0xffff /**< constantly on */
66 
67 static unsigned blink_counter = 0;
68 
69 /*
70  * IMPORTANT: The arming state machine critically
71  * depends on using the same threshold
72  * for arming and disarming. Since disarming
73  * is quite deadly for the system, a similar
74  * length can be justified.
75  */
76 #define ARM_COUNTER_THRESHOLD 10
77 
79 
80 static void safety_check_button(void *arg);
81 static void failsafe_blink(void *arg);
82 
83 void
85 {
86  /* arrange for the button handler to be called at 10Hz */
87  hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
88 }
89 
90 void
92 {
93  /* arrange for the failsafe blinker to be called at 8Hz */
94  hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
95 }
96 
97 static void
99 {
100  /*
101  * Debounce the safety button, change state if it has been held for long enough.
102  *
103  */
105 
106  /*
107  * Keep pressed for a while to arm.
108  *
109  * Note that the counting sequence has to be same length
110  * for arming / disarming in order to end up as proper
111  * state machine, keep ARM_COUNTER_THRESHOLD the same
112  * length in all cases of the if/else struct below.
113  */
116 
118  counter++;
119 
120  } else if (counter == ARM_COUNTER_THRESHOLD) {
121  /* switch to armed state */
122  atomic_modify_or(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
123  counter++;
124  }
125 
126  } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
127 
129  counter++;
130 
131  } else if (counter == ARM_COUNTER_THRESHOLD) {
132  /* change to disarmed state and notify the FMU */
133  atomic_modify_clear(&r_status_flags, PX4IO_P_STATUS_FLAGS_SAFETY_OFF);
134  counter++;
135  }
136 
137  } else {
138  counter = 0;
139  }
140 
141  /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
142  uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM;
143 
144  if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) {
146  pattern = LED_PATTERN_IO_FMU_ARMED;
147 
148  } else {
149  pattern = LED_PATTERN_IO_ARMED;
150  }
151 
153  pattern = LED_PATTERN_FMU_ARMED;
154 
155  } else if (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) {
156  pattern = LED_PATTERN_FMU_OK_TO_ARM;
157 
158  }
159 
160  /* Turn the LED on if we have a 1 at the current bit position */
161  LED_SAFETY(pattern & (1 << blink_counter++));
162 
163  if (blink_counter > 15) {
164  blink_counter = 0;
165  }
166 }
167 
168 static void
170 {
171  /* indicate that a serious initialisation error occured */
173  LED_AMBER(true);
174  return;
175  }
176 
177  static bool failsafe = false;
178 
179  /* blink the failsafe LED if we don't have FMU input */
181  failsafe = !failsafe;
182 
183  } else {
184  failsafe = false;
185  }
186 
187  LED_AMBER(failsafe);
188 }
#define PX4IO_P_STATUS_FLAGS_FMU_OK
Definition: protocol.h:113
#define BUTTON_SAFETY
Definition: px4io.h:183
#define LED_PATTERN_FMU_REFUSE_TO_ARM
fast blinking
Definition: safety.c:62
static void failsafe_blink(void *arg)
Definition: safety.c:169
void failsafe_led_init(void)
Definition: safety.c:91
#define r_setup_arming
Definition: px4io.h:118
High-resolution timer with callouts and timekeeping.
void atomic_modify_clear(volatile uint16_t *target, uint16_t modification)
Definition: px4io.c:102
#define LED_PATTERN_FMU_ARMED
long off, then quad blink
Definition: safety.c:64
#define LED_AMBER
Definition: drv_board_led.h:52
#define LED_PATTERN_IO_ARMED
long off, then double blink
Definition: safety.c:63
#define LED_SAFETY
Definition: drv_board_led.h:55
#define ARM_COUNTER_THRESHOLD
Definition: safety.c:76
void * arg
Definition: drv_hrt.h:78
static void safety_check_button(void *arg)
Definition: safety.c:98
#define PX4IO_P_SETUP_ARMING_IO_ARM_OK
Definition: protocol.h:183
void atomic_modify_or(volatile uint16_t *target, uint16_t modification)
Definition: px4io.c:95
#define r_status_flags
Definition: px4io.h:107
static unsigned counter
Definition: safety.c:56
#define LED_PATTERN_FMU_OK_TO_ARM
slow blinking
Definition: safety.c:61
static struct hrt_call arming_call
Definition: safety.c:49
static struct hrt_call failsafe_call
Definition: safety.c:50
__EXPORT void hrt_call_every(struct hrt_call *entry, hrt_abstime delay, hrt_abstime interval, hrt_callout callout, void *arg)
Call callout(arg) after delay, and then after every interval.
static unsigned blink_counter
Definition: safety.c:67
#define PX4IO_P_STATUS_FLAGS_SAFETY_OFF
Definition: protocol.h:119
General defines and structures for the PX4IO module firmware.
void safety_init(void)
Safety switch/LED.
Definition: safety.c:84
#define PX4IO_P_SETUP_ARMING_FMU_ARMED
Definition: protocol.h:184
#define PX4IO_P_STATUS_FLAGS_INIT_OK
Definition: protocol.h:117
Callout record.
Definition: drv_hrt.h:72
#define LED_PATTERN_IO_FMU_ARMED
constantly on
Definition: safety.c:65
static bool safety_button_pressed
Definition: safety.c:78