PX4 Firmware
PX4 Autopilot Software http://px4.io
airspeedCheck.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 #include "../PreFlightCheck.hpp"
35 
36 #include <HealthFlags.h>
37 #include <drivers/drv_hrt.h>
38 #include <math.h>
39 #include <systemlib/mavlink_log.h>
40 #include <uORB/Subscription.hpp>
41 #include <uORB/topics/airspeed.h>
43 
44 using namespace time_literals;
45 
47  const bool report_fail, const bool prearm)
48 {
49  bool present = true;
50  bool success = true;
51 
52  uORB::SubscriptionData<airspeed_s> airspeed_sub{ORB_ID(airspeed)};
53  airspeed_sub.update();
54  const airspeed_s &airspeed = airspeed_sub.get();
55 
56  if (hrt_elapsed_time(&airspeed.timestamp) > 1_s) {
57  if (report_fail && !optional) {
58  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor missing");
59  }
60 
61  present = false;
62  success = false;
63  goto out;
64  }
65 
66  /*
67  * Check if voter thinks the confidence is low. High-end sensors might have virtually zero noise
68  * on the bench and trigger false positives of the voter. Therefore only fail this
69  * for a pre-arm check, as then the cover is off and the natural airflow in the field
70  * will ensure there is not zero noise.
71  */
72  if (prearm && fabsf(airspeed.confidence) < 0.95f) {
73  if (report_fail) {
74  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Airspeed Sensor stuck");
75  }
76 
77  present = true;
78  success = false;
79  goto out;
80  }
81 
82  /**
83  * Check if airspeed is higher than 4m/s (accepted max) while the vehicle is landed / not flying
84  * Negative and positive offsets are considered. Do not check anymore while arming because pitot cover
85  * might have been removed.
86  */
87  if (fabsf(airspeed.indicated_airspeed_m_s) > 4.0f && !prearm) {
88  if (report_fail) {
89  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: check Airspeed Cal or Pitot");
90  }
91 
92  present = true;
93  success = false;
94  goto out;
95  }
96 
97 out:
98  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_DIFFPRESSURE, present, !optional, success, status);
99 
100  return success;
101 }
static orb_advert_t * mavlink_log_pub
static struct vehicle_status_s status
Definition: Commander.cpp:138
static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional, const bool report_fail, const bool prearm)
High-resolution timer with callouts and timekeeping.
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
Definition: HealthFlags.cpp:44
float confidence
Definition: airspeed.h:57
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
static hrt_abstime hrt_elapsed_time(const hrt_abstime *then)
Compute the delta between a timestamp taken in the past and now.
Definition: drv_hrt.h:102
float indicated_airspeed_m_s
Definition: airspeed.h:54
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
Contains helper functions to efficiently set the system health flags from commander and preflight che...
uint64_t timestamp
Definition: airspeed.h:53