34 #include "../PreFlightCheck.hpp" 45 const bool report_fail,
const bool enforce_gps_required)
49 float test_limit = 1.0f;
51 bool gps_success =
true;
52 bool gps_present =
true;
75 mavlink_log_critical(mavlink_log_pub,
"Preflight Fail: horizontal velocity estimate not stable");
143 for (uint8_t index = 13; index < 16; index++) {
146 float test_uncertainty = 3.0f * sqrtf(fmaxf(status.
covariances[index], 0.0f));
148 if (fabsf(status.
states[index]) > test_limit + test_uncertainty) {
161 if (fabsf(status.
states[10]) > test_limit || fabsf(status.
states[11]) > test_limit
162 || fabsf(status.
states[12]) > test_limit) {
172 if (enforce_gps_required || report_fail) {
173 const bool ekf_gps_fusion = status.
control_mode_flags & (1 << estimator_status_s::CS_GPS);
176 gps_success = ekf_gps_fusion;
178 if (ekf_gps_check_fail) {
181 const char *message =
nullptr;
184 message =
"Preflight%s: GPS fix too low";
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";
190 message =
"Preflight%s: GPS PDOP too low";
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";
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";
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";
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";
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";
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";
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";
214 if (!ekf_gps_fusion) {
216 message =
"Preflight%s: Estimator not using GPS";
221 message =
"Preflight%s: Poor GPS Quality";
226 if (enforce_gps_required) {
237 if (enforce_gps_required) {
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);
static orb_advert_t * mavlink_log_pub
#define mavlink_log_critical(_pub, _text,...)
Send a mavlink critical message and print to console.
uint32_t control_mode_flags
static struct vehicle_status_s status
__EXPORT int param_get(param_t param, void *val)
Copy the value of a parameter.
bool pre_flt_fail_innov_vel_horiz
void set_health_flags(uint64_t subsystem_type, bool present, bool enabled, bool ok, vehicle_status_s &status)
Global flash based parameter store.
bool pre_flt_fail_innov_vel_vert
#define ORB_ID(_name)
Generates a pointer to the uORB metadata structure for a given topic.
bool pre_flt_fail_innov_heading
bool pre_flt_fail_mag_field_disturbed
__BEGIN_DECLS typedef void * orb_advert_t
ORB topic advertiser handle.
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)
__EXPORT param_t param_find(const char *name)
Look up a parameter by name.
Contains helper functions to efficiently set the system health flags from commander and preflight che...
uint16_t gps_check_fail_flags
bool pre_flt_fail_innov_height
#define mavlink_log_warning(_pub, _text,...)
Send a mavlink warning message and print to console.