PX4 Firmware
PX4 Autopilot Software http://px4.io
PreFlightCheck.hpp
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 /**
35  * @file PreFlightCheck.hpp
36  *
37  * Check if flight is safely possible
38  * if not prevent it and inform the user.
39  */
40 
41 #pragma once
42 
43 #include <uORB/topics/safety.h>
45 
47 #include <drivers/drv_hrt.h>
48 
50 {
51 public:
52  PreFlightCheck() = default;
53  ~PreFlightCheck() = default;
54 
55  /**
56  * Runs a preflight check on all sensors to see if they are properly calibrated and healthy
57  *
58  * The function won't fail the test if optional sensors are not found, however,
59  * it will fail the test if optional sensors are found but not in working condition.
60  *
61  * @param mavlink_log_pub
62  * Mavlink output orb handle reference for feedback when a sensor fails
63  * @param checkMag
64  * true if the magneteometer should be checked
65  * @param checkAcc
66  * true if the accelerometers should be checked
67  * @param checkGyro
68  * true if the gyroscopes should be checked
69  * @param checkBaro
70  * true if the barometer should be checked
71  * @param checkAirspeed
72  * true if the airspeed sensor should be checked
73  * @param checkRC
74  * true if the Remote Controller should be checked
75  * @param checkGNSS
76  * true if the GNSS receiver should be checked
77  * @param checkPower
78  * true if the system power should be checked
79  **/
82  const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot);
83 
84  static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags,
85  const safety_s &safety, const uint8_t arm_requirements);
86 
87  typedef enum {
89  ARM_REQ_MISSION_BIT = (1 << 0),
91  ARM_REQ_GPS_BIT = (1 << 2),
94 
95 private:
96  static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
97  const bool optional, int32_t &device_id, const bool report_fail);
98  static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
99  static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
100  const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail);
101  static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
102  const bool optional, int32_t &device_id, const bool report_fail);
103  static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance,
104  const bool optional, int32_t &device_id, const bool report_fail);
105  static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status);
106  static bool airspeedCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool optional,
107  const bool report_fail, const bool prearm);
108  static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL);
109  static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
110  const bool prearm);
111  static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
112  const bool report_fail, const bool enforce_gps_required);
113  static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail,
114  const bool prearm);
115  static bool check_calibration(const char *param_template, const int32_t device_id);
116 };
static bool imuConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status)
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)
static bool magConsistencyCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const bool report_status)
static struct vehicle_status_flags_s status_flags
Definition: Commander.cpp:155
~PreFlightCheck()=default
LidarLite * instance
Definition: ll40ls.cpp:65
static bool failureDetectorCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm)
static int32_t device_id[max_accel_sens]
High-resolution timer with callouts and timekeeping.
static bool preflightCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, vehicle_status_flags_s &status_flags, const bool checkGNSS, bool reportFailures, const bool prearm, const hrt_abstime &time_since_boot)
Runs a preflight check on all sensors to see if they are properly calibrated and healthy.
static struct safety_s safety
Definition: Commander.cpp:140
PreFlightCheck()=default
static bool magnometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail)
static bool gyroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail)
Definition: gyroCheck.cpp:46
static bool baroCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, int32_t &device_id, const bool report_fail)
Definition: baroCheck.cpp:46
__BEGIN_DECLS typedef uint64_t hrt_abstime
Absolute time, in microsecond units.
Definition: drv_hrt.h:58
static int rcCalibrationCheck(orb_advert_t *mavlink_log_pub, bool report_fail, bool isVTOL)
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
Definition: uORB.h:134
static bool ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional, const bool report_fail, const bool enforce_gps_required)
Definition: ekf2Check.cpp:44
static bool accelerometerCheck(orb_advert_t *mavlink_log_pub, vehicle_status_s &status, const uint8_t instance, const bool optional, const bool dynamic, int32_t &device_id, const bool report_fail)
static uint8_t arm_requirements
Definition: Commander.cpp:159
static bool powerCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_s &status, const bool report_fail, const bool prearm)
Definition: powerCheck.cpp:43
static bool preArmCheck(orb_advert_t *mavlink_log_pub, const vehicle_status_flags_s &status_flags, const safety_s &safety, const uint8_t arm_requirements)
Definition: preArmCheck.cpp:40
static bool check_calibration(const char *param_template, const int32_t device_id)