dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Improve HIL publication blocking call handling

This commit is contained in:
Lorenz Meier 2014-03-16 18:38:55 +01:00
parent bb7962936e
commit 772fce9f82
1 changed files with 8 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
src/modules/commander/state_machine_helper.cpp
View File

@ -309,10 +309,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
bool valid_transition = false; bool valid_transition = false;
int ret = ERROR; int ret = ERROR;
warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state);
if (current_status->hil_state == new_state) { if (current_status->hil_state == new_state) {
warnx("Hil state not changed");
valid_transition = true; valid_transition = true;
} else { } 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) { while ((direntry = readdir(d)) != NULL) {
int sensfd = ::open(direntry->d_name, 0); 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); 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); closedir(d);