zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

config.c: add documentation & do cleanup (remove err(), ...) 

Also changes the order of the arguments for consistency.
sbg
Beat Küng 8 years ago
parent
be25c337f9
commit
4ea44e51b7
1 changed files with 133 additions and 86 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Unified View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 219
      src/systemcmds/config/config.c

219
src/systemcmds/config/config.c
Unescape View File

@ -40,6 +40,8 @@
*/ */
#include <px4_config.h> #include <px4_config.h>
#include <px4_log.h>
#include <px4_module.h>
#include <stdio.h> #include <stdio.h>
#include <stdlib.h> #include <stdlib.h>
@ -57,147 +59,180 @@
#include <drivers/drv_device.h> #include <drivers/drv_device.h>
#include "systemlib/systemlib.h" #include "systemlib/systemlib.h"
#include "systemlib/err.h"
#include "systemlib/param/param.h" #include "systemlib/param/param.h"
__EXPORT int config_main(int argc, char *argv[]); __EXPORT int config_main(int argc, char *argv[]);
static void do_gyro(int argc, char *argv[]); static int do_gyro(int argc, char *argv[]);
static void do_accel(int argc, char *argv[]); static int do_accel(int argc, char *argv[]);
static void do_mag(int argc, char *argv[]); static int do_mag(int argc, char *argv[]);
static void do_device(int argc, char *argv[]); static int do_device(int argc, char *argv[]);
static void print_usage(void);
int int
config_main(int argc, char *argv[]) config_main(int argc, char *argv[])
{ {
bool is_device_cmd = argc >= 3 && (!strcmp(argv[2], "block") || !strcmp(argv[2], "unblock")); bool is_device_cmd = argc >= 2 && (!strcmp(argv[1], "block") || !strcmp(argv[1], "unblock"));
if (argc >= 2) { if (argc >= 3) {
if (!is_device_cmd && !strncmp(argv[1], "/dev/gyro", 9)) { if (!is_device_cmd && !strncmp(argv[2], "/dev/gyro", 9)) {
do_gyro(argc - 1, argv + 1); return do_gyro(argc - 1, argv + 1);
} else if (!is_device_cmd && !strncmp(argv[1], "/dev/accel", 10)) { } else if (!is_device_cmd && !strncmp(argv[2], "/dev/accel", 10)) {
do_accel(argc - 1, argv + 1); return do_accel(argc - 1, argv + 1);
} else if (!is_device_cmd && !strncmp(argv[1], "/dev/mag", 8)) { } else if (!is_device_cmd && !strncmp(argv[2], "/dev/mag", 8)) {
do_mag(argc - 1, argv + 1); return do_mag(argc - 1, argv + 1);
} else { } else {
do_device(argc - 1, argv + 1); return do_device(argc - 1, argv + 1);
} }
} }
errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); print_usage();
return 1;
} }
static void static void
print_usage(void)
{
PRINT_MODULE_DESCRIPTION("Configure a sensor driver (sampling & publication rate, range, etc.)");
PRINT_MODULE_USAGE_NAME("config", "command");
PRINT_MODULE_USAGE_PARAM_COMMENT("The <file:dev> argument is typically one of /dev/{gyro,accel,mag}i");
PRINT_MODULE_USAGE_COMMAND_DESCR("block", "Block sensor topic publication");
PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("unblock", "Unblock sensor topic publication");
PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("sampling", "Set sensor sampling rate");
PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and sampling rate in Hz", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("rate", "Set sensor publication rate");
PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and publication rate in Hz", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("range", "Set sensor measurement range");
PRINT_MODULE_USAGE_ARG("<file:dev> <rate>", "Sensor device file and range", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Perform sensor self-test (and print info)");
PRINT_MODULE_USAGE_ARG("<file:dev>", "Sensor device file", false);
}
static int
do_device(int argc, char *argv[]) do_device(int argc, char *argv[])
{ {
if (argc < 2) { if (argc < 2) {
errx(1, "no device path provided and command provided."); print_usage();
return 1;
} }
int fd; int fd;
fd = open(argv[0], 0); fd = open(argv[1], 0);
if (fd < 0) { if (fd < 0) {
warn("%s", argv[0]); PX4_ERR("open %s failed (%i)", argv[1], errno);
errx(1, "FATAL: no device found"); return 1;
} else { } else {
int ret; int ret;
if (argc == 2 && !strcmp(argv[1], "block")) { if (argc == 2 && !strcmp(argv[0], "block")) {
/* disable the device publications */ /* disable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); ret = ioctl(fd, DEVIOCSPUBBLOCK, 1);
if (ret) { if (ret) {
errx(ret, "uORB publications could not be blocked"); PX4_ERR("uORB publications could not be blocked");
return 1;
} }
} else if (argc == 2 && !strcmp(argv[1], "unblock")) { } else if (argc == 2 && !strcmp(argv[0], "unblock")) {
/* enable the device publications */ /* enable the device publications */
ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); ret = ioctl(fd, DEVIOCSPUBBLOCK, 0);
if (ret) { if (ret) {
errx(ret, "uORB publications could not be unblocked"); PX4_ERR("uORB publications could not be unblocked");
return 1;
} }
} else { } else {
errx(1, "no valid command: %s", argv[1]); print_usage();
return 1;
} }
} }
exit(0); return 0;
} }
static void static int
do_gyro(int argc, char *argv[]) do_gyro(int argc, char *argv[])
{ {
int fd; int fd;
fd = open(argv[0], 0); fd = open(argv[1], 0);
if (fd < 0) { if (fd < 0) {
warn("%s", argv[0]); PX4_ERR("open %s failed (%i)", argv[1], errno);
errx(1, "FATAL: no gyro found"); return 1;
} else { } else {
int ret; int ret;
if (argc == 3 && !strcmp(argv[1], "sampling")) { if (argc == 3 && !strcmp(argv[0], "sampling")) {
/* set the gyro internal sampling rate up to at least i Hz */ /* set the gyro internal sampling rate up to at least i Hz */
ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "sampling rate could not be set"); PX4_ERR("sampling rate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "rate")) { } else if (argc == 3 && !strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */ /* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "pollrate could not be set"); PX4_ERR("pollrate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "range")) { } else if (argc == 3 && !strcmp(argv[0], "range")) {
/* set the range to i dps */ /* set the range to i dps */
ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "range could not be set"); PX4_ERR("range could not be set");
return 1;
} }
} else if (argc == 2 && !strcmp(argv[1], "check")) { } else if (argc == 2 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, GYROIOCSELFTEST, 0); ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret) { if (ret) {
warnx("gyro self test FAILED! Check calibration:"); PX4_WARN("gyro self test FAILED! Check calibration:");
struct gyro_calibration_s scale; struct gyro_calibration_s scale;
ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale);
if (ret) { if (ret) {
err(1, "failed getting gyro scale"); PX4_ERR("failed getting gyro scale");
return 1;
} }
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset,
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); (double)scale.z_offset);
PX4_INFO("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else { } else {
warnx("gyro calibration and self test OK"); PX4_INFO("gyro calibration and self test OK");
} }
} else { } else {
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2000' to set measurement range to 2000 dps\n\t"); print_usage();
return 1;
} }
int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0);
@ -208,78 +243,84 @@ do_gyro(int argc, char *argv[])
param_get(param_find("CAL_GYRO0_ID"), &(calibration_id)); param_get(param_find("CAL_GYRO0_ID"), &(calibration_id));
warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", PX4_INFO("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps",
id, calibration_id, srate, prate, range); id, calibration_id, srate, prate, range);
close(fd); close(fd);
} }
exit(0); return 0;
} }
static void static int
do_mag(int argc, char *argv[]) do_mag(int argc, char *argv[])
{ {
int fd; int fd;
fd = open(argv[0], 0); fd = open(argv[1], 0);
if (fd < 0) { if (fd < 0) {
warn("%s", argv[0]); PX4_ERR("open %s failed (%i)", argv[1], errno);
errx(1, "FATAL: no magnetometer found"); return 1;
} else { } else {
int ret; int ret;
if (argc == 3 && !strcmp(argv[1], "sampling")) { if (argc == 3 && !strcmp(argv[0], "sampling")) {
/* set the mag internal sampling rate up to at least i Hz */ /* set the mag internal sampling rate up to at least i Hz */
ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "sampling rate could not be set"); PX4_ERR("sampling rate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "rate")) { } else if (argc == 3 && !strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */ /* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "pollrate could not be set"); PX4_ERR("pollrate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "range")) { } else if (argc == 3 && !strcmp(argv[0], "range")) {
/* set the range to i G */ /* set the range to i G */
ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "range could not be set"); PX4_ERR("range could not be set");
return 1;
} }
} else if (argc == 2 && !strcmp(argv[1], "check")) { } else if (argc == 2 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, MAGIOCSELFTEST, 0); ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret) { if (ret) {
warnx("mag self test FAILED! Check calibration:"); PX4_WARN("mag self test FAILED! Check calibration:");
struct mag_calibration_s scale; struct mag_calibration_s scale;
ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale);
if (ret) { if (ret) {
err(ret, "failed getting mag scale"); PX4_ERR("failed getting mag scale");
return 1;
} }
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset,
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); (double)scale.z_offset);
PX4_INFO("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else { } else {
warnx("mag calibration and self test OK"); PX4_INFO("mag calibration and self test OK");
} }
} else { } else {
errx(1, "wrong or no arguments given. Try: \n\n\t'check' for the self test\n\t"); print_usage();
return 1;
} }
int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0); int srate = ioctl(fd, MAGIOCGSAMPLERATE, 0);
@ -290,78 +331,84 @@ do_mag(int argc, char *argv[])
param_get(param_find("CAL_MAG0_ID"), &(calibration_id)); param_get(param_find("CAL_MAG0_ID"), &(calibration_id));
warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", PX4_INFO("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga",
id, calibration_id, srate, prate, range); id, calibration_id, srate, prate, range);
close(fd); close(fd);
} }
exit(0); return 0;
} }
static void static int
do_accel(int argc, char *argv[]) do_accel(int argc, char *argv[])
{ {
int fd; int fd;
fd = open(argv[0], 0); fd = open(argv[1], 0);
if (fd < 0) { if (fd < 0) {
warn("%s", argv[0]); PX4_ERR("open %s failed (%i)", argv[1], errno);
errx(1, "FATAL: no accelerometer found"); return 1;
} else { } else {
int ret; int ret;
if (argc == 3 && !strcmp(argv[1], "sampling")) { if (argc == 3 && !strcmp(argv[0], "sampling")) {
/* set the accel internal sampling rate up to at least i Hz */ /* set the accel internal sampling rate up to at least i Hz */
ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "sampling rate could not be set"); PX4_ERR("sampling rate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "rate")) { } else if (argc == 3 && !strcmp(argv[0], "rate")) {
/* set the driver to poll at i Hz */ /* set the driver to poll at i Hz */
ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "pollrate could not be set"); PX4_ERR("pollrate could not be set");
return 1;
} }
} else if (argc == 3 && !strcmp(argv[1], "range")) { } else if (argc == 3 && !strcmp(argv[0], "range")) {
/* set the range to i G */ /* set the range to i G */
ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0));
if (ret) { if (ret) {
errx(ret, "range could not be set"); PX4_ERR("range could not be set");
return 1;
} }
} else if (argc == 2 && !strcmp(argv[1], "check")) { } else if (argc == 2 && !strcmp(argv[0], "check")) {
ret = ioctl(fd, ACCELIOCSELFTEST, 0); ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret) { if (ret) {
warnx("accel self test FAILED! Check calibration:"); PX4_WARN("accel self test FAILED! Check calibration:");
struct accel_calibration_s scale; struct accel_calibration_s scale;
ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale);
if (ret) { if (ret) {
err(ret, "failed getting accel scale"); PX4_ERR("failed getting accel scale");
return 1;
} }
warnx("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, (double)scale.z_offset); PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset,
warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); (double)scale.z_offset);
PX4_INFO("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale);
} else { } else {
warnx("accel calibration and self test OK"); PX4_INFO("accel calibration and self test OK");
} }
} else { } else {
errx(1, "no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t"); print_usage();
return 1;
} }
int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
@ -372,11 +419,11 @@ do_accel(int argc, char *argv[])
param_get(param_find("CAL_ACC0_ID"), &(calibration_id)); param_get(param_find("CAL_ACC0_ID"), &(calibration_id));
warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", PX4_INFO("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G",
id, calibration_id, srate, prate, range); id, calibration_id, srate, prate, range);
close(fd); close(fd);
} }
exit(0); return 0;
} }

Write Preview
Loading…
Cancel
Save