PX4 Firmware
PX4 Autopilot Software http://px4.io
hott_telemetry.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
4  * Author: Simon Wilks <sjwilks@gmail.com>
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 hott_telemetry_main.c
37  * @author Simon Wilks <sjwilks@gmail.com>
38  *
39  * Graupner HoTT Telemetry implementation.
40  *
41  * The HoTT receiver polls each device at a regular interval at which point
42  * a data packet can be returned if necessary.
43  *
44  */
45 
46 #include <fcntl.h>
47 #include <px4_platform_common/px4_config.h>
48 #include <px4_platform_common/defines.h>
49 #include <px4_platform_common/tasks.h>
50 #include <poll.h>
51 #include <stdlib.h>
52 #include <stdio.h>
53 #include <string.h>
54 #include <sys/ioctl.h>
55 #include <unistd.h>
56 #include <systemlib/err.h>
57 #include <perf/perf_counter.h>
58 
59 #include "../comms.h"
60 #include "../messages.h"
61 
62 #define DEFAULT_UART "/dev/ttyS6"; /**< Serial4 */
63 
64 static int thread_should_exit = false; /**< Deamon exit flag */
65 static int thread_running = false; /**< Deamon status flag */
66 static int deamon_task; /**< Handle of deamon task / thread */
67 static const char daemon_name[] = "hott_telemetry";
68 static const char commandline_usage[] =
69  "usage: hott_telemetry start|status|stop [-d <device>] [-t <timeout ms>] [-r <read delay us>] [-w <write delay us>]";
70 
71 static uint8_t read_log[16];
72 
76 
84 
85 /**
86  * Deamon management function.
87  */
88 extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
89 
90 /**
91  * Mainloop of daemon.
92  */
93 int hott_telemetry_thread_main(int argc, char *argv[]);
94 
95 static int recv_req_id(int uart, uint8_t *id);
96 static int send_data(int uart, uint8_t *buffer, size_t size);
97 
98 int
99 recv_req_id(int uart, uint8_t *id)
100 {
101  uint8_t mode;
102 
103  struct pollfd fds;
104  fds.fd = uart;
105  fds.events = POLLIN;
106 
107  if (poll(&fds, 1, timeout_ms) > 0) {
108  /* Get the mode: binary or text */
109  read(uart, &mode, sizeof(mode));
110 
111  perf_count(reqs_count);
112 
113  // Debug log
114  for (int x = 15; x > 0; x--) {
115  read_log[x] = read_log[x - 1];
116  }
117 
118  read_log[0] = mode;
119 
120  /* if we have a binary mode request */
121  if (mode != BINARY_MODE_REQUEST_ID) {
122  return PX4_ERROR;
123  }
124 
125  /* Read the device ID being polled */
126  read(uart, id, sizeof(*id));
127 
128  } else {
129  warnx("UART timeout on TX/RX port");
130  return PX4_ERROR;
131  }
132 
133  return PX4_OK;
134 }
135 
136 int
137 send_data(int uart, uint8_t *buffer, size_t size)
138 {
139  usleep(read_delay_us);
140 
141  uint16_t checksum = 0;
142 
143  for (size_t i = 0; i < size; i++) {
144  if (i == size - 1) {
145  /* Set the checksum: the first uint8_t is taken as the checksum. */
146  buffer[i] = checksum & 0xff;
147 
148  } else {
149  checksum += buffer[i];
150  }
151 
152  write(uart, &buffer[i], sizeof(buffer[i]));
153 
154  /* Sleep before sending the next byte. */
155  usleep(write_delay_us);
156  }
157 
158  /* A hack the reads out what was written so the next read from the receiver doesn't get it. */
159  /* TODO: Fix this!! */
160  uint8_t dummy[size];
161  read(uart, &dummy, size);
162 
163  return PX4_OK;
164 }
165 
166 int
167 hott_telemetry_thread_main(int argc, char *argv[])
168 {
169  connect_count = perf_alloc(PC_COUNT, "reconnects ");
170  recon_port = perf_alloc(PC_COUNT, "reopen port ");
171  reqs_count = perf_alloc(PC_COUNT, "requests ");
172  bin_reply = perf_alloc(PC_COUNT, "bin replies ");
173  txt_reply = perf_alloc(PC_COUNT, "text replies ");
174  bad_reply = perf_alloc(PC_COUNT, "unknown replies ");
175  dead_reply = perf_alloc(PC_COUNT, "dead replies ");
176 
177  thread_running = true;
178 
179  const char *device = DEFAULT_UART;
180 
181  /* read commandline arguments */
182  for (int i = 0; i < argc && argv[i]; i++) {
183  if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
184  if (argc > i + 1) {
185  device = argv[i + 1];
186 
187  } else {
188  thread_running = false;
189  errx(1, "missing parameter to -d\n%s", commandline_usage);
190  }
191  }
192  }
193 
194  /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
195  int uart = open_uart(device);
196 
197  if (uart < 0) {
198  errx(1, "Failed opening HoTT UART, exiting.");
199  thread_running = false;
200  }
201 
203 
204  uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
205  size_t size = 0;
206  uint8_t id = 0;
207 
208  bool connected = true;
209  int recon = 0;
210 
211  while (!thread_should_exit) {
212  // Listen for and serve poll from the receiver.
213  if (recv_req_id(uart, &id) == PX4_OK) {
214  if (!connected) {
215  connected = true;
216  }
217 
218  switch (id) {
219  case EAM_SENSOR_ID:
220  build_eam_response(buffer, &size);
221  perf_count(bin_reply);
222  break;
223 
224  case GAM_SENSOR_ID:
225  build_gam_response(buffer, &size);
226  perf_count(bin_reply);
227  break;
228 
229  case GPS_SENSOR_ID:
230  build_gps_response(buffer, &size);
231  perf_count(bin_reply);
232  break;
233 
235  perf_count(dead_reply);
236  break;
237 
238  default:
239  perf_count(bad_reply);
240  continue; // Not a module we support.
241  }
242 
243  send_data(uart, buffer, size);
244 
245  } else {
246  if (connected) {
247  connected = false;
248 
249  } else {
250  recon++;
251  }
252 
253  if (recon > 100) {
254  perf_count(recon_port);
255  close(uart);
256  uart = open_uart(device);
257  perf_reset(reqs_count);
258  perf_reset(bin_reply);
259  perf_reset(txt_reply);
260  perf_reset(dead_reply);
261  perf_reset(bad_reply);
262  }
263  }
264  }
265 
266  warnx("exiting");
267 
268  close(uart);
269 
270  thread_running = false;
271 
272  perf_free(connect_count);
273  perf_free(recon_port);
274  perf_free(reqs_count);
275  perf_free(bin_reply);
276  perf_free(txt_reply);
277  perf_free(bad_reply);
278  perf_free(dead_reply);
279 
280  return 0;
281 }
282 
283 /**
284  * Process command line arguments and start the daemon.
285  */
286 int
287 hott_telemetry_main(int argc, char *argv[])
288 {
289  if (argc < 2) {
290  errx(1, "missing command\n%s", commandline_usage);
291  }
292 
293  /* read commandline arguments */
294  for (int i = 0; i < argc && argv[i]; i++) {
295  if (strcmp(argv[i], "-t") == 0 || strcmp(argv[i], "--timeout") == 0) { //device set
296  if (argc > i + 1) {
297  timeout_ms = atoi(argv[i + 1]);
298 
299  } else {
300  errx(1, "missing parameter to -t\n%s", commandline_usage);
301  }
302  }
303 
304  if (strcmp(argv[i], "-r") == 0 || strcmp(argv[i], "--read-delay") == 0) { //device set
305  if (argc > i + 1) {
306  read_delay_us = atoi(argv[i + 1]);
307 
308  } else {
309  errx(1, "missing parameter to -r\n%s", commandline_usage);
310  }
311  }
312 
313  if (strcmp(argv[i], "-w") == 0 || strcmp(argv[i], "--write-delay") == 0) { //device set
314  if (argc > i + 1) {
315  write_delay_us = atoi(argv[i + 1]);
316 
317  } else {
318  errx(1, "missing parameter to -w\n%s", commandline_usage);
319  }
320  }
321  }
322 
323  if (!strcmp(argv[1], "start")) {
324 
325  if (thread_running) {
326  warnx("already running");
327  exit(0);
328  }
329 
330  thread_should_exit = false;
331  deamon_task = px4_task_spawn_cmd(daemon_name,
332  SCHED_DEFAULT,
333  SCHED_PRIORITY_DEFAULT,
334  1500,
336  (argv) ? (char *const *)&argv[2] : (char *const *)NULL);
337  exit(0);
338  }
339 
340  if (!strcmp(argv[1], "stop")) {
341  thread_should_exit = true;
342  exit(0);
343  }
344 
345  if (!strcmp(argv[1], "status")) {
346  if (thread_running) {
347  warnx("is running");
348 
349 
350  for (int x = 15; x >= 0; x--) {
351  printf("%2x ", read_log[x]);
352  }
353 
354  printf("\npoll timeout : %i ms\n", timeout_ms);
355  printf("post write delay : %i us\n", write_delay_us);
356  printf("post read delay : %i us\n", read_delay_us);
357  perf_print_counter(recon_port);
358  perf_print_counter(connect_count);
359  perf_print_counter(reqs_count);
360  perf_print_counter(bin_reply);
361  perf_print_counter(txt_reply);
362  perf_print_counter(bad_reply);
363  perf_print_counter(dead_reply);
364 
365  } else {
366  warnx("not started");
367  }
368 
369  exit(0);
370  }
371 
372  errx(1, "unrecognized command\n%s", commandline_usage);
373 }
static int timeout_ms
#define BINARY_MODE_REQUEST_ID
Definition: messages.h:62
static const char daemon_name[]
Definition: I2C.hpp:51
void build_gam_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:174
#define MAX_MESSAGE_BUFFER_SIZE
Definition: messages.h:240
__EXPORT int hott_telemetry_main(int argc, char *argv[])
Deamon management function.
perf_counter_t dead_reply
static int read_delay_us
Namespace encapsulating all device framework classes, functions and data.
Definition: CDev.cpp:47
count the number of times an event occurs
Definition: perf_counter.h:55
static int thread_running
Deamon status flag.
static void read(bootloader_app_shared_t *pshared)
void perf_reset(perf_counter_t handle)
Reset a performance counter.
void init_sub_messages(void)
Definition: messages.cpp:74
#define GAM_SENSOR_ID
Definition: messages.h:128
void perf_count(perf_counter_t handle)
Count a performance event.
static int send_data(int uart, uint8_t *buffer, size_t size)
Header common to all counters.
static int write_delay_us
static int thread_should_exit
Deamon exit flag.
#define POLL_TIMEOUT_IN_MSECS
Definition: messages.h:48
void perf_free(perf_counter_t handle)
Free a counter.
#define perf_alloc(a, b)
Definition: px4io.h:59
#define warnx(...)
Definition: err.h:95
static int deamon_task
Handle of deamon task / thread.
Simple error/warning functions, heavily inspired by the BSD functions of the same names...
perf_counter_t reqs_count
perf_counter_t txt_reply
#define GPS_SENSOR_ID
Definition: messages.h:181
#define POST_WRITE_DELAY_IN_USECS
Definition: messages.h:59
#define POST_READ_DELAY_IN_USECS
Definition: messages.h:54
int open_uart(const char *device)
Definition: comms.cpp:49
void build_eam_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:132
static void write(bootloader_app_shared_t *pshared)
perf_counter_t bad_reply
perf_counter_t recon_port
#define EAM_SENSOR_ID
Definition: messages.h:74
void perf_print_counter(perf_counter_t handle)
Print one performance counter to stdout.
#define errx(eval,...)
Definition: err.h:89
perf_counter_t connect_count
static int recv_req_id(int uart, uint8_t *id)
int hott_telemetry_thread_main(int argc, char *argv[])
Mainloop of daemon.
perf_counter_t bin_reply
#define DEFAULT_UART
Serial4.
static uint8_t read_log[16]
void build_gps_response(uint8_t *buffer, size_t *size)
Definition: messages.cpp:209
mode
Definition: vtol_type.h:76
Performance measuring tools.
static const char commandline_usage[]