diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-11 09:55:29 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-02-11 09:55:29 +0100 |
commit | 4d9f0ccac406f4f4730f331910d9519fc161e6bf (patch) | |
tree | 9e453242f49e67ae9c718fcde81f148421ec10e8 /src/modules/commander | |
parent | efecd85658eecd0b0651a21c37d3936589074e91 (diff) | |
parent | 0388d9adefb33c98f1e4350e3f2ed59a7fdd5359 (diff) | |
download | px4-firmware-4d9f0ccac406f4f4730f331910d9519fc161e6bf.tar.gz px4-firmware-4d9f0ccac406f4f4730f331910d9519fc161e6bf.tar.bz2 px4-firmware-4d9f0ccac406f4f4730f331910d9519fc161e6bf.zip |
Merge branch 'master' into paul_estimator
Diffstat (limited to 'src/modules/commander')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 30 |
1 files changed, 30 insertions, 0 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..7ae61d9ef 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -42,6 +42,8 @@ #include <unistd.h> #include <stdint.h> #include <stdbool.h> +#include <dirent.h> +#include <fcntl.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -51,6 +53,7 @@ #include <systemlib/param/param.h> #include <systemlib/err.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_device.h> #include <mavlink/mavlink_log.h> #include "state_machine_helper.h" @@ -491,6 +494,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; + + // Disable publication of all attached sensors + + /* list directory */ + DIR *d; + struct dirent *direntry; + d = opendir("/dev"); + if (d) { + + while ((direntry = readdir(d)) != NULL) { + + int sensfd = ::open(direntry->d_name, 0); + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + } + + closedir(d); + + warnx("directory listing ok (FS mounted and readable)"); + + } else { + /* failed opening dir */ + warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); + return 1; + } } break; |