diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-02-18 19:09:33 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-02-18 19:09:33 +0400 |
commit | 1eecc73483c860c1c35cb7e381d7e39656f0c2ef (patch) | |
tree | 13d89b34778ca6533f7771eed34f66ae231f8a4f /src/systemcmds/config/config.c | |
parent | 1d40582cc0301f0af330fe69ee63aa8e3d301214 (diff) | |
parent | 3508a42f6a308ad4dc53d6c3efc3b2f8013cc086 (diff) | |
download | px4-firmware-1eecc73483c860c1c35cb7e381d7e39656f0c2ef.tar.gz px4-firmware-1eecc73483c860c1c35cb7e381d7e39656f0c2ef.tar.bz2 px4-firmware-1eecc73483c860c1c35cb7e381d7e39656f0c2ef.zip |
Merge branch 'beta' into ready_rtl_fix
Diffstat (limited to 'src/systemcmds/config/config.c')
-rw-r--r-- | src/systemcmds/config/config.c | 56 |
1 files changed, 49 insertions, 7 deletions
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 80689f20c..476015f3e 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -56,6 +56,7 @@ #include <drivers/drv_gyro.h> #include <drivers/drv_accel.h> #include <drivers/drv_mag.h> +#include <drivers/drv_device.h> #include "systemlib/systemlib.h" #include "systemlib/err.h" @@ -65,6 +66,7 @@ __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[]); int config_main(int argc, char *argv[]) @@ -72,14 +74,12 @@ config_main(int argc, char *argv[]) if (argc >= 2) { if (!strcmp(argv[1], "gyro")) { do_gyro(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "accel")) { + } else if (!strcmp(argv[1], "accel")) { do_accel(argc - 2, argv + 2); - } - - if (!strcmp(argv[1], "mag")) { + } else if (!strcmp(argv[1], "mag")) { do_mag(argc - 2, argv + 2); + } else { + do_device(argc - 1, argv + 1); } } @@ -87,6 +87,48 @@ config_main(int argc, char *argv[]) } static void +do_device(int argc, char *argv[]) +{ + if (argc < 2) { + errx(1, "no device path provided and command provided."); + } + + int fd; + int ret; + + fd = open(argv[0], 0); + + if (fd < 0) { + warn("%s", argv[0]); + errx(1, "FATAL: no device found"); + + } else { + + if (argc == 2 && !strcmp(argv[1], "block")) { + + /* disable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 1); + + if (ret) + errx(ret,"uORB publications could not be blocked"); + + } else if (argc == 2 && !strcmp(argv[1], "unblock")) { + + /* enable the device publications */ + ret = ioctl(fd, DEVIOCSPUBBLOCK, 0); + + if (ret) + errx(ret,"uORB publications could not be unblocked"); + + } else { + errx("no valid command: %s", argv[1]); + } + } + + exit(0); +} + +static void do_gyro(int argc, char *argv[]) { int fd; @@ -124,7 +166,7 @@ do_gyro(int argc, char *argv[]) if (ret) errx(ret,"range could not be set"); - } else if(argc == 1 && !strcmp(argv[0], "check")) { + } else if (argc == 1 && !strcmp(argv[0], "check")) { ret = ioctl(fd, GYROIOCSELFTEST, 0); if (ret) { |