fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

System disables all driver publications it can get hold of once entering HIL

Browse Source
This commit is contained in:
Lorenz Meier 2014-01-12 11:47:35 +01:00
parent 5a0c635369
commit b23af61087
1 changed files with 31 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -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;