diff options
author | Anton Babushkin <anton.babushkin@me.com> | 2014-03-30 00:25:26 +0400 |
---|---|---|
committer | Anton Babushkin <anton.babushkin@me.com> | 2014-03-30 00:25:26 +0400 |
commit | d2553bfd2930eb02664d564559fa361b80c63f61 (patch) | |
tree | 21cda568519c33a261edaaa9c370cc9ba9c7f466 /src/modules/commander/state_machine_helper.cpp | |
parent | 0789189c0588ebebd24a523f9639411be89c6a9b (diff) | |
parent | 9b1de5004c673ebe8bdf68f1b518565cccd6b05b (diff) | |
download | px4-firmware-d2553bfd2930eb02664d564559fa361b80c63f61.tar.gz px4-firmware-d2553bfd2930eb02664d564559fa361b80c63f61.tar.bz2 px4-firmware-d2553bfd2930eb02664d564559fa361b80c63f61.zip |
Merge branch 'master' into offboard2
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 53 |
1 files changed, 44 insertions, 9 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 7ea025e8a..9cecf5371 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -44,6 +44,7 @@ #include <stdbool.h> #include <dirent.h> #include <fcntl.h> +#include <string.h> #include <uORB/uORB.h> #include <uORB/topics/vehicle_status.h> @@ -321,10 +322,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s bool valid_transition = false; int ret = ERROR; - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); valid_transition = true; } else { @@ -352,23 +350,60 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s /* list directory */ DIR *d; - struct dirent *direntry; d = opendir("/dev"); if (d) { + struct dirent *direntry; + char devname[24]; + while ((direntry = readdir(d)) != NULL) { - int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + /* skip serial ports */ + if (!strncmp("tty", direntry->d_name, 3)) { + continue; + } + /* skip mtd devices */ + if (!strncmp("mtd", direntry->d_name, 3)) { + continue; + } + /* skip ram devices */ + if (!strncmp("ram", direntry->d_name, 3)) { + continue; + } + /* skip MMC devices */ + if (!strncmp("mmc", direntry->d_name, 3)) { + continue; + } + /* skip mavlink */ + if (!strcmp("mavlink", direntry->d_name)) { + continue; + } + /* skip console */ + if (!strcmp("console", direntry->d_name)) { + continue; + } + /* skip null */ + if (!strcmp("null", direntry->d_name)) { + continue; + } + + snprintf(devname, sizeof(devname), "/dev/%s", direntry->d_name); + + int sensfd = ::open(devname, 0); + + if (sensfd < 0) { + warn("failed opening device %s", devname); + continue; + } + + int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 1); close(sensfd); - printf("Disabling %s\n: %s", direntry->d_name, (!block_ret) ? "OK" : "FAIL"); + printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR"); } closedir(d); - warnx("directory listing ok (FS mounted and readable)"); - } else { /* failed opening dir */ warnx("FAILED LISTING DEVICE ROOT DIRECTORY"); |