diff options
| author | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-16 18:38:55 +0100 |
|---|---|---|
| committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-03-16 18:38:55 +0100 |
| commit | 772fce9f82749bde8c14199fcc73cfa2235e760e (patch) | |
| tree | a7f00b2a63eca04b4d82280ec30dd5cda97cc2aa /src/modules/commander/state_machine_helper.cpp | |
| parent | bb7962936e15c594821ae10e4510febfec0d6b9f (diff) | |
| download | px4-firmware-772fce9f82749bde8c14199fcc73cfa2235e760e.tar.gz px4-firmware-772fce9f82749bde8c14199fcc73cfa2235e760e.tar.bz2 px4-firmware-772fce9f82749bde8c14199fcc73cfa2235e760e.zip | |
commander: Improve HIL publication blocking call handling
Diffstat (limited to 'src/modules/commander/state_machine_helper.cpp')
| -rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 13 |
1 files changed, 8 insertions, 5 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 31955d3e5..131d6a550 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -309,10 +309,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 { @@ -347,10 +344,16 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s while ((direntry = readdir(d)) != NULL) { int sensfd = ::open(direntry->d_name, 0); - int block_ret = ::ioctl(sensfd, DEVIOCSPUBBLOCK, 0); + + if (sensfd < 0) { + warn("failed opening device"); + return 1; + } + + 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", direntry->d_name, (block_ret == OK) ? "OK" : "FAIL"); } closedir(d); |