dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

preflight check airspeed use differential_pressure

This commit is contained in:
Daniel Agar 2017-07-08 14:01:14 -04:00 committed by Lorenz Meier
parent e303c2ad89
commit b804616ad0
5 changed files with 37 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/airspeed.msg
View File

@ -3,4 +3,3 @@ float32 true_airspeed_m_s # true filtered airspeed in meters per second, -1 if
float32 true_airspeed_unfiltered_m_s # true airspeed in meters per second, -1 if unknown
float32 air_temperature_celsius # air temperature in degrees celsius, -1000 if unknown
float32 confidence # confidence value from 0 to 1 for this sensor
float32 differential_pressure_filtered_pa # filtered differential pressure, can be negative

42
src/modules/commander/PreflightCheck.cpp
View File

@ -64,9 +64,10 @@
#include <drivers/drv_airspeed.h>
#include <uORB/topics/airspeed.h>
#include <uORB/topics/vehicle_gps_position.h>
#include <uORB/topics/differential_pressure.h>
#include <uORB/topics/estimator_status.h>
#include <uORB/topics/sensor_preflight.h>
#include <uORB/topics/vehicle_gps_position.h>
#include "PreflightCheck.h"
@ -366,15 +367,27 @@ static bool baroCheck(orb_advert_t *mavlink_log_pub, unsigned instance, bool opt
return success;
}
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm, hrt_abstime time_since_boot)
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool report_fail, bool prearm)
{
bool success = true;
int ret;
int fd = orb_subscribe(ORB_ID(airspeed));
int fd_airspeed = orb_subscribe(ORB_ID(airspeed));
int fd_diffpres = orb_subscribe(ORB_ID(differential_pressure));
struct differential_pressure_s differential_pressure;
if ((ret = orb_copy(ORB_ID(differential_pressure), fd_diffpres, &differential_pressure)) ||
(hrt_elapsed_time(&differential_pressure.timestamp) > (500 * 1000))) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
}
success = false;
goto out;
}
struct airspeed_s airspeed;
if ((ret = orb_copy(ORB_ID(airspeed), fd, &airspeed)) ||
if ((ret = orb_copy(ORB_ID(airspeed), fd_airspeed, &airspeed)) ||
(hrt_elapsed_time(&airspeed.timestamp) > (500 * 1000))) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED SENSOR MISSING");
@ -398,27 +411,23 @@ static bool airspeedCheck(orb_advert_t *mavlink_log_pub, bool optional, bool rep
}
/**
* Check if differential pressure is off by more than 15Pa which equals to 5m/s when measuring no airspeed.
* Check if differential pressure is off by more than 15Pa which equals ~5m/s when measuring no airspeed.
* Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
* might have been removed.
*/
if (time_since_boot < 1e6) {
// the airspeed driver filter doesn't deliver the actual value yet
success = false;
goto out;
}
if (fabsf(airspeed.differential_pressure_filtered_pa) > 15.0f && !prearm) {
if (fabsf(differential_pressure.differential_pressure_filtered_pa) > 15.0f && !prearm) {
if (report_fail) {
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: AIRSPEED CALIBRATION BAD OR PITOT UNCOVERED");
mavlink_log_critical(mavlink_log_pub, "PREFLIGHT FAIL: CHECK AIRSPEED CAL OR PITOT");
}
success = false;
goto out;
}
out:
orb_unsubscribe(fd);
orb_unsubscribe(fd_airspeed);
orb_unsubscribe(fd_diffpres);
return success;
}
@ -561,6 +570,11 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot)
{
if (time_since_boot < 1e6) {
// the airspeed driver filter doesn't deliver the actual value yet
return true;
}
#ifdef __PX4_QURT
// WARNING: Preflight checks are important and should be added back when
// all the sensors are supported
@ -706,7 +720,7 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
/* ---- AIRSPEED ---- */
if (checkAirspeed) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm, time_since_boot)) {
if (!airspeedCheck(mavlink_log_pub, true, reportFailures, prearm)) {
failed = true;
}
}

16
src/modules/commander/PreflightCheck.h
View File

@ -72,16 +72,16 @@ bool preflightCheck(orb_advert_t *mavlink_log_pub, bool checkMag, bool checkAcc,
bool checkGyro, bool checkBaro, bool checkAirspeed, bool checkRC, bool checkGNSS,
bool checkDynamic, bool isVTOL, bool reportFailures, bool prearm, hrt_abstime time_since_boot);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;
static constexpr unsigned max_mandatory_gyro_count = 1;
static constexpr unsigned max_optional_gyro_count = 3;
const unsigned max_mandatory_accel_count = 1;
const unsigned max_optional_accel_count = 3;
static constexpr unsigned max_mandatory_accel_count = 1;
static constexpr unsigned max_optional_accel_count = 3;
const unsigned max_mandatory_mag_count = 1;
const unsigned max_optional_mag_count = 4;
static constexpr unsigned max_mandatory_mag_count = 1;
static constexpr unsigned max_optional_mag_count = 4;
const unsigned max_mandatory_baro_count = 1;
const unsigned max_optional_baro_count = 1;
static constexpr unsigned max_mandatory_baro_count = 1;
static constexpr unsigned max_optional_baro_count = 1;
}

2
src/modules/commander/airspeed_calibration.cpp
View File

@ -167,7 +167,7 @@ int do_airspeed_calibration(orb_advert_t *mavlink_log_pub)
if (PX4_ISFINITE(diff_pres_offset)) {
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
int fd_scale = px4_open(AIRSPEED0_DEVICE_PATH, 0);
airscale.offset_pa = diff_pres_offset;
if (fd_scale > 0) {
if (PX4_OK != px4_ioctl(fd_scale, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {

1
src/modules/sensors/sensors.cpp
View File

@ -395,7 +395,6 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw)
_voted_sensors_update.baro_pressure(), air_temperature_celsius));
_airspeed.air_temperature_celsius = air_temperature_celsius;
_airspeed.differential_pressure_filtered_pa = _diff_pres.differential_pressure_filtered_pa;
int instance;
orb_publish_auto(ORB_ID(airspeed), &_airspeed_pub, &_airspeed, &instance, ORB_PRIO_DEFAULT);