diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 19c699d7dc..09c074bef9 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -40,6 +40,8 @@ */ #include +#include +#include #include #include @@ -57,147 +59,180 @@ #include #include "systemlib/systemlib.h" -#include "systemlib/err.h" #include "systemlib/param/param.h" __EXPORT int config_main(int argc, char *argv[]); -static void do_gyro(int argc, char *argv[]); -static void do_accel(int argc, char *argv[]); -static void do_mag(int argc, char *argv[]); -static void do_device(int argc, char *argv[]); +static int do_gyro(int argc, char *argv[]); +static int do_accel(int argc, char *argv[]); +static int do_mag(int argc, char *argv[]); +static int do_device(int argc, char *argv[]); +static void print_usage(void); int 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 (!is_device_cmd && !strncmp(argv[1], "/dev/gyro", 9)) { - do_gyro(argc - 1, argv + 1); + if (argc >= 3) { + if (!is_device_cmd && !strncmp(argv[2], "/dev/gyro", 9)) { + return do_gyro(argc - 1, argv + 1); - } else if (!is_device_cmd && !strncmp(argv[1], "/dev/accel", 10)) { - do_accel(argc - 1, argv + 1); + } else if (!is_device_cmd && !strncmp(argv[2], "/dev/accel", 10)) { + return do_accel(argc - 1, argv + 1); - } else if (!is_device_cmd && !strncmp(argv[1], "/dev/mag", 8)) { - do_mag(argc - 1, argv + 1); + } else if (!is_device_cmd && !strncmp(argv[2], "/dev/mag", 8)) { + return do_mag(argc - 1, argv + 1); } 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 +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 argument is typically one of /dev/{gyro,accel,mag}i"); + + PRINT_MODULE_USAGE_COMMAND_DESCR("block", "Block sensor topic publication"); + PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("unblock", "Unblock sensor topic publication"); + PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); + + PRINT_MODULE_USAGE_COMMAND_DESCR("sampling", "Set sensor sampling rate"); + PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and sampling rate in Hz", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("rate", "Set sensor publication rate"); + PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and publication rate in Hz", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("range", "Set sensor measurement range"); + PRINT_MODULE_USAGE_ARG(" ", "Sensor device file and range", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("check", "Perform sensor self-test (and print info)"); + PRINT_MODULE_USAGE_ARG("", "Sensor device file", false); +} + +static int do_device(int argc, char *argv[]) { if (argc < 2) { - errx(1, "no device path provided and command provided."); + print_usage(); + return 1; } int fd; - fd = open(argv[0], 0); + fd = open(argv[1], 0); if (fd < 0) { - warn("%s", argv[0]); - errx(1, "FATAL: no device found"); + PX4_ERR("open %s failed (%i)", argv[1], errno); + return 1; } else { int ret; - if (argc == 2 && !strcmp(argv[1], "block")) { + if (argc == 2 && !strcmp(argv[0], "block")) { /* disable the device publications */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); 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 */ ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); if (ret) { - errx(ret, "uORB publications could not be unblocked"); + PX4_ERR("uORB publications could not be unblocked"); + return 1; } } 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[]) { int fd; - fd = open(argv[0], 0); + fd = open(argv[1], 0); if (fd < 0) { - warn("%s", argv[0]); - errx(1, "FATAL: no gyro found"); + PX4_ERR("open %s failed (%i)", argv[1], errno); + return 1; } else { 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 */ ret = ioctl(fd, GYROIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, GYROIOCSRANGE, strtoul(argv[2], NULL, 0)); 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); if (ret) { - warnx("gyro self test FAILED! Check calibration:"); + PX4_WARN("gyro self test FAILED! Check calibration:"); struct gyro_calibration_s scale; ret = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&scale); 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); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, + (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 { - warnx("gyro calibration and self test OK"); + PX4_INFO("gyro calibration and self test OK"); } } 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); @@ -208,78 +243,84 @@ do_gyro(int argc, char *argv[]) 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", - id, calibration_id, srate, prate, range); + 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); close(fd); } - exit(0); + return 0; } -static void +static int do_mag(int argc, char *argv[]) { int fd; - fd = open(argv[0], 0); + fd = open(argv[1], 0); if (fd < 0) { - warn("%s", argv[0]); - errx(1, "FATAL: no magnetometer found"); + PX4_ERR("open %s failed (%i)", argv[1], errno); + return 1; } else { 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 */ ret = ioctl(fd, MAGIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, MAGIOCSRANGE, strtoul(argv[2], NULL, 0)); 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); if (ret) { - warnx("mag self test FAILED! Check calibration:"); + PX4_WARN("mag self test FAILED! Check calibration:"); struct mag_calibration_s scale; ret = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&scale); 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); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, + (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 { - warnx("mag calibration and self test OK"); + PX4_INFO("mag calibration and self test OK"); } } 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); @@ -290,78 +331,84 @@ do_mag(int argc, char *argv[]) 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", - id, calibration_id, srate, prate, range); + 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); close(fd); } - exit(0); + return 0; } -static void +static int do_accel(int argc, char *argv[]) { int fd; - fd = open(argv[0], 0); + fd = open(argv[1], 0); if (fd < 0) { - warn("%s", argv[0]); - errx(1, "FATAL: no accelerometer found"); + PX4_ERR("open %s failed (%i)", argv[1], errno); + return 1; } else { 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 */ ret = ioctl(fd, ACCELIOCSSAMPLERATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, SENSORIOCSPOLLRATE, strtoul(argv[2], NULL, 0)); 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 */ ret = ioctl(fd, ACCELIOCSRANGE, strtoul(argv[2], NULL, 0)); 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); if (ret) { - warnx("accel self test FAILED! Check calibration:"); + PX4_WARN("accel self test FAILED! Check calibration:"); struct accel_calibration_s scale; ret = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&scale); 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); - warnx("scale: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_scale, (double)scale.y_scale, (double)scale.z_scale); + PX4_INFO("offsets: X: % 9.6f Y: % 9.6f Z: % 9.6f", (double)scale.x_offset, (double)scale.y_offset, + (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 { - warnx("accel calibration and self test OK"); + PX4_INFO("accel calibration and self test OK"); } } 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); @@ -372,11 +419,11 @@ do_accel(int argc, char *argv[]) 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", - id, calibration_id, srate, prate, range); + 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); close(fd); } - exit(0); + return 0; }