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

Merged release_v1.0.0 branch into master

Browse Source
This commit is contained in:
Lorenz Meier
2015-06-25 21:45:17 +02:00
parent 74bdec2cc6 5b93d92339
commit 454becdae5
73 changed files with 2726 additions and 887 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -39,7 +39,6 @@
* @author Julian Oes <julian@oes.ch>
*/
#include <px4_posix.h>
#include <stdio.h>
#include <unistd.h>
#include <stdint.h>
@@ -131,12 +130,12 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
prearm_ret = prearm_check(status, mavlink_fd);
}
#ifdef __PX4_NUTTX
/*
* Perform an atomic state update
*/
#ifdef __PX4_NUTTX
irqstate_t flags = irqsave();
#endif
#endif
/* enforce lockdown in HIL */
if (status->hil_state == vehicle_status_s::HIL_STATE_ON) {
@@ -262,10 +261,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
status->arming_state = new_arming_state;
}
#ifdef __PX4_NUTTX
/* end of atomic state update */
#ifdef __PX4_NUTTX
irqrestore(flags);
#endif
#endif
}
if (ret == TRANSITION_DENIED) {
@@ -366,125 +365,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
return ret;
}
#ifdef __PX4_NUTTX
static transition_result_t disable_publication(const int mavlink_fd)
{
transition_result_t ret;
/* 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_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
return ret;
}
#else
static transition_result_t disable_publication(const int mavlink_fd)
{
transition_result_t ret;
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
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");
}
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
return ret;
}
#endif
/**
* Transition from one hil state to another
*/
transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
transition_result_t hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd)
{
transition_result_t ret = TRANSITION_DENIED;
@@ -495,7 +379,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
switch (new_state) {
case vehicle_status_s::HIL_STATE_OFF:
/* we're in HIL and unexpected things can happen if we disable HIL now */
mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
ret = TRANSITION_DENIED;
break;
@@ -503,7 +387,107 @@ transition_result_t hil_state_transition(hil_state_t new_state, orb_advert_t sta
if (current_status->arming_state == vehicle_status_s::ARMING_STATE_INIT
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY
|| current_status->arming_state == vehicle_status_s::ARMING_STATE_STANDBY_ERROR) {
ret = disable_publication(mavlink_fd);
#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);
#else
const char *devname;
unsigned int handle = 0;
for(;;) {
devname = px4_get_device_names(&handle);
if (devname == NULL)
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");
}
#endif
ret = TRANSITION_CHANGED;
mavlink_log_critical(mavlink_fd, "Switched to ON hil state");
} else {
/* failed opening dir */
mavlink_log_info(mavlink_fd, "FAILED LISTING DEVICE ROOT DIRECTORY");
ret = TRANSITION_DENIED;
}
} else {
mavlink_log_critical(mavlink_fd, "Not switching to HIL when armed");
ret = TRANSITION_DENIED;