PX4 Firmware
PX4 Autopilot Software http://px4.io
cdevtest_example.cpp
Go to the documentation of this file.
1 
2 /****************************************************************************
3  *
4  * Copyright (C) 2015 Mark Charlebois. All rights reserved.
5  *
6  * Redistribution and use in source and binary forms, with or without
7  * modification, are permitted provided that the following conditions
8  * are met:
9  *
10  * 1. Redistributions of source code must retain the above copyright
11  * notice, this list of conditions and the following disclaimer.
12  * 2. Redistributions in binary form must reproduce the above copyright
13  * notice, this list of conditions and the following disclaimer in
14  * the documentation and/or other materials provided with the
15  * distribution.
16  * 3. Neither the name PX4 nor the names of its contributors may be
17  * used to endorse or promote products derived from this software
18  * without specific prior written permission.
19  *
20  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
21  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
22  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
23  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
24  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
25  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
26  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
27  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
28  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
29  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
30  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
31  * POSSIBILITY OF SUCH DAMAGE.
32  *
33  ****************************************************************************/
34 
35 /**
36  * @file cdevtest_example.cpp
37  * Example for Linux
38  *
39  * @author Mark Charlebois <charlebm@gmail.com>
40  */
41 
42 #include "cdevtest_example.h"
43 
44 #include <px4_platform_common/tasks.h>
45 #include <px4_platform_common/time.h>
46 #include <drivers/drv_device.h>
47 #include <lib/cdev/CDev.hpp>
48 #include <unistd.h>
49 #include <stdio.h>
50 
51 px4::AppState CDevExample::appState;
52 
53 #define TESTDEV "/dev/cdevtest"
54 
55 static bool g_exit = false;
56 
57 static int writer_main(int argc, char *argv[])
58 {
59  char buf[1];
60 
61  int fd = px4_open(TESTDEV, PX4_F_WRONLY);
62 
63  if (fd < 0) {
64  PX4_INFO("Writer: Open failed %d", fd);
65  return PX4_ERROR;
66  }
67 
68  int ret = 0;
69  int i = 0;
70 
71  while (!g_exit) {
72  // Wait for 2 seconds
73  PX4_INFO("Writer: Sleeping for 2 sec");
74  ret = px4_sleep(2);
75 
76  if (ret < 0) {
77  PX4_INFO("Writer: sleep failed %d %d", ret, errno);
78  return ret;
79  }
80 
81  buf[0] = 'A' + (char)(i % 26);
82  PX4_INFO("Writer: writing char '%c'", buf[0]);
83  ret = px4_write(fd, buf, 1);
84  ++i;
85  }
86 
87  px4_close(fd);
88  PX4_INFO("Writer: stopped");
89  return ret;
90 }
91 
92 class PrivData
93 {
94 public:
96  ~PrivData() = default;
97 
98  size_t _read_offset;
99 };
100 
101 class CDevNode : public cdev::CDev
102 {
103 public:
105  CDev(TESTDEV),
106  _is_open_for_write(false),
107  _write_offset(0) {}
108 
109  ~CDevNode() override = default;
110 
111  int open(cdev::file_t *handlep) override;
112  int close(cdev::file_t *handlep) override;
113  ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen) override;
114  ssize_t read(cdev::file_t *handlep, char *buffer, size_t buflen) override;
115 private:
118  char _buf[1000];
119 };
120 
122 {
123  // Only allow one writer
124  if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) {
125  errno = EBUSY;
126  return -1;
127  }
128 
129  int ret = CDev::open(handlep);
130 
131  if (ret != 0) {
132  return ret;
133  }
134 
135  handlep->f_priv = new PrivData;
136 
137  if (_is_open_for_write && (handlep->f_oflags & PX4_F_WRONLY)) {
138  _is_open_for_write = true;
139  }
140 
141  return 0;
142 }
143 
145 {
146  delete (PrivData *)handlep->f_priv;
147  handlep->f_priv = nullptr;
148  CDev::close(handlep);
149 
150  // Enable a new writer of the device is re-opened for write
151  if ((handlep->f_oflags & PX4_F_WRONLY) && _is_open_for_write) {
152  _is_open_for_write = false;
153  }
154 
155  return 0;
156 }
157 
158 ssize_t CDevNode::write(cdev::file_t *handlep, const char *buffer, size_t buflen)
159 {
160  for (size_t i = 0; i < buflen && _write_offset < 1000; i++) {
161  _buf[_write_offset] = buffer[i];
162  _write_offset++;
163  }
164 
165  // ignore what was written, but let pollers know something was written
166  poll_notify(POLLIN);
167 
168  return buflen;
169 }
170 
171 ssize_t CDevNode::read(cdev::file_t *handlep, char *buffer, size_t buflen)
172 {
173  PrivData *p = (PrivData *)handlep->f_priv;
174  ssize_t chars_read = 0;
175  PX4_INFO("read %zu write %zu", p->_read_offset, _write_offset);
176 
177  for (size_t i = 0; i < buflen && (p->_read_offset < _write_offset); i++) {
178  buffer[i] = _buf[p->_read_offset];
179  p->_read_offset++;
180  chars_read++;
181  }
182 
183  return chars_read;
184 }
185 
187 {
188  if (_node) {
189  delete _node;
190  _node = nullptr;
191  }
192 }
193 
194 int CDevExample::do_poll(int fd, int timeout, int iterations, int delayms_after_poll)
195 {
196  int pollret, readret;
197  int loop_count = 0;
198  char readbuf[10];
199  px4_pollfd_struct_t fds[1];
200 
201  fds[0].fd = fd;
202  fds[0].events = POLLIN;
203  fds[0].revents = 0;
204 
205  bool mustblock = (timeout < 0);
206 
207  // Test indefinte blocking poll
208  while ((!appState.exitRequested()) && (loop_count < iterations)) {
209  pollret = px4_poll(fds, 1, timeout);
210 
211  if (pollret < 0) {
212  PX4_ERR("Reader: px4_poll failed %d FAIL", pollret);
213  goto fail;
214  }
215 
216  PX4_INFO("Reader: px4_poll returned %d", pollret);
217 
218  if (pollret) {
219  readret = px4_read(fd, readbuf, 10);
220 
221  if (readret != 1) {
222  if (mustblock) {
223  PX4_ERR("Reader: read failed %d FAIL", readret);
224  goto fail;
225 
226  } else {
227  PX4_INFO("Reader: read failed %d FAIL", readret);
228  }
229 
230  } else {
231  readbuf[readret] = '\0';
232  PX4_INFO("Reader: px4_poll returned %d, read '%s' PASS", pollret, readbuf);
233  }
234  }
235 
236  if (delayms_after_poll) {
237  px4_usleep(delayms_after_poll * 1000);
238  }
239 
240  loop_count++;
241  }
242 
243  return 0;
244 fail:
245  return 1;
246 }
248 {
249  appState.setRunning(true);
250 
251  _node = new CDevNode();
252 
253  if (_node == nullptr) {
254  PX4_INFO("Failed to allocate CDevNode");
255  return -ENOMEM;
256  }
257 
258  if (_node->init() != PX4_OK) {
259  PX4_INFO("Failed to init CDevNode");
260  return 1;
261  }
262 
263  int fd = px4_open(TESTDEV, PX4_F_RDONLY);
264 
265  if (fd < 0) {
266  PX4_INFO("Open failed %d", fd);
267  return PX4_ERROR;
268  }
269 
270  // Start a task that will write something in 4 seconds
271  (void)px4_task_spawn_cmd("writer",
272  SCHED_DEFAULT,
273  SCHED_PRIORITY_MAX - 6,
274  2000,
275  writer_main,
276  (char *const *)nullptr);
277 
278  int ret = 0;
279 
280  PX4_INFO("TEST: BLOCKING POLL ---------------");
281 
282  if (do_poll(fd, -1, 3, 0)) {
283  ret = 1;
284  goto fail2;
285  }
286 
287  PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
288 
289  if (do_poll(fd, 0, 3, 0)) {
290  ret = 1;
291  goto fail2;
292  goto fail2;
293  }
294 
295  PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
296 
297  if (do_poll(fd, 0, 3, 0)) {
298  ret = 1;
299  goto fail2;
300  goto fail2;
301  }
302 
303  PX4_INFO("TEST: ZERO TIMEOUT POLL -----------");
304 
305  if (do_poll(fd, 0, 3, 0)) {
306  ret = 1;
307  goto fail2;
308  }
309 
310  PX4_INFO("TEST: 100ms TIMEOUT POLL -----------");
311 
312  if (do_poll(fd, 0, 30, 100)) {
313  ret = 1;
314  goto fail2;
315  }
316 
317  PX4_INFO("TEST: 1 SEC TIMOUT POLL ------------");
318 
319  if (do_poll(fd, 1000, 3, 0)) {
320  ret = 1;
321  goto fail2;
322  }
323 
324  PX4_INFO("TEST: waiting for writer to stop");
325 fail2:
326  g_exit = true;
327  px4_close(fd);
328  PX4_INFO("TEST: waiting for writer to stop");
329  px4_sleep(3);
330  return ret;
331 }
int do_poll(int fd, int timeout, int iterations, int delayms_after_poll)
int open(cdev::file_t *handlep) override
Handle an open of the device.
ssize_t write(cdev::file_t *handlep, const char *buffer, size_t buflen) override
Perform a write to the device.
int px4_poll(px4_pollfd_struct_t *fds, nfds_t nfds, int timeout)
bool _is_open_for_write
static px4::AppState appState
size_t _write_offset
static void read(bootloader_app_shared_t *pshared)
Generic device / sensor interface.
~PrivData()=default
Abstract class for any character device.
Definition: CDev.hpp:58
ssize_t px4_read(int fd, void *buffer, size_t buflen)
ssize_t px4_write(int fd, const void *buffer, size_t buflen)
int close(cdev::file_t *handlep) override
Handle a close of the device.
#define TESTDEV
static bool g_exit
int fd
Definition: dataman.cpp:146
static void write(bootloader_app_shared_t *pshared)
static int writer_main(int argc, char *argv[])
int px4_open(const char *path, int flags,...)
size_t _read_offset
ssize_t read(cdev::file_t *handlep, char *buffer, size_t buflen) override
Perform a read from the device.
int px4_close(int fd)