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 master into airspeed_test_fix

Browse Source
This commit is contained in:
Lorenz Meier
2014-07-11 22:03:05 +02:00
parent 25f3f6e7f2 66e840ebd7
commit 21ce6676a1
10 changed files with 142 additions and 62 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -71,8 +71,6 @@
#endif
static const int ERROR = -1;
static int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd);
// This array defines the arming state transitions. The rows are the new state, and the columns
// are the current state. Using new state and current state you can index into the array which
// will be true for a valid transition or false for a invalid transition. In some cases even
@@ -623,12 +621,13 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
{
int ret;
bool failed = false;
int fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL SENSOR MISSING");
ret = fd;
failed = true;
goto system_eval;
}
@@ -636,6 +635,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret != OK) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL CALIBRATION");
failed = true;
goto system_eval;
}
@@ -645,29 +645,25 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret == sizeof(acc)) {
/* evaluate values */
float accel_scale = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
if (accel_scale < 9.78f || accel_scale > 9.83f) {
mavlink_log_info(mavlink_fd, "#audio: Accelerometer calibration recommended.");
}
if (accel_scale > 30.0f /* m/s^2 */) {
if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL RANGE");
mavlink_log_info(mavlink_fd, "#audio: hold still while arming");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
} else {
ret = OK;
}
} else {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: ACCEL READ");
/* this is frickin' fatal */
ret = ERROR;
failed = true;
goto system_eval;
}
if (!status->is_rotary_wing) {
/* accel done, close it */
close(fd);
fd = orb_subscribe(ORB_ID(airspeed));
struct airspeed_s airspeed;
@@ -675,17 +671,17 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
if (ret = orb_copy(ORB_ID(airspeed), fd, &airspeed) ||
hrt_elapsed_time(&airspeed.timestamp) > 50 * 1000) {
mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED SENSOR MISSING");
failed = true;
goto system_eval;
}
if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING");
if (fabsf(airspeed.indicated_airspeed_m_s > 6.0f)) {
mavlink_log_critical(mavlink_fd, "#audio: WIND OR CALIBRATION MISSING");
// XXX do not make this fatal yet
ret = OK;
}
}
system_eval:
close(fd);
return ret;
return (failed);
}