hott_sensors.cpp 6.17 KB
Newer Older
Simon Wilks's avatar
Simon Wilks committed
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
/****************************************************************************
 *
 *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
 *   Author: Simon Wilks <sjwilks@gmail.com>
 *
 * 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 hott_sensors.c
 * @author Simon Wilks <sjwilks@gmail.com>
 *
 * Graupner HoTT sensor driver implementation.
 *
 * Poll any sensors connected to the PX4 via the telemetry wire.
 */

#include <fcntl.h>
#include <nuttx/config.h>
#include <poll.h>
#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <systemlib/err.h>
#include <systemlib/systemlib.h>

#include "../comms.h"
#include "../messages.h"

Simon Wilks's avatar
Simon Wilks committed
58
#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
59
60
61
62
63
64
65

/* Oddly, ERROR is not defined for C++ */
#ifdef ERROR
# undef ERROR
#endif
static const int ERROR = -1;

Simon Wilks's avatar
Simon Wilks committed
66
67
68
69
static int thread_should_exit = false;		/**< Deamon exit flag */
static int thread_running = false;		/**< Deamon status flag */
static int deamon_task;				/**< Handle of deamon task / thread */
static const char daemon_name[] = "hott_sensors";
Simon Wilks's avatar
Simon Wilks committed
70
static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
Simon Wilks's avatar
Simon Wilks committed
71
72
73
74

/**
 * Deamon management function.
 */
75
extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
Simon Wilks's avatar
Simon Wilks committed
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

/**
 * Mainloop of daemon.
 */
int hott_sensors_thread_main(int argc, char *argv[]);

static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
static int send_poll(int uart, uint8_t *buffer, size_t size);

int
send_poll(int uart, uint8_t *buffer, size_t size)
{
	for (size_t i = 0; i < size; i++) {
		write(uart, &buffer[i], sizeof(buffer[i]));

		/* Sleep before sending the next byte. */
		usleep(POST_WRITE_DELAY_IN_USECS);
	}

	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
	/* TODO: Fix this!! */
	uint8_t dummy[size];
	read(uart, &dummy, size);

	return OK;
}

int
recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
{
	static const int timeout_ms = 1000;
107
108
109
110
	
	struct pollfd fds;
	fds.fd = uart;
	fds.events = POLLIN;
Simon Wilks's avatar
Simon Wilks committed
111
112
	
	// XXX should this poll be inside the while loop???
113
	if (poll(&fds, 1, timeout_ms) > 0) {
Simon Wilks's avatar
Simon Wilks committed
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
		int i = 0;
		bool stop_byte_read = false;
		while (true)  {
			read(uart, &buffer[i], sizeof(buffer[i]));

			if (stop_byte_read) {
				// XXX process checksum
				*size = ++i;
				return OK;
			}
			// XXX can some other field not have the STOP BYTE value?
			if (buffer[i] == STOP_BYTE) {
				*id = buffer[1];
				stop_byte_read = true;
			}
			i++;
		}
	}
	return ERROR;
}

int
hott_sensors_thread_main(int argc, char *argv[])
{
	warnx("starting");

	thread_running = true;

142
	const char *device = DEFAULT_UART;
Simon Wilks's avatar
Simon Wilks committed
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157

	/* read commandline arguments */
	for (int i = 0; i < argc && argv[i]; i++) {
		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
			if (argc > i + 1) {
				device = argv[i + 1];

			} else {
				thread_running = false;
				errx(1, "missing parameter to -d\n%s", commandline_usage);
			}
		}
	}

	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
Simon Wilks's avatar
Simon Wilks committed
158
	const int uart = open_uart(device);
Simon Wilks's avatar
Simon Wilks committed
159
160
161
162
163
	if (uart < 0) {
		errx(1, "Failed opening HoTT UART, exiting.");
		thread_running = false;
	}

164
	init_pub_messages();
Simon Wilks's avatar
Simon Wilks committed
165

166
	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
Simon Wilks's avatar
Simon Wilks committed
167
168
169
170
	size_t size = 0;
	uint8_t id = 0;
	while (!thread_should_exit) {
		// Currently we only support a General Air Module sensor.
171
		build_gam_request(&buffer[0], &size);
Simon Wilks's avatar
Simon Wilks committed
172
		send_poll(uart, buffer, size);
Simon Wilks's avatar
Simon Wilks committed
173
174
175
176

		// The sensor will need a little time before it starts sending.
		usleep(5000);

177
		recv_data(uart, &buffer[0], &size, &id);
Simon Wilks's avatar
Simon Wilks committed
178
179
180

		// Determine which moduel sent it and process accordingly.
		if (id == GAM_SENSOR_ID) {
Simon Wilks's avatar
Simon Wilks committed
181
			publish_gam_message(buffer);
Simon Wilks's avatar
Simon Wilks committed
182
		} else {
Simon Wilks's avatar
Simon Wilks committed
183
184
			warnx("Unknown sensor ID: %d", id);
		}
Simon Wilks's avatar
Simon Wilks committed
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
	}

	warnx("exiting");
	close(uart);
	thread_running = false;

	return 0;
}

/**
 * Process command line arguments and start the daemon.
 */
int
hott_sensors_main(int argc, char *argv[])
{
	if (argc < 1) {
		errx(1, "missing command\n%s", commandline_usage);
	}

	if (!strcmp(argv[1], "start")) {

		if (thread_running) {
			warnx("deamon already running");
			exit(0);
		}

		thread_should_exit = false;
212
213
		deamon_task = task_spawn_cmd(daemon_name,
					     SCHED_DEFAULT,
214
					     SCHED_PRIORITY_DEFAULT,
215
					     1024,
216
217
					     hott_sensors_thread_main,
					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
Simon Wilks's avatar
Simon Wilks committed
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
		exit(0);
	}

	if (!strcmp(argv[1], "stop")) {
		thread_should_exit = true;
		exit(0);
	}

	if (!strcmp(argv[1], "status")) {
		if (thread_running) {
			warnx("daemon is running");

		} else {
			warnx("daemon not started");
		}

		exit(0);
	}

	errx(1, "unrecognized command\n%s", commandline_usage);
}