diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-10 15:52:19 +0200 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2014-07-10 15:52:19 +0200 |
commit | 3157b06fee99b57fe336f7772b293f8689ff8cdf (patch) | |
tree | 8a3fe2fe22eeaaaec009afec1b829f0d9cfcb27f | |
parent | 58cae259e40a3dcab132f42348d57767fca600a1 (diff) | |
download | px4-firmware-3157b06fee99b57fe336f7772b293f8689ff8cdf.tar.gz px4-firmware-3157b06fee99b57fe336f7772b293f8689ff8cdf.tar.bz2 px4-firmware-3157b06fee99b57fe336f7772b293f8689ff8cdf.zip |
Generalized the airspeed check
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 29 |
1 files changed, 11 insertions, 18 deletions
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 6b96e3a3f..d894c9db0 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -52,6 +52,7 @@ #include <uORB/topics/vehicle_status.h> #include <uORB/topics/actuator_controls.h> #include <uORB/topics/differential_pressure.h> +#include <uORB/topics/airspeed.h> #include <systemlib/systemlib.h> #include <systemlib/param/param.h> #include <systemlib/err.h> @@ -666,29 +667,21 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd) } if (!status->is_rotary_wing) { - fd = open(AIRSPEED_DEVICE_PATH, O_RDONLY); - if (fd < 0) { + fd = orb_subscribe(ORB_ID(airspeed)); + + struct airspeed_s airspeed; + + 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"); - ret = fd; goto system_eval; } - struct differential_pressure_s diff_pres; - - ret = read(fd, &diff_pres, sizeof(diff_pres)); - - if (ret == sizeof(diff_pres)) { - if (fabsf(diff_pres.differential_pressure_filtered_pa > 5.0f)) { - mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); - // XXX do not make this fatal yet - ret = OK; - } - } else { - mavlink_log_critical(mavlink_fd, "#audio: FAIL: AIRSPEED READ"); - /* this is frickin' fatal */ - ret = ERROR; - goto system_eval; + if (fabsf(airspeed.indicated_airspeed_m_s > 5.0f)) { + mavlink_log_critical(mavlink_fd, "#audio: WARNING AIRSPEED CALIBRATION MISSING"); + // XXX do not make this fatal yet + ret = OK; } } |