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
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
|
/****************************************************************************
*
* 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 PPM input decoder.
*
* Works in conjunction with the HRT driver.
*/
#include <nuttx/config.h>
#include <nuttx/arch.h>
#include <nuttx/irq.h>
#include <sys/types.h>
#include <stdbool.h>
#include <assert.h>
#include <debug.h>
#include <time.h>
#include <queue.h>
#include <errno.h>
#include <string.h>
#include <stdio.h>
#include <fcntl.h>
#include <arch/board/board.h>
#include <arch/board/drv_ppm_input.h>
#include <arch/board/up_hrt.h>
#include "chip.h"
#include "up_internal.h"
#include "up_arch.h"
#include "stm32_internal.h"
#include "stm32_gpio.h"
#include "stm32_tim.h"
#ifdef CONFIG_HRT_PPM
# ifndef CONFIG_HRT_TIMER
# error CONFIG_HRT_PPM requires CONFIG_HRT_TIMER
# endif
/*
* PPM decoder tuning parameters.
*
* The PPM decoder works as follows.
*
* Initially, the decoder waits in the UNSYNCH state for two edges
* separated by PPM_MIN_START. Once the second edge is detected,
* the decoder moves to the ARM state.
*
* The ARM state expects an edge within PPM_MAX_PULSE_WIDTH, being the
* timing mark for the first channel. If this is detected, it moves to
* the INACTIVE state.
*
* The INACTIVE phase waits for and discards the next edge, as it is not
* significant. Once the edge is detected, it moves to the ACTIVE stae.
*
* The ACTIVE state expects an edge within PPM_MAX_PULSE_WIDTH, and when
* received calculates the time from the previous mark and records
* this time as the value for the next channel.
*
* If at any time waiting for an edge, the delay from the previous edge
* exceeds PPM_MIN_START the frame is deemed to have ended and the recorded
* values are advertised to clients.
*/
#define PPM_MAX_PULSE_WIDTH 500 /* maximum width of a pulse */
#define PPM_MIN_CHANNEL_VALUE 750 /* shortest valid channel signal */
#define PPM_MAX_CHANNEL_VALUE 2400 /* longest valid channel signal */
#define PPM_MIN_START 5000 /* shortest valid start gap */
/* Input timeout - after this interval we assume signal is lost */
#define PPM_INPUT_TIMEOUT 100 * 1000 /* 100ms */
/* Number of same-sized frames required to 'lock' */
#define PPM_CHANNEL_LOCK 3 /* should be less than the input timeout */
/* decoded PPM buffer */
#define PPM_MIN_CHANNELS 4
#define PPM_MAX_CHANNELS 12
static uint16_t ppm_buffer[PPM_MAX_CHANNELS];
static unsigned ppm_decoded_channels;
static uint16_t ppm_temp_buffer[PPM_MAX_CHANNELS];
/* PPM decoder state machine */
static struct {
uint16_t last_edge; /* last capture time */
uint16_t last_mark; /* last significant edge */
unsigned next_channel;
enum {
UNSYNCH = 0,
ARM,
ACTIVE,
INACTIVE
} phase;
} ppm;
/* last time we got good data */
static hrt_abstime ppm_timestamp;
#ifndef CONFIG_DISABLE_MQUEUE
/* message queue we advertise PPM data on */
static mqd_t ppm_message_queue;
#endif
/* set if PPM data has not been read */
static bool ppm_fresh_data;
/* PPM device node file ops */
static int ppm_read(struct file *filp, char *buffer, size_t len);
static int ppm_ioctl(struct file *filp, int cmd, unsigned long arg);
static const struct file_operations ppm_fops = {
.read = ppm_read,
.ioctl = ppm_ioctl
};
/*
* Initialise the PPM system for client use.
*/
int
ppm_input_init(const char *mq_name)
{
int err;
/* configure the PPM input pin */
stm32_configgpio(GPIO_PPM_IN);
/* and register the device node */
if (OK != (err = register_driver(PPM_DEVICE_NODE, &ppm_fops, 0666, NULL)))
return err;
#ifndef CONFIG_DISABLE_MQUEUE
if (mq_name != NULL) {
/* create the message queue */
struct mq_attr attr = {
.mq_maxmsg = 1,
.mq_msgsize = sizeof(ppm_buffer)
};
ppm_message_queue = mq_open(mq_name, O_WRONLY | O_CREAT | O_NONBLOCK, 0666, &attr);
if (ppm_message_queue < 0)
return -errno;
}
#endif
return OK;
}
/*
* Handle the PPM decoder state machine.
*/
void
ppm_input_decode(bool reset, uint16_t count)
{
uint16_t width;
uint16_t interval;
unsigned i;
/* if we missed an edge, we have to give up */
if (reset)
goto error;
/* how long since the last edge? */
width = count - ppm.last_edge;
ppm.last_edge = count;
/*
* If this looks like a start pulse, then push the last set of values
* and reset the state machine.
*
* Note that this is not a "high performance" design; it implies a whole
* frame of latency between the pulses being received and their being
* considered valid.
*/
if (width >= PPM_MIN_START) {
/*
* If the number of channels changes unexpectedly, we don't want
* to just immediately jump on the new count as it may be a result
* of noise or dropped edges. Instead, take a few frames to settle.
*/
if (ppm.next_channel != ppm_decoded_channels) {
static int new_channel_count;
static int new_channel_holdoff;
if (new_channel_count != ppm.next_channel) {
/* start the lock counter for the new channel count */
new_channel_count = ppm.next_channel;
new_channel_holdoff = PPM_CHANNEL_LOCK;
} else if (new_channel_holdoff > 0) {
/* this frame matched the last one, decrement the lock counter */
new_channel_holdoff--;
} else {
/* we have seen PPM_CHANNEL_LOCK frames with the new count, accept it */
ppm_decoded_channels = new_channel_count;
new_channel_count = 0;
}
} else {
/* frame channel count matches expected, let's use it */
if (ppm.next_channel > PPM_MIN_CHANNELS) {
for (i = 0; i < ppm.next_channel; i++)
ppm_buffer[i] = ppm_temp_buffer[i];
ppm_timestamp = hrt_absolute_time();
ppm_fresh_data = true;
#ifndef CONFIG_DISABLE_MQUEUE
/* advertise the new data to the message queue */
mq_send(ppm_message_queue, ppm_buffer, ppm_decoded_channels * sizeof(ppm_buffer[0]), 0);
#endif
}
}
/* reset for the next frame */
ppm.next_channel = 0;
/* next edge is the reference for the first channel */
ppm.phase = ARM;
return;
}
switch (ppm.phase) {
case UNSYNCH:
/* we are waiting for a start pulse - nothing useful to do here */
return;
case ARM:
/* we expect a pulse giving us the first mark */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
/* record the mark timing, expect an inactive edge */
ppm.last_mark = count;
ppm.phase = INACTIVE;
return;
case INACTIVE:
/* this edge is not interesting, but now we are ready for the next mark */
ppm.phase = ACTIVE;
/* note that we don't bother looking at the timing of this edge */
return;
case ACTIVE:
/* we expect a well-formed pulse */
if (width > PPM_MAX_PULSE_WIDTH)
goto error; /* pulse was too long */
/* determine the interval from the last mark */
interval = count - ppm.last_mark;
ppm.last_mark = count;
/* if the mark-mark timing is out of bounds, abandon the frame */
if ((interval < PPM_MIN_CHANNEL_VALUE) || (interval > PPM_MAX_CHANNEL_VALUE))
goto error;
/* if we have room to store the value, do so */
if (ppm.next_channel < PPM_MAX_CHANNELS)
ppm_temp_buffer[ppm.next_channel++] = interval;
ppm.phase = INACTIVE;
return;
}
/* the state machine is corrupted; reset it */
error:
/* we don't like the state of the decoder, reset it and try again */
ppm.phase = UNSYNCH;
ppm_decoded_channels = 0;
}
static int
ppm_read(struct file *filp, char *buffer, size_t len)
{
size_t avail;
/* the size of the returned data indicates the number of channels */
avail = ppm_decoded_channels * sizeof(ppm_buffer[0]);
/* if we have not decoded a frame, that's an I/O error */
if (avail == 0)
return -EIO;
/* if the caller's buffer is too small, that's also bad */
if (len < avail)
return -EFBIG;
/* if the caller doesn't want to block, and there is no fresh data, that's EWOULDBLOCK */
if ((filp->f_oflags & O_NONBLOCK) && (!ppm_fresh_data))
return -EWOULDBLOCK;
/*
* Return the channel data.
*
* Note that we have to block the HRT while copying to avoid the
* possibility that we'll get interrupted in the middle of copying
* a single value.
*/
irqstate_t flags = irqsave();
memcpy(buffer, ppm_buffer, avail);
ppm_fresh_data = false;
irqrestore(flags);
return OK;
}
static int
ppm_ioctl(struct file *filp, int cmd, unsigned long arg)
{
switch (cmd) {
case PPM_INPUT_STATUS:
/* if we have received a frame within the timeout, the signal is "good" */
if ((hrt_absolute_time() - ppm_timestamp) < PPM_INPUT_TIMEOUT) {
*(ppm_input_status_t *)arg = PPM_STATUS_SIGNAL_CURRENT;
} else {
/* reset the number of channels so that any attempt to read data will fail */
ppm_decoded_channels = 0;
*(ppm_input_status_t *)arg = PPM_STATUS_NO_SIGNAL;
}
return OK;
case PPM_INPUT_CHANNELS:
*(ppm_input_channel_count_t *)arg = ppm_decoded_channels;
return OK;
default:
return -ENOTTY;
}
}
#endif /* CONFIG_HRT_PPM */
|