PX4 Firmware
PX4 Autopilot Software http://px4.io
ekf2Check.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 <math.h>
38 #include <lib/parameters/param.h>
39 #include <systemlib/mavlink_log.h>
40 #include <uORB/Subscription.hpp>
43 
44 bool PreFlightCheck::ekf2Check(orb_advert_t *mavlink_log_pub, vehicle_status_s &vehicle_status, const bool optional,
45  const bool report_fail, const bool enforce_gps_required)
46 {
47  bool success = true; // start with a pass and change to a fail if any test fails
48  bool present = true;
49  float test_limit = 1.0f; // pass limit re-used for each test
50 
51  bool gps_success = true;
52  bool gps_present = true;
53 
54  // Get estimator status data if available and exit with a fail recorded if not
55  uORB::SubscriptionData<estimator_status_s> status_sub{ORB_ID(estimator_status)};
56  status_sub.update();
57  const estimator_status_s &status = status_sub.get();
58 
59  if (status.timestamp == 0) {
60  present = false;
61  goto out;
62  }
63 
64  // Check if preflight check performed by estimator has failed
65  if (status.pre_flt_fail_innov_heading ||
70  if (report_fail) {
71  if (status.pre_flt_fail_innov_heading) {
72  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: heading estimate not stable");
73 
74  } else if (status.pre_flt_fail_innov_vel_horiz) {
75  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: horizontal velocity estimate not stable");
76 
77  } else if (status.pre_flt_fail_innov_vel_horiz) {
78  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: vertical velocity estimate not stable");
79 
80  } else if (status.pre_flt_fail_innov_height) {
81  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: height estimate not stable");
82 
83  } else if (status.pre_flt_fail_mag_field_disturbed) {
84  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: strong magnetic interference detected");
85  }
86  }
87 
88  success = false;
89  goto out;
90  }
91 
92  // check vertical position innovation test ratio
93  param_get(param_find("COM_ARM_EKF_HGT"), &test_limit);
94 
95  if (status.hgt_test_ratio > test_limit) {
96  if (report_fail) {
97  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Height estimate error");
98  }
99 
100  success = false;
101  goto out;
102  }
103 
104  // check velocity innovation test ratio
105  param_get(param_find("COM_ARM_EKF_VEL"), &test_limit);
106 
107  if (status.vel_test_ratio > test_limit) {
108  if (report_fail) {
109  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Velocity estimate error");
110  }
111 
112  success = false;
113  goto out;
114  }
115 
116  // check horizontal position innovation test ratio
117  param_get(param_find("COM_ARM_EKF_POS"), &test_limit);
118 
119  if (status.pos_test_ratio > test_limit) {
120  if (report_fail) {
121  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Position estimate error");
122  }
123 
124  success = false;
125  goto out;
126  }
127 
128  // check magnetometer innovation test ratio
129  param_get(param_find("COM_ARM_EKF_YAW"), &test_limit);
130 
131  if (status.mag_test_ratio > test_limit) {
132  if (report_fail) {
133  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: Yaw estimate error");
134  }
135 
136  success = false;
137  goto out;
138  }
139 
140  // check accelerometer delta velocity bias estimates
141  param_get(param_find("COM_ARM_EKF_AB"), &test_limit);
142 
143  for (uint8_t index = 13; index < 16; index++) {
144  // allow for higher uncertainty in estimates for axes that are less observable to prevent false positives
145  // adjust test threshold by 3-sigma
146  float test_uncertainty = 3.0f * sqrtf(fmaxf(status.covariances[index], 0.0f));
147 
148  if (fabsf(status.states[index]) > test_limit + test_uncertainty) {
149  if (report_fail) {
150  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Accelerometer Bias");
151  }
152 
153  success = false;
154  goto out;
155  }
156  }
157 
158  // check gyro delta angle bias estimates
159  param_get(param_find("COM_ARM_EKF_GB"), &test_limit);
160 
161  if (fabsf(status.states[10]) > test_limit || fabsf(status.states[11]) > test_limit
162  || fabsf(status.states[12]) > test_limit) {
163  if (report_fail) {
164  mavlink_log_critical(mavlink_log_pub, "Preflight Fail: High Gyro Bias");
165  }
166 
167  success = false;
168  goto out;
169  }
170 
171  // If GPS aiding is required, declare fault condition if the required GPS quality checks are failing
172  if (enforce_gps_required || report_fail) {
173  const bool ekf_gps_fusion = status.control_mode_flags & (1 << estimator_status_s::CS_GPS);
174  const bool ekf_gps_check_fail = status.gps_check_fail_flags > 0;
175 
176  gps_success = ekf_gps_fusion; // default to success if gps data is fused
177 
178  if (ekf_gps_check_fail) {
179  if (report_fail) {
180  // Only report the first failure to avoid spamming
181  const char *message = nullptr;
182 
183  if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
184  message = "Preflight%s: GPS fix too low";
185 
186  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
187  message = "Preflight%s: not enough GPS Satellites";
188 
189  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_PDOP)) {
190  message = "Preflight%s: GPS PDOP too low";
191 
192  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
193  message = "Preflight%s: GPS Horizontal Pos Error too high";
194 
195  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
196  message = "Preflight%s: GPS Vertical Pos Error too high";
197 
198  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
199  message = "Preflight%s: GPS Speed Accuracy too low";
200 
201  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
202  message = "Preflight%s: GPS Horizontal Pos Drift too high";
203 
204  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
205  message = "Preflight%s: GPS Vertical Pos Drift too high";
206 
207  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
208  message = "Preflight%s: GPS Hor Speed Drift too high";
209 
210  } else if (status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
211  message = "Preflight%s: GPS Vert Speed Drift too high";
212 
213  } else {
214  if (!ekf_gps_fusion) {
215  // Likely cause unknown
216  message = "Preflight%s: Estimator not using GPS";
217  gps_present = false;
218 
219  } else {
220  // if we land here there was a new flag added and the code not updated. Show a generic message.
221  message = "Preflight%s: Poor GPS Quality";
222  }
223  }
224 
225  if (message) {
226  if (enforce_gps_required) {
227  mavlink_log_critical(mavlink_log_pub, message, " Fail");
228 
229  } else {
230  mavlink_log_warning(mavlink_log_pub, message, "");
231  }
232  }
233  }
234 
235  gps_success = false;
236 
237  if (enforce_gps_required) {
238  success = false;
239  goto out;
240  }
241  }
242  }
243 
244 out:
245  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_AHRS, present, !optional, success && present, vehicle_status);
246  set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_GPS, gps_present, enforce_gps_required, gps_success, vehicle_status);
247 
248  return success;
249 }
static orb_advert_t * mavlink_log_pub
static struct vehicle_status_s status
Definition: Commander.cpp:138
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
Definition: parameters.cpp:589
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
Definition: HealthFlags.cpp:44
Global flash based parameter store.
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
Definition: uORB.h:87
__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
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Definition: parameters.cpp:370
Contains helper functions to efficiently set the system health flags from commander and preflight che...