yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

state_machine_helper: remove unneeded code to disable sensor publication

Browse Source 
In HIL mode we do not start the sensors anymore, so this is not needed.

Also it did not work (I did not try to find the reason, just noticed the
sensors kept publishing in HIL mode)
This commit is contained in:
Beat Küng
2017-03-09 11:17:54 +01:00
committed by Lorenz Meier
parent af1984ac07
commit 32995a5bc1
1 changed files with 0 additions and 130 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -493,138 +493,8 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
#ifdef __PX4_NUTTX
/* Disable publication of all attached sensors */
/* list directory */
DIR *d;
d = opendir("/dev");
if (d) {
struct dirent *direntry;
char devname[24];
while ((direntry = readdir(d)) != NULL) {
/* 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: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
closedir(d);
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_log_pub, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_log_pub, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
#else
// Handle VDev devices
const char *devname;
unsigned int handle = 0;
for (;;) {
devname = px4_get_device_names(&handle);
if (devname == nullptr) {
break;
}
/* skip mavlink */
if (!strcmp("/dev/mavlink", devname)) {
continue;
}
int sensfd = px4_open(devname, 0);
if (sensfd < 0) {
warn("failed opening device %s", devname);
continue;
}
int block_ret = px4_ioctl(sensfd, DEVIOCSPUBBLOCK, 1);
px4_close(sensfd);
printf("Disabling %s: %s\n", devname, (block_ret == OK) ? "OK" : "ERROR");
}
// Handle DF devices
const char *df_dev_path;
unsigned int index = 0;
for (;;) {
if (DevMgr::getNextDeviceName(index, &df_dev_path) < 0) {
break;
}
DevHandle h;
DevMgr::getHandle(df_dev_path, h);
if (!h.isValid()) {
warn("failed opening device %s", df_dev_path);
continue;
}
int block_ret = h.ioctl(DEVIOCSPUBBLOCK, 1);
DevMgr::releaseHandle(h);
printf("Disabling %s: %s\n", df_dev_path, (block_ret == OK) ? "OK" : "ERROR");
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_log_pub, "Switched to ON hil state");
#endif
} else {
mavlink_log_critical(mavlink_log_pub, "Not switching to HIL when armed");