diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:47:35 +0100 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-01-12 11:47:35 +0100 |
commit | b23af6108772f8049ca94dfd8c648e1014917062 (patch) | |
tree | 565f3caba9350e77f2a08071937b574e84ab10ca /src | |
parent | 5a0c6353690a903b0cbf03aee7cb040726f25fd8 (diff) | |
download | px4-firmware-b23af6108772f8049ca94dfd8c648e1014917062.tar.gz px4-firmware-b23af6108772f8049ca94dfd8c648e1014917062.tar.bz2 px4-firmware-b23af6108772f8049ca94dfd8c648e1014917062.zip |
System disables all driver publications it can get hold of once entering HIL
Diffstat (limited to 'src')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 31 |
1 files changed, 31 insertions, 0 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 490fc8fc6..44e3aa787 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,34 @@ 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) { + + bool blocked = false; + int sensfd = ::open(direntry->d_name, 0); + ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + close(sensfd); + + printf("Disabling %s\n: %s", direntry->d_name, (blocked) ? "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; |