1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
|
/****************************************************************************
*
* Copyright (C) 2012 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 Safety button logic.
*/
#include <nuttx/config.h>
#include <stdio.h>
#include <stdbool.h>
#include <fcntl.h>
#include <unistd.h>
#include <debug.h>
#include <stdlib.h>
#include <errno.h>
#include <nuttx/clock.h>
#include <drivers/drv_hrt.h>
#include "px4io.h"
static struct hrt_call arming_call;
static struct hrt_call heartbeat_call;
static struct hrt_call failsafe_call;
/*
* Count the number of times in a row that we see the arming button
* held down.
*/
static unsigned counter = 0;
/*
* Define the various LED flash sequences for each system state.
*/
#define LED_PATTERN_SAFE 0xffff /**< always on */
#define LED_PATTERN_VECTOR_FLIGHT_MODE_OK 0xFFFE /**< always on with short break */
#define LED_PATTERN_FMU_ARMED 0x4444 /**< slow blinking */
#define LED_PATTERN_IO_ARMED 0x5555 /**< fast blinking */
#define LED_PATTERN_IO_FMU_ARMED 0x5050 /**< long off then double blink */
static unsigned blink_counter = 0;
/*
* IMPORTANT: The arming state machine critically
* depends on using the same threshold
* for arming and disarming. Since disarming
* is quite deadly for the system, a similar
* length can be justified.
*/
#define ARM_COUNTER_THRESHOLD 10
static bool safety_button_pressed;
static void safety_check_button(void *arg);
static void heartbeat_blink(void *arg);
static void failsafe_blink(void *arg);
void
safety_init(void)
{
/* arrange for the button handler to be called at 10Hz */
hrt_call_every(&arming_call, 1000, 100000, safety_check_button, NULL);
/* arrange for the heartbeat handler to be called at 4Hz */
hrt_call_every(&heartbeat_call, 1000, 250000, heartbeat_blink, NULL);
/* arrange for the failsafe blinker to be called at 8Hz */
hrt_call_every(&failsafe_call, 1000, 125000, failsafe_blink, NULL);
}
static void
safety_check_button(void *arg)
{
/*
* Debounce the safety button, change state if it has been held for long enough.
*
*/
safety_button_pressed = BUTTON_SAFETY;
/*
* Keep pressed for a while to arm.
*
* Note that the counting sequence has to be same length
* for arming / disarming in order to end up as proper
* state machine, keep ARM_COUNTER_THRESHOLD the same
* length in all cases of the if/else struct below.
*/
if (safety_button_pressed && !system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to armed state and notify the FMU */
system_state.armed = true;
counter++;
system_state.fmu_report_due = true;
}
/* Disarm quickly */
} else if (safety_button_pressed && system_state.armed) {
if (counter < ARM_COUNTER_THRESHOLD) {
counter++;
} else if (counter == ARM_COUNTER_THRESHOLD) {
/* change to disarmed state and notify the FMU */
system_state.armed = false;
counter++;
system_state.fmu_report_due = true;
}
} else {
counter = 0;
}
/* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */
uint16_t pattern = LED_PATTERN_SAFE;
if (system_state.armed) {
if (system_state.arm_ok) {
pattern = LED_PATTERN_IO_FMU_ARMED;
} else {
pattern = LED_PATTERN_IO_ARMED;
}
} else if (system_state.arm_ok) {
pattern = LED_PATTERN_FMU_ARMED;
} else if (system_state.vector_flight_mode_ok) {
pattern = LED_PATTERN_VECTOR_FLIGHT_MODE_OK;
}
/* Turn the LED on if we have a 1 at the current bit position */
LED_SAFETY(pattern & (1 << blink_counter++));
if (blink_counter > 15) {
blink_counter = 0;
}
}
static void
heartbeat_blink(void *arg)
{
static bool heartbeat = false;
/* XXX add flags here that need to be frobbed by various loops */
LED_BLUE(heartbeat = !heartbeat);
}
static void
failsafe_blink(void *arg)
{
static bool failsafe = false;
/* blink the failsafe LED if we don't have FMU input */
if (!system_state.mixer_fmu_available) {
failsafe = !failsafe;
} else {
failsafe = false;
}
LED_AMBER(failsafe);
}
|