PX4 Firmware
PX4 Autopilot Software http://px4.io
config.c
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file config.c
36  * @author Lorenz Meier <lm@inf.ethz.ch>
37  * @author Julian Oes <joes@student.ethz.ch>
38  *
39  * config tool. Takes the device name as the first parameter.
40  */
41 
42 #include <px4_platform_common/px4_config.h>
43 #include <px4_platform_common/log.h>
44 #include <px4_platform_common/module.h>
45 
46 #include <stdio.h>
47 #include <stdlib.h>
48 #include <string.h>
49 #include <stdbool.h>
50 #include <unistd.h>
51 #include <fcntl.h>
52 #include <sys/stat.h>
53 
54 #include <arch/board/board.h>
55 
56 #include <drivers/drv_gyro.h>
57 #include <drivers/drv_accel.h>
58 #include <drivers/drv_mag.h>
59 #include <drivers/drv_device.h>
60 
61 #include <parameters/param.h>
62 
63 __EXPORT int config_main(int argc, char *argv[]);
64 
65 static int do_gyro(int argc, char *argv[]);
66 static int do_accel(int argc, char *argv[]);
67 static int do_mag(int argc, char *argv[]);
68 static int do_device(int argc, char *argv[]);
69 static void print_usage(void);
70 
71 int
72 config_main(int argc, char *argv[])
73 {
74  bool is_device_cmd = argc >= 2 && (!strcmp(argv[1], "block") || !strcmp(argv[1], "unblock"));
75 
76  if (argc >= 3) {
77  if (!is_device_cmd && !strncmp(argv[2], "/dev/gyro", 9)) {
78  return do_gyro(argc - 1, argv + 1);
79 
80  } else if (!is_device_cmd && !strncmp(argv[2], "/dev/accel", 10)) {
81  return do_accel(argc - 1, argv + 1);
82 
83  } else if (!is_device_cmd && !strncmp(argv[2], "/dev/mag", 8)) {
84  return do_mag(argc - 1, argv + 1);
85 
86  } else {
87  return do_device(argc - 1, argv + 1);
88  }
89  }
90 
91  print_usage();
92  return 1;
93 }
94 static void
96 {
97  PRINT_MODULE_DESCRIPTION("Configure a sensor driver (sampling & publication rate, range, etc.)");
98 
99  PRINT_MODULE_USAGE_NAME("config", "command");
100  PRINT_MODULE_USAGE_PARAM_COMMENT("The <file:dev> argument is typically one of /dev/{gyro,accel,mag}i");
101 
102  PRINT_MODULE_USAGE_COMMAND_DESCR("block", "Block sensor topic publication");
103  PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
104  PRINT_MODULE_USAGE_COMMAND_DESCR("unblock", "Unblock sensor topic publication");
105  PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
106 
107  PRINT_MODULE_USAGE_COMMAND_DESCR("sampling", "Set sensor sampling rate");
108  PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and sampling rate in Hz", false);
109  PRINT_MODULE_USAGE_COMMAND_DESCR("rate", "Set sensor publication rate");
110  PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and publication rate in Hz", false);
111  PRINT_MODULE_USAGE_COMMAND_DESCR("range", "Set sensor measurement range");
112  PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and range", false);
113  PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Perform sensor self-test (and print info)");
114  PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
115 }
116 
117 static int
118 do_device(int argc, char *argv[])
119 {
120  if (argc < 2) {
121  print_usage();
122  return 1;
123  }
124 
125  int fd;
126 
127  fd = open(argv[1], 0);
128 
129  if (fd < 0) {
130  PX4_ERR("open %s failed (%i)", argv[1], errno);
131  return 1;
132 
133  } else {
134 
135  int ret;
136 
137  if (argc == 2 && !strcmp(argv[0], "block")) {
138 
139  /* disable the device publications */
140  ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
141 
142  if (ret) {
143  PX4_ERR("uORB publications could not be blocked");
144  return 1;
145  }
146 
147  } else if (argc == 2 && !strcmp(argv[0], "unblock")) {
148 
149  /* enable the device publications */
150  ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
151 
152  if (ret) {
153  PX4_ERR("uORB publications could not be unblocked");
154  return 1;
155  }
156 
157  } else {
158  print_usage();
159  return 1;
160  }
161  }
162 
163  return 0;
164 }
165 
166 static int
167 do_gyro(int argc, char *argv[])
168 {
169  int fd;
170 
171  fd = open(argv[1], 0);
172 
173  if (fd < 0) {
174  PX4_ERR("open %s failed (%i)", argv[1], errno);
175  return 1;
176 
177  } else {
178 
179  int ret;
180 
181  if (argc == 3 && !strcmp(argv[0], "rate")) {
182 
183  /* set the driver to poll at i Hz */
184  ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
185 
186  if (ret) {
187  PX4_ERR("pollrate could not be set");
188  return 1;
189  }
190 
191  } else {
192  print_usage();
193  return 1;
194  }
195 
196  int id = ioctl(fd, DEVIOCGDEVICEID, 0);
197  int32_t calibration_id = 0;
198 
199  param_get(param_find("CAL_GYRO0_ID"), &(calibration_id));
200 
201  PX4_INFO("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)",
202  id, calibration_id);
203 
204  close(fd);
205  }
206 
207  return 0;
208 }
209 
210 static int
211 do_mag(int argc, char *argv[])
212 {
213  int fd;
214 
215  fd = open(argv[1], 0);
216 
217  if (fd < 0) {
218  PX4_ERR("open %s failed (%i)", argv[1], errno);
219  return 1;
220 
221  } else {
222 
223  int ret;
224 
225  if (argc == 3 && !strcmp(argv[0], "rate")) {
226 
227  /* set the driver to poll at i Hz */
228  ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
229 
230  if (ret) {
231  PX4_ERR("pollrate could not be set");
232  return 1;
233  }
234 
235  } else if (argc == 3 && !strcmp(argv[0], "range")) {
236 
237  /* set the range to i G */
238  ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0));
239 
240  if (ret) {
241  PX4_ERR("range could not be set");
242  return 1;
243  }
244 
245  } else {
246  print_usage();
247  return 1;
248  }
249 
250  int id = ioctl(fd, DEVIOCGDEVICEID, 0);
251  int32_t calibration_id = 0;
252 
253  param_get(param_find("CAL_MAG0_ID"), &(calibration_id));
254 
255  PX4_INFO("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)",
256  id, calibration_id);
257 
258  close(fd);
259  }
260 
261  return 0;
262 }
263 
264 static int
265 do_accel(int argc, char *argv[])
266 {
267  int fd;
268 
269  fd = open(argv[1], 0);
270 
271  if (fd < 0) {
272  PX4_ERR("open %s failed (%i)", argv[1], errno);
273  return 1;
274 
275  } else {
276 
277  int ret;
278 
279  if (argc == 3 && !strcmp(argv[0], "rate")) {
280 
281  /* set the driver to poll at i Hz */
282  ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
283 
284  if (ret) {
285  PX4_ERR("pollrate could not be set");
286  return 1;
287  }
288 
289  } else {
290  print_usage();
291  return 1;
292  }
293 
294  int id = ioctl(fd, DEVIOCGDEVICEID, 0);
295  int32_t calibration_id = 0;
296 
297  param_get(param_find("CAL_ACC0_ID"), &(calibration_id));
298 
299  PX4_INFO("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)",
300  id, calibration_id);
301 
302  close(fd);
303  }
304 
305  return 0;
306 }
Accelerometer driver interface.
Gyroscope driver interface.
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
static int do_gyro(int argc, char *argv[])
Definition: config.c:167
static int do_accel(int argc, char *argv[])
Definition: config.c:265
Definition: I2C.hpp:51
Global flash based parameter store.
static int do_mag(int argc, char *argv[])
Definition: config.c:211
static int do_device(int argc, char *argv[])
Definition: config.c:118
#define MAGIOCSRANGE
set the measurement range to handle (at least) arg Gauss
Definition: drv_mag.h:79
#define SENSORIOCSPOLLRATE
Set the driver polling rate to (arg) Hz, or one of the SENSOR_POLLRATE constants. ...
Definition: drv_sensor.h:134
Generic device / sensor interface.
int fd
Definition: dataman.cpp:146
__EXPORT int config_main(int argc, char *argv[])
Definition: config.c:72
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
static void print_usage(void)
Definition: config.c:95