mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-26 16:28:06 +08:00
commander: move estimator checks to arming check
This commit is contained in:
Beat Küng
@@ -7,6 +7,7 @@ uint64 timestamp # time since system start (microseconds)
|
||||
uint32 mode_req_angular_velocity
|
||||
uint32 mode_req_attitude
|
||||
uint32 mode_req_local_position
|
||||
uint32 mode_req_local_position_relaxed
|
||||
uint32 mode_req_global_position
|
||||
uint32 mode_req_local_alt
|
||||
uint32 mode_req_mission
|
||||
@@ -23,6 +24,7 @@ bool angular_velocity_valid
|
||||
bool attitude_valid
|
||||
bool local_altitude_valid
|
||||
bool local_position_valid # set to true by the commander app if the quality of the local position estimate is good enough to use for navigation
|
||||
bool local_position_valid_relaxed # Local position with reduced accuracy requirements (e.g. flying with optical flow)
|
||||
bool local_velocity_valid # set to true by the commander app if the quality of the local horizontal velocity data is good enough to use for navigation
|
||||
bool global_position_valid # set to true by the commander app if the quality of the global position estimate is good enough to use for navigation
|
||||
bool gps_position_valid
|
||||
@@ -30,12 +32,6 @@ bool home_position_valid # indicates a valid home position (a valid home positi
|
||||
bool escs_error # set to true if one or more ESCs reporting esc_status are offline
|
||||
bool escs_failure # set to true if one or more ESCs reporting esc_status has a failure
|
||||
|
||||
bool position_reliant_on_gps
|
||||
bool position_reliant_on_optical_flow
|
||||
bool position_reliant_on_vision_position
|
||||
|
||||
bool dead_reckoning
|
||||
|
||||
bool offboard_control_signal_lost
|
||||
|
||||
bool rc_signal_found_once
|
||||
|
||||
@@ -39,6 +39,6 @@ target_include_directories(ArmStateMachine PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
|
||||
target_link_libraries(ArmStateMachine PUBLIC health_and_arming_checks)
|
||||
|
||||
px4_add_functional_gtest(SRC ArmStateMachineTest.cpp
|
||||
LINKLIBS ArmStateMachine health_and_arming_checks sensor_calibration ArmAuthorization mode_util
|
||||
LINKLIBS ArmStateMachine health_and_arming_checks hysteresis sensor_calibration ArmAuthorization mode_util
|
||||
)
|
||||
|
||||
|
||||
@@ -787,9 +787,6 @@ Commander::Commander() :
|
||||
// default for vtol is rotary wing
|
||||
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
|
||||
|
||||
_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
|
||||
_vehicle_gps_position_valid.set_hysteresis_time_from(true, 0);
|
||||
|
||||
_param_mav_comp_id = param_find("MAV_COMP_ID");
|
||||
_param_mav_sys_id = param_find("MAV_SYS_ID");
|
||||
_param_mav_type = param_find("MAV_TYPE");
|
||||
@@ -2109,11 +2106,6 @@ Commander::run()
|
||||
|
||||
_boot_timestamp = hrt_absolute_time();
|
||||
|
||||
// initially set to failed
|
||||
_last_lpos_fail_time_us = _boot_timestamp;
|
||||
_last_gpos_fail_time_us = _boot_timestamp;
|
||||
_last_lvel_fail_time_us = _boot_timestamp;
|
||||
|
||||
arm_auth_init(&_mavlink_log_pub, &_vehicle_status.system_id);
|
||||
|
||||
while (!should_exit()) {
|
||||
@@ -3209,42 +3201,6 @@ void Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
||||
}
|
||||
}
|
||||
|
||||
bool Commander::check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
|
||||
const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us,
|
||||
const bool was_valid)
|
||||
{
|
||||
bool valid = was_valid;
|
||||
const bool data_stale = ((hrt_elapsed_time(&data_timestamp_us) > _param_com_pos_fs_delay.get() * 1_s)
|
||||
|| (data_timestamp_us == 0));
|
||||
const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
|
||||
const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
|
||||
|
||||
// Check accuracy with hysteresis in both test level and time
|
||||
if (level_check_pass) {
|
||||
if (!was_valid) {
|
||||
// check if probation period has elapsed
|
||||
if (hrt_elapsed_time(&last_fail_time_us) > 1_s) {
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// level check failed
|
||||
if (was_valid) {
|
||||
// FAILURE! no longer valid
|
||||
valid = false;
|
||||
}
|
||||
|
||||
last_fail_time_us = hrt_absolute_time();
|
||||
}
|
||||
|
||||
if (was_valid != valid) {
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
void
|
||||
Commander::update_control_mode()
|
||||
{
|
||||
@@ -3745,9 +3701,6 @@ void Commander::battery_status_check()
|
||||
|
||||
void Commander::estimator_check()
|
||||
{
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
const bool run_quality_checks = true;
|
||||
|
||||
_local_position_sub.update();
|
||||
_global_position_sub.update();
|
||||
|
||||
@@ -3761,263 +3714,6 @@ void Commander::estimator_check()
|
||||
_heading_reset_counter = lpos.heading_reset_counter;
|
||||
}
|
||||
|
||||
const bool mag_fault_prev = _estimator_status_flags_sub.get().cs_mag_fault;
|
||||
const bool gnss_heading_fault_prev = _estimator_status_flags_sub.get().cs_gps_yaw_fault;
|
||||
|
||||
// use primary estimator_status
|
||||
if (_estimator_selector_status_sub.updated()) {
|
||||
estimator_selector_status_s estimator_selector_status;
|
||||
|
||||
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
|
||||
if (estimator_selector_status.primary_instance != _estimator_status_sub.get_instance()) {
|
||||
_estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
_estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_estimator_status_flags_sub.update()) {
|
||||
const estimator_status_flags_s &estimator_status_flags = _estimator_status_flags_sub.get();
|
||||
|
||||
_vehicle_status_flags.dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
|
||||
|| estimator_status_flags.cs_inertial_dead_reckoning;
|
||||
|
||||
if (!(estimator_status_flags.cs_inertial_dead_reckoning || estimator_status_flags.cs_wind_dead_reckoning)) {
|
||||
// position requirements (update if not dead reckoning)
|
||||
bool gps = estimator_status_flags.cs_gps;
|
||||
bool optical_flow = estimator_status_flags.cs_opt_flow;
|
||||
bool vision_position = estimator_status_flags.cs_ev_pos;
|
||||
|
||||
_vehicle_status_flags.position_reliant_on_gps = gps && !optical_flow && !vision_position;
|
||||
_vehicle_status_flags.position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
|
||||
_vehicle_status_flags.position_reliant_on_vision_position = !gps && !optical_flow && vision_position;
|
||||
}
|
||||
|
||||
// Check for a magnetomer fault and notify the user
|
||||
if (!mag_fault_prev && estimator_status_flags.cs_mag_fault) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Compass needs calibration - Land now!\t");
|
||||
events::send(events::ID("commander_stopping_mag_use"), events::Log::Critical,
|
||||
"Stopping compass use! Land now and calibrate the compass");
|
||||
}
|
||||
|
||||
if (!gnss_heading_fault_prev && estimator_status_flags.cs_gps_yaw_fault) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "GNSS heading not reliable - Land now!\t");
|
||||
events::send(events::ID("commander_stopping_gnss_heading_use"), events::Log::Critical,
|
||||
"GNSS heading not reliable. Land now!");
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
|
||||
* for a short time interval after takeoff.
|
||||
* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
|
||||
* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
|
||||
* if this does not fix the issue we need to stop using a position controlled
|
||||
* mode to prevent flyaway crashes.
|
||||
*/
|
||||
bool pre_flt_fail_innov_heading = false;
|
||||
bool pre_flt_fail_innov_vel_horiz = false;
|
||||
|
||||
if (_estimator_status_sub.updated()) {
|
||||
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub.copy(&estimator_status)) {
|
||||
|
||||
pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
|
||||
pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
|
||||
|
||||
if (run_quality_checks && _vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
|
||||
if (!_arm_state_machine.isArmed()) {
|
||||
_nav_test_failed = false;
|
||||
_nav_test_passed = false;
|
||||
|
||||
} else {
|
||||
if (!_nav_test_passed) {
|
||||
// Both test ratios need to pass/fail together to change the nav test status
|
||||
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
|
||||
&& (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
|
||||
|
||||
const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
|
||||
|
||||
if (innovation_pass) {
|
||||
_time_last_innov_pass = hrt_absolute_time();
|
||||
|
||||
// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
|
||||
const bool sufficient_time = (_vehicle_status.takeoff_time != 0)
|
||||
&& (hrt_elapsed_time(&_vehicle_status.takeoff_time) > 30_s);
|
||||
const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
|
||||
|
||||
// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
|
||||
if (hrt_elapsed_time(&_time_last_innov_fail) > 10_s
|
||||
&& (sufficient_time || sufficient_speed)) {
|
||||
_nav_test_passed = true;
|
||||
_nav_test_failed = false;
|
||||
}
|
||||
|
||||
} else if (innovation_fail) {
|
||||
_time_last_innov_fail = hrt_absolute_time();
|
||||
|
||||
if (!_nav_test_failed && hrt_elapsed_time(&_time_last_innov_pass) > 2_s) {
|
||||
// if the innovation test has failed continuously, declare the nav as failed
|
||||
_nav_test_failed = true;
|
||||
mavlink_log_emergency(&_mavlink_log_pub, "Navigation failure! Land and recalibrate sensors\t");
|
||||
events::send(events::ID("commander_navigation_failure"), events::Log::Emergency,
|
||||
"Navigation failure! Land and recalibrate the sensors");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// run position and velocity accuracy checks
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
if (run_quality_checks) {
|
||||
float lpos_eph_threshold_adj = _param_com_pos_fs_eph.get();
|
||||
|
||||
// relax local position eph threshold in operator controlled position mode
|
||||
if (_commander_state.main_state == commander_state_s::MAIN_STATE_POSCTL &&
|
||||
((_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)
|
||||
|| (_vehicle_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL))) {
|
||||
|
||||
// Set the allowable position uncertainty based on combination of flight and estimator state
|
||||
// When we are in a operator demanded position control mode and are solely reliant on optical flow,
|
||||
// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
|
||||
if (_vehicle_status_flags.position_reliant_on_optical_flow) {
|
||||
lpos_eph_threshold_adj = INFINITY;
|
||||
}
|
||||
}
|
||||
|
||||
bool xy_valid = lpos.xy_valid && !_nav_test_failed;
|
||||
bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
|
||||
|
||||
if (!_arm_state_machine.isArmed()) {
|
||||
if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
|
||||
xy_valid = false;
|
||||
}
|
||||
|
||||
if (pre_flt_fail_innov_vel_horiz) {
|
||||
v_xy_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
const vehicle_global_position_s &gpos = _global_position_sub.get();
|
||||
|
||||
_vehicle_status_flags.global_position_valid =
|
||||
check_posvel_validity(xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
|
||||
_last_gpos_fail_time_us, _vehicle_status_flags.global_position_valid);
|
||||
|
||||
_vehicle_status_flags.local_position_valid =
|
||||
check_posvel_validity(xy_valid, lpos.eph, lpos_eph_threshold_adj, lpos.timestamp,
|
||||
_last_lpos_fail_time_us, _vehicle_status_flags.local_position_valid);
|
||||
|
||||
_vehicle_status_flags.local_velocity_valid =
|
||||
check_posvel_validity(v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
|
||||
_last_lvel_fail_time_us, _vehicle_status_flags.local_velocity_valid);
|
||||
}
|
||||
|
||||
|
||||
// altitude
|
||||
_vehicle_status_flags.local_altitude_valid = lpos.z_valid
|
||||
&& (hrt_elapsed_time(&lpos.timestamp) < (_param_com_pos_fs_delay.get() * 1_s));
|
||||
|
||||
|
||||
// attitude
|
||||
vehicle_attitude_s attitude{};
|
||||
_vehicle_attitude_sub.copy(&attitude);
|
||||
const matrix::Quatf q{attitude.q};
|
||||
const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f)
|
||||
&& (fabsf(q(1)) <= 1.f)
|
||||
&& (fabsf(q(2)) <= 1.f)
|
||||
&& (fabsf(q(3)) <= 1.f);
|
||||
const bool norm_in_tolerance = (fabsf(1.f - q.norm()) <= 1e-6f);
|
||||
|
||||
const bool attitude_valid = (hrt_elapsed_time(&attitude.timestamp) < 1_s)
|
||||
&& norm_in_tolerance && no_element_larger_than_one;
|
||||
|
||||
if (_vehicle_status_flags.attitude_valid && !attitude_valid) {
|
||||
PX4_ERR("attitude estimate no longer valid");
|
||||
}
|
||||
|
||||
_vehicle_status_flags.attitude_valid = attitude_valid;
|
||||
|
||||
|
||||
// angular velocity
|
||||
vehicle_angular_velocity_s angular_velocity{};
|
||||
_vehicle_angular_velocity_sub.copy(&angular_velocity);
|
||||
const bool condition_angular_velocity_time_valid = (angular_velocity.timestamp != 0)
|
||||
&& (hrt_elapsed_time(&angular_velocity.timestamp) < 1_s);
|
||||
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
|
||||
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
|
||||
const bool angular_velocity_valid = condition_angular_velocity_time_valid
|
||||
&& condition_angular_velocity_finite;
|
||||
|
||||
if (_vehicle_status_flags.angular_velocity_valid && !angular_velocity_valid) {
|
||||
const char err_str[] {"angular velocity no longer valid"};
|
||||
|
||||
if (!condition_angular_velocity_time_valid) {
|
||||
PX4_ERR("%s (timeout)", err_str);
|
||||
|
||||
} else if (!condition_angular_velocity_finite) {
|
||||
PX4_ERR("%s (non-finite values)", err_str);
|
||||
}
|
||||
}
|
||||
|
||||
_vehicle_status_flags.angular_velocity_valid = angular_velocity_valid;
|
||||
|
||||
|
||||
// gps
|
||||
const bool condition_gps_position_was_valid = _vehicle_status_flags.gps_position_valid;
|
||||
|
||||
if (_vehicle_gps_position_sub.updated()) {
|
||||
sensor_gps_s vehicle_gps_position;
|
||||
|
||||
if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
|
||||
|
||||
bool time = (vehicle_gps_position.timestamp != 0) && (hrt_elapsed_time(&vehicle_gps_position.timestamp) < 1_s);
|
||||
|
||||
bool fix = vehicle_gps_position.fix_type >= 2;
|
||||
bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
|
||||
bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
|
||||
bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
|
||||
bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
|
||||
|
||||
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, hrt_absolute_time());
|
||||
_vehicle_status_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
|
||||
|
||||
_vehicle_gps_position_timestamp_last = vehicle_gps_position.timestamp;
|
||||
|
||||
if (condition_gps_position_was_valid) {
|
||||
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
|
||||
mavlink_log_warning(&_mavlink_log_pub, "GPS jamming state critical\t");
|
||||
events::send(events::ID("commander_gps_jamming_critical"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"GPS jamming state critical");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
const hrt_abstime now_us = hrt_absolute_time();
|
||||
|
||||
if (now_us > _vehicle_gps_position_timestamp_last + GPS_VALID_TIME) {
|
||||
_vehicle_gps_position_valid.set_state_and_update(false, now_us);
|
||||
_vehicle_status_flags.gps_position_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
if (condition_gps_position_was_valid && !_vehicle_status_flags.gps_position_valid) {
|
||||
PX4_DEBUG("GPS no longer valid");
|
||||
|
||||
// report GPS failure if flying and the global position estimate is still valid
|
||||
if (!_vehicle_land_detected.landed && _vehicle_status_flags.global_position_valid) {
|
||||
mavlink_log_warning(&_mavlink_log_pub, "GPS no longer valid\t");
|
||||
events::send(events::ID("commander_gps_lost"), {events::Log::Critical, events::LogInternal::Info},
|
||||
"GPS no longer valid");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Commander::manual_control_check()
|
||||
|
||||
@@ -70,9 +70,6 @@
|
||||
#include <uORB/topics/cpuload.h>
|
||||
#include <uORB/topics/distance_sensor.h>
|
||||
#include <uORB/topics/esc_status.h>
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/geofence_result.h>
|
||||
#include <uORB/topics/iridiumsbd_status.h>
|
||||
#include <uORB/topics/manual_control_setpoint.h>
|
||||
@@ -84,8 +81,6 @@
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
#include <uORB/topics/system_power.h>
|
||||
#include <uORB/topics/telemetry_status.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_command.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/vehicle_land_detected.h>
|
||||
@@ -134,10 +129,6 @@ private:
|
||||
|
||||
void battery_status_check();
|
||||
|
||||
bool check_posvel_validity(const bool data_valid, const float data_accuracy, const float required_accuracy,
|
||||
const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us,
|
||||
const bool was_valid);
|
||||
|
||||
void control_status_leds(bool changed, const uint8_t battery_warning);
|
||||
|
||||
/**
|
||||
@@ -205,13 +196,8 @@ private:
|
||||
(ParamBool<px4::params::COM_HOME_EN>) _param_com_home_en,
|
||||
(ParamBool<px4::params::COM_HOME_IN_AIR>) _param_com_home_in_air,
|
||||
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv, /*Not realy used for now*/
|
||||
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
|
||||
(ParamInt<px4::params::COM_POSCTL_NAVL>) _param_com_posctl_navl, /* failsafe response to loss of navigation accuracy */
|
||||
|
||||
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay,
|
||||
|
||||
(ParamInt<px4::params::COM_LOW_BAT_ACT>) _param_com_low_bat_act,
|
||||
(ParamFloat<px4::params::COM_BAT_ACT_T>) _param_com_bat_act_t,
|
||||
(ParamInt<px4::params::COM_IMB_PROP_ACT>) _param_com_imb_prop_act,
|
||||
@@ -293,20 +279,6 @@ private:
|
||||
|
||||
ArmStateMachine _arm_state_machine{};
|
||||
|
||||
hrt_abstime _last_gpos_fail_time_us{0}; /**< Last time that the global position validity recovery check failed (usec) */
|
||||
hrt_abstime _last_lpos_fail_time_us{0}; /**< Last time that the local position validity recovery check failed (usec) */
|
||||
hrt_abstime _last_lvel_fail_time_us{0}; /**< Last time that the local velocity validity recovery check failed (usec) */
|
||||
|
||||
/* class variables used to check for navigation failure after takeoff */
|
||||
hrt_abstime _time_last_innov_pass{0}; /**< last time velocity and position innovations passed */
|
||||
hrt_abstime _time_last_innov_fail{0}; /**< last time velocity and position innovations failed */
|
||||
bool _nav_test_passed{false}; /**< true if the post takeoff navigation test has passed */
|
||||
bool _nav_test_failed{false}; /**< true if the post takeoff navigation test has failed */
|
||||
|
||||
static constexpr hrt_abstime GPS_VALID_TIME{3_s};
|
||||
Hysteresis _vehicle_gps_position_valid{false};
|
||||
hrt_abstime _vehicle_gps_position_timestamp_last{0};
|
||||
|
||||
bool _geofence_loiter_on{false};
|
||||
bool _geofence_rtl_on{false};
|
||||
bool _geofence_land_on{false};
|
||||
@@ -392,15 +364,11 @@ private:
|
||||
uORB::Subscription _action_request_sub {ORB_ID(action_request)};
|
||||
uORB::Subscription _cpuload_sub{ORB_ID(cpuload)};
|
||||
uORB::Subscription _esc_status_sub{ORB_ID(esc_status)};
|
||||
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _geofence_result_sub{ORB_ID(geofence_result)};
|
||||
uORB::Subscription _iridiumsbd_status_sub{ORB_ID(iridiumsbd_status)};
|
||||
uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
uORB::Subscription _vtol_vehicle_status_sub{ORB_ID(vtol_vehicle_status)};
|
||||
@@ -414,7 +382,6 @@ private:
|
||||
uORB::Subscription _power_button_state_sub {ORB_ID(power_button_state)};
|
||||
#endif // BOARD_HAS_POWER_CONTROL
|
||||
|
||||
uORB::SubscriptionData<estimator_status_flags_s> _estimator_status_flags_sub{ORB_ID(estimator_status_flags)};
|
||||
uORB::SubscriptionData<mission_result_s> _mission_result_sub{ORB_ID(mission_result)};
|
||||
uORB::SubscriptionData<offboard_control_mode_s> _offboard_control_mode_sub{ORB_ID(offboard_control_mode)};
|
||||
uORB::SubscriptionData<vehicle_global_position_s> _global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
|
||||
@@ -124,7 +124,7 @@ private:
|
||||
&_imu_consistency_checks,
|
||||
&_magnetometer_checks,
|
||||
&_manual_control_checks,
|
||||
&_mode_checks,
|
||||
&_mode_checks, // must be after _estimator_checks
|
||||
&_parachute_checks,
|
||||
&_power_checks,
|
||||
&_rc_calibration_checks,
|
||||
|
||||
@@ -35,21 +35,48 @@
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
EstimatorChecks::EstimatorChecks()
|
||||
{
|
||||
_vehicle_gps_position_valid.set_hysteresis_time_from(false, GPS_VALID_TIME);
|
||||
_vehicle_gps_position_valid.set_hysteresis_time_from(true, 0);
|
||||
|
||||
// initially set to failed
|
||||
_last_lpos_fail_time_us = hrt_absolute_time();
|
||||
_last_lpos_relaxed_fail_time_us = _last_lpos_fail_time_us;
|
||||
_last_gpos_fail_time_us = _last_lpos_fail_time_us;
|
||||
_last_lvel_fail_time_us = _last_lpos_fail_time_us;
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
{
|
||||
if (_param_sys_mc_est_group.get() != 2) {
|
||||
return;
|
||||
sensor_gps_s vehicle_gps_position;
|
||||
|
||||
if (_vehicle_gps_position_sub.copy(&vehicle_gps_position)) {
|
||||
checkGps(context, reporter, vehicle_gps_position);
|
||||
|
||||
} else {
|
||||
vehicle_gps_position = {};
|
||||
}
|
||||
|
||||
const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
|
||||
bool missing_data = false;
|
||||
vehicle_local_position_s lpos;
|
||||
|
||||
if (!_vehicle_local_position_sub.copy(&lpos)) {
|
||||
lpos = {};
|
||||
}
|
||||
|
||||
bool pre_flt_fail_innov_heading = false;
|
||||
bool pre_flt_fail_innov_vel_horiz = false;
|
||||
bool missing_data = false;
|
||||
const NavModes required_groups = (NavModes)reporter.failsafeFlags().mode_req_attitude;
|
||||
|
||||
// Change topics to primary estimator instance
|
||||
if (_param_sens_imu_mode.get() == 0) { // multi-ekf
|
||||
estimator_selector_status_s estimator_selector_status;
|
||||
|
||||
if (_estimator_selector_status_sub.copy(&estimator_selector_status)) {
|
||||
bool instance_changed = _estimator_status_sub.ChangeInstance(estimator_selector_status.primary_instance)
|
||||
&& _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
&& _estimator_sensor_bias_sub.ChangeInstance(estimator_selector_status.primary_instance)
|
||||
&& _estimator_status_flags_sub.ChangeInstance(estimator_selector_status.primary_instance);
|
||||
|
||||
if (!instance_changed) {
|
||||
missing_data = true;
|
||||
@@ -64,8 +91,11 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
estimator_status_s estimator_status;
|
||||
|
||||
if (_estimator_status_sub.copy(&estimator_status)) {
|
||||
pre_flt_fail_innov_heading = estimator_status.pre_flt_fail_innov_heading;
|
||||
pre_flt_fail_innov_vel_horiz = estimator_status.pre_flt_fail_innov_vel_horiz;
|
||||
|
||||
checkEstimatorStatus(context, reporter, estimator_status, required_groups);
|
||||
checkEstimatorStatusFlags(context, reporter, estimator_status, lpos);
|
||||
|
||||
} else {
|
||||
missing_data = true;
|
||||
@@ -87,6 +117,15 @@ void EstimatorChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
reporter.setIsPresent(health_component_t::local_position_estimate);
|
||||
checkSensorBias(context, reporter, required_groups);
|
||||
}
|
||||
|
||||
// set mode requirements
|
||||
const bool condition_gps_position_was_valid = reporter.failsafeFlags().gps_position_valid;
|
||||
setModeRequirementFlags(context, pre_flt_fail_innov_heading, pre_flt_fail_innov_vel_horiz, lpos, vehicle_gps_position,
|
||||
reporter.failsafeFlags());
|
||||
|
||||
if (condition_gps_position_was_valid && !reporter.failsafeFlags().gps_position_valid) {
|
||||
gpsNoLongerValid(context, reporter);
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &reporter,
|
||||
@@ -163,6 +202,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
// check vertical position innovation test ratio
|
||||
if (estimator_status.hgt_test_ratio > _param_com_arm_ekf_hgt.get()) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
@@ -181,6 +221,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
// check velocity innovation test ratio
|
||||
if (estimator_status.vel_test_ratio > _param_com_arm_ekf_vel.get()) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
@@ -199,6 +240,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
// check horizontal position innovation test ratio
|
||||
if (estimator_status.pos_test_ratio > _param_com_arm_ekf_pos.get()) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
@@ -217,6 +259,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
// check magnetometer innovation test ratio
|
||||
if (estimator_status.mag_test_ratio > _param_com_arm_ekf_yaw.get()) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* Test ratio: {1:.3}, limit: {2:.3}.
|
||||
*
|
||||
@@ -256,6 +299,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_GPS_FIX)) {
|
||||
message = "Preflight%s: GPS fix too low";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -267,6 +311,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MIN_SAT_COUNT)) {
|
||||
message = "Preflight%s: not enough GPS Satellites";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -278,6 +323,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_PDOP)) {
|
||||
message = "Preflight%s: GPS PDOP too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -289,6 +335,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_ERR)) {
|
||||
message = "Preflight%s: GPS Horizontal Pos Error too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -300,6 +347,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_ERR)) {
|
||||
message = "Preflight%s: GPS Vertical Pos Error too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -311,6 +359,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_SPD_ERR)) {
|
||||
message = "Preflight%s: GPS Speed Accuracy too low";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -322,6 +371,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_DRIFT)) {
|
||||
message = "Preflight%s: GPS Horizontal Pos Drift too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -333,6 +383,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_DRIFT)) {
|
||||
message = "Preflight%s: GPS Vertical Pos Drift too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -344,6 +395,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_HORZ_SPD_ERR)) {
|
||||
message = "Preflight%s: GPS Hor Speed Drift too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -355,6 +407,7 @@ void EstimatorChecks::checkEstimatorStatus(const Context &context, Report &repor
|
||||
} else if (estimator_status.gps_check_fail_flags & (1 << estimator_status_s::GPS_CHECK_FAIL_MAX_VERT_SPD_ERR)) {
|
||||
message = "Preflight%s: GPS Vert Speed Drift too high";
|
||||
/* EVENT
|
||||
* @description
|
||||
* <profile name="dev">
|
||||
* This check can be configured via <param>EKF2_GPS_CHECK</param> parameter.
|
||||
* </profile>
|
||||
@@ -415,10 +468,11 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
|
||||
|
||||
if (fabsf(bias.accel_bias[axis_index]) > ekf_ab_test_limit + test_uncertainty) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* An accelerometer recalibration might help.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
|
||||
* Axis {1}: |{2:.8}| \> {3:.8} + {4:.8}
|
||||
*
|
||||
* This check can be configured via <param>EKF2_ABL_LIM</param> parameter.
|
||||
* </profile>
|
||||
@@ -448,10 +502,11 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
|
||||
|
||||
if (fabsf(bias.gyro_bias[axis_index]) > ekf_gb_test_limit + test_uncertainty) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* A Gyro recalibration might help.
|
||||
*
|
||||
* <profile name="dev">
|
||||
* Axis {1}: |{2:.8}| > {3:.8} + {4:.8}
|
||||
* Axis {1}: |{2:.8}| \> {3:.8} + {4:.8}
|
||||
*
|
||||
* This check can be configured via <param>EKF2_ABL_GYRLIM</param> parameter.
|
||||
* </profile>
|
||||
@@ -471,3 +526,298 @@ void EstimatorChecks::checkSensorBias(const Context &context, Report &reporter,
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkEstimatorStatusFlags(const Context &context, Report &reporter,
|
||||
const estimator_status_s &estimator_status, const vehicle_local_position_s &lpos)
|
||||
{
|
||||
estimator_status_flags_s estimator_status_flags;
|
||||
|
||||
if (_estimator_status_flags_sub.copy(&estimator_status_flags)) {
|
||||
|
||||
bool dead_reckoning = estimator_status_flags.cs_wind_dead_reckoning
|
||||
|| estimator_status_flags.cs_inertial_dead_reckoning;
|
||||
|
||||
if (!dead_reckoning) {
|
||||
// position requirements (update if not dead reckoning)
|
||||
bool gps = estimator_status_flags.cs_gps;
|
||||
bool optical_flow = estimator_status_flags.cs_opt_flow;
|
||||
bool vision_position = estimator_status_flags.cs_ev_pos;
|
||||
|
||||
_position_reliant_on_optical_flow = !gps && optical_flow && !vision_position;
|
||||
}
|
||||
|
||||
// Check for a magnetomer fault and notify the user
|
||||
if (estimator_status_flags.cs_mag_fault) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Land and calibrate the compass.
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_mag_fault"),
|
||||
events::Log::Critical, "Stopping compass use");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Compass needs calibration - Land now!\t");
|
||||
}
|
||||
}
|
||||
|
||||
if (estimator_status_flags.cs_gps_yaw_fault) {
|
||||
/* EVENT
|
||||
* @description
|
||||
* Land now
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_gnss_fault"),
|
||||
events::Log::Critical, "GNSS heading not reliable");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "GNSS heading not reliable - Land now!\t");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
/* Check estimator status for signs of bad yaw induced post takeoff navigation failure
|
||||
* for a short time interval after takeoff.
|
||||
* Most of the time, the drone can recover from a bad initial yaw using GPS-inertial
|
||||
* heading estimation (yaw emergency estimator) or GPS heading (fixed wings only), but
|
||||
* if this does not fix the issue we need to stop using a position controlled
|
||||
* mode to prevent flyaway crashes.
|
||||
*/
|
||||
|
||||
if (context.status().vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
|
||||
if (!context.isArmed()) {
|
||||
_nav_test_failed = false;
|
||||
_nav_test_passed = false;
|
||||
|
||||
} else {
|
||||
if (!_nav_test_passed) {
|
||||
// Both test ratios need to pass/fail together to change the nav test status
|
||||
const bool innovation_pass = (estimator_status.vel_test_ratio < 1.f) && (estimator_status.pos_test_ratio < 1.f)
|
||||
&& (estimator_status.vel_test_ratio > FLT_EPSILON) && (estimator_status.pos_test_ratio > FLT_EPSILON);
|
||||
|
||||
const bool innovation_fail = (estimator_status.vel_test_ratio >= 1.f) && (estimator_status.pos_test_ratio >= 1.f);
|
||||
|
||||
if (innovation_pass) {
|
||||
_time_last_innov_pass = now;
|
||||
|
||||
// if nav status is unconfirmed, confirm yaw angle as passed after 30 seconds or achieving 5 m/s of speed
|
||||
const bool sufficient_time = context.status().takeoff_time != 0
|
||||
&& now - context.status().takeoff_time > 30_s;
|
||||
const bool sufficient_speed = matrix::Vector2f(lpos.vx, lpos.vy).longerThan(5.f);
|
||||
|
||||
// Even if the test already failed, allow it to pass if it did not fail during the last 10 seconds
|
||||
if (now - _time_last_innov_fail > 10_s && (sufficient_time || sufficient_speed)) {
|
||||
_nav_test_passed = true;
|
||||
_nav_test_failed = false;
|
||||
}
|
||||
|
||||
} else if (innovation_fail) {
|
||||
_time_last_innov_fail = now;
|
||||
|
||||
if (now - _time_last_innov_pass > 2_s) {
|
||||
// if the innovation test has failed continuously, declare the nav as failed
|
||||
_nav_test_failed = true;
|
||||
/* EVENT
|
||||
* @description
|
||||
* Land and recalibrate the sensors.
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::All, health_component_t::local_position_estimate,
|
||||
events::ID("check_estimator_nav_failure"),
|
||||
events::Log::Emergency, "Navigation failure");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "Navigation failure! Land and recalibrate sensors\t");
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const
|
||||
{
|
||||
if (vehicle_gps_position.jamming_state == sensor_gps_s::JAMMING_STATE_CRITICAL) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure(NavModes::None, health_component_t::gps,
|
||||
events::ID("check_estimator_gps_jamming_critical"),
|
||||
events::Log::Critical, "GPS reports critical jamming state");
|
||||
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_critical(reporter.mavlink_log_pub(), "GPS reports critical jamming state\t");
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::gpsNoLongerValid(const Context &context, Report &reporter) const
|
||||
{
|
||||
PX4_DEBUG("GPS no longer valid");
|
||||
|
||||
// report GPS failure if armed and the global position estimate is still valid
|
||||
if (context.isArmed() && reporter.failsafeFlags().global_position_valid) {
|
||||
if (reporter.mavlink_log_pub()) {
|
||||
mavlink_log_warning(reporter.mavlink_log_pub(), "GPS no longer valid\t");
|
||||
}
|
||||
|
||||
events::send(events::ID("check_estimator_gps_lost"), {events::Log::Error, events::LogInternal::Info},
|
||||
"GPS no longer valid");
|
||||
}
|
||||
}
|
||||
|
||||
void EstimatorChecks::setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading,
|
||||
bool pre_flt_fail_innov_vel_horiz,
|
||||
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position, vehicle_status_flags_s &failsafe_flags)
|
||||
{
|
||||
// The following flags correspond to mode requirements, and are reported in the corresponding mode checks
|
||||
|
||||
const hrt_abstime now = hrt_absolute_time();
|
||||
|
||||
vehicle_global_position_s gpos;
|
||||
|
||||
if (!_vehicle_global_position_sub.copy(&gpos)) {
|
||||
gpos = {};
|
||||
}
|
||||
|
||||
// run position and velocity accuracy checks
|
||||
// Check if quality checking of position accuracy and consistency is to be performed
|
||||
float lpos_eph_threshold_relaxed = _param_com_pos_fs_eph.get();
|
||||
|
||||
// Set the allowable position uncertainty based on combination of flight and estimator state
|
||||
// When we are in a operator demanded position control mode and are solely reliant on optical flow,
|
||||
// do not check position error because it will gradually increase throughout flight and the operator will compensate for the drift
|
||||
if (_position_reliant_on_optical_flow) {
|
||||
lpos_eph_threshold_relaxed = INFINITY;
|
||||
}
|
||||
|
||||
bool xy_valid = lpos.xy_valid && !_nav_test_failed;
|
||||
bool v_xy_valid = lpos.v_xy_valid && !_nav_test_failed;
|
||||
|
||||
if (!context.isArmed()) {
|
||||
if (pre_flt_fail_innov_heading || pre_flt_fail_innov_vel_horiz) {
|
||||
xy_valid = false;
|
||||
}
|
||||
|
||||
if (pre_flt_fail_innov_vel_horiz) {
|
||||
v_xy_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
failsafe_flags.global_position_valid =
|
||||
checkPosVelValidity(now, xy_valid, gpos.eph, _param_com_pos_fs_eph.get(), gpos.timestamp,
|
||||
_last_gpos_fail_time_us, failsafe_flags.global_position_valid);
|
||||
|
||||
failsafe_flags.local_position_valid =
|
||||
checkPosVelValidity(now, xy_valid, lpos.eph, _param_com_pos_fs_eph.get(), lpos.timestamp,
|
||||
_last_lpos_fail_time_us, failsafe_flags.local_position_valid);
|
||||
|
||||
failsafe_flags.local_position_valid_relaxed =
|
||||
checkPosVelValidity(now, xy_valid, lpos.eph, lpos_eph_threshold_relaxed, lpos.timestamp,
|
||||
_last_lpos_relaxed_fail_time_us, failsafe_flags.local_position_valid);
|
||||
|
||||
failsafe_flags.local_velocity_valid =
|
||||
checkPosVelValidity(now, v_xy_valid, lpos.evh, _param_com_vel_fs_evh.get(), lpos.timestamp,
|
||||
_last_lvel_fail_time_us, failsafe_flags.local_velocity_valid);
|
||||
|
||||
|
||||
// altitude
|
||||
failsafe_flags.local_altitude_valid = lpos.z_valid
|
||||
&& (now - lpos.timestamp < (_param_com_pos_fs_delay.get() * 1_s));
|
||||
|
||||
|
||||
// attitude
|
||||
vehicle_attitude_s attitude;
|
||||
|
||||
if (_vehicle_attitude_sub.copy(&attitude)) {
|
||||
const matrix::Quatf q{attitude.q};
|
||||
const float eps = 1e-5f;
|
||||
const bool no_element_larger_than_one = (fabsf(q(0)) <= 1.f + eps)
|
||||
&& (fabsf(q(1)) <= 1.f + eps)
|
||||
&& (fabsf(q(2)) <= 1.f + eps)
|
||||
&& (fabsf(q(3)) <= 1.f + eps);
|
||||
const bool norm_in_tolerance = fabsf(1.f - q.norm()) <= eps;
|
||||
|
||||
failsafe_flags.attitude_valid = now - attitude.timestamp < 1_s && norm_in_tolerance && no_element_larger_than_one;
|
||||
|
||||
} else {
|
||||
failsafe_flags.attitude_valid = false;
|
||||
}
|
||||
|
||||
// angular velocity
|
||||
vehicle_angular_velocity_s angular_velocity{};
|
||||
_vehicle_angular_velocity_sub.copy(&angular_velocity);
|
||||
const bool condition_angular_velocity_time_valid = angular_velocity.timestamp != 0
|
||||
&& now - angular_velocity.timestamp < 1_s;
|
||||
const bool condition_angular_velocity_finite = PX4_ISFINITE(angular_velocity.xyz[0])
|
||||
&& PX4_ISFINITE(angular_velocity.xyz[1]) && PX4_ISFINITE(angular_velocity.xyz[2]);
|
||||
const bool angular_velocity_valid = condition_angular_velocity_time_valid
|
||||
&& condition_angular_velocity_finite;
|
||||
|
||||
if (failsafe_flags.angular_velocity_valid && !angular_velocity_valid) {
|
||||
const char err_str[] {"angular velocity no longer valid"};
|
||||
|
||||
if (!condition_angular_velocity_time_valid) {
|
||||
PX4_ERR("%s (timeout)", err_str);
|
||||
|
||||
} else if (!condition_angular_velocity_finite) {
|
||||
PX4_ERR("%s (non-finite values)", err_str);
|
||||
}
|
||||
}
|
||||
|
||||
failsafe_flags.angular_velocity_valid = angular_velocity_valid;
|
||||
|
||||
|
||||
// gps
|
||||
if (vehicle_gps_position.timestamp != 0) {
|
||||
bool time = now - vehicle_gps_position.timestamp < 1_s;
|
||||
|
||||
bool fix = vehicle_gps_position.fix_type >= 2;
|
||||
bool eph = vehicle_gps_position.eph < _param_com_pos_fs_eph.get();
|
||||
bool epv = vehicle_gps_position.epv < _param_com_pos_fs_epv.get();
|
||||
bool evh = vehicle_gps_position.s_variance_m_s < _param_com_vel_fs_evh.get();
|
||||
bool no_jamming = (vehicle_gps_position.jamming_state != sensor_gps_s::JAMMING_STATE_CRITICAL);
|
||||
|
||||
_vehicle_gps_position_valid.set_state_and_update(time && fix && eph && epv && evh && no_jamming, now);
|
||||
failsafe_flags.gps_position_valid = _vehicle_gps_position_valid.get_state();
|
||||
|
||||
} else {
|
||||
failsafe_flags.gps_position_valid = false;
|
||||
}
|
||||
}
|
||||
|
||||
bool EstimatorChecks::checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
|
||||
const float required_accuracy,
|
||||
const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us,
|
||||
const bool was_valid) const
|
||||
{
|
||||
bool valid = was_valid;
|
||||
const bool data_stale = (now - data_timestamp_us > _param_com_pos_fs_delay.get() * 1_s)
|
||||
|| (data_timestamp_us == 0);
|
||||
const float req_accuracy = (was_valid ? required_accuracy * 2.5f : required_accuracy);
|
||||
const bool level_check_pass = data_valid && !data_stale && (data_accuracy < req_accuracy);
|
||||
|
||||
// Check accuracy with hysteresis in both test level and time
|
||||
if (level_check_pass) {
|
||||
if (!was_valid) {
|
||||
// check if probation period has elapsed
|
||||
if (now - last_fail_time_us > 1_s) {
|
||||
valid = true;
|
||||
}
|
||||
}
|
||||
|
||||
} else {
|
||||
// level check failed
|
||||
if (was_valid) {
|
||||
// FAILURE! no longer valid
|
||||
valid = false;
|
||||
}
|
||||
|
||||
last_fail_time_us = now;
|
||||
}
|
||||
|
||||
return valid;
|
||||
}
|
||||
|
||||
|
||||
@@ -39,13 +39,21 @@
|
||||
#include <uORB/topics/estimator_selector_status.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_status.h>
|
||||
#include <uORB/topics/estimator_sensor_bias.h>
|
||||
#include <uORB/topics/estimator_status_flags.h>
|
||||
#include <uORB/topics/vehicle_angular_velocity.h>
|
||||
#include <uORB/topics/vehicle_attitude.h>
|
||||
#include <uORB/topics/vehicle_local_position.h>
|
||||
#include <uORB/topics/vehicle_global_position.h>
|
||||
#include <uORB/topics/sensor_gps.h>
|
||||
|
||||
#include <lib/hysteresis/hysteresis.h>
|
||||
|
||||
using namespace time_literals;
|
||||
|
||||
class EstimatorChecks : public HealthAndArmingCheckBase
|
||||
{
|
||||
public:
|
||||
EstimatorChecks() = default;
|
||||
EstimatorChecks();
|
||||
~EstimatorChecks() = default;
|
||||
|
||||
void checkAndReport(const Context &context, Report &reporter) override;
|
||||
@@ -54,13 +62,49 @@ private:
|
||||
void checkEstimatorStatus(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
|
||||
NavModes required_groups);
|
||||
void checkSensorBias(const Context &context, Report &reporter, NavModes required_groups);
|
||||
void checkEstimatorStatusFlags(const Context &context, Report &reporter, const estimator_status_s &estimator_status,
|
||||
const vehicle_local_position_s &lpos);
|
||||
|
||||
void checkGps(const Context &context, Report &reporter, const sensor_gps_s &vehicle_gps_position) const;
|
||||
void gpsNoLongerValid(const Context &context, Report &reporter) const;
|
||||
void setModeRequirementFlags(const Context &context, bool pre_flt_fail_innov_heading, bool pre_flt_fail_innov_vel_horiz,
|
||||
const vehicle_local_position_s &lpos, const sensor_gps_s &vehicle_gps_position,
|
||||
vehicle_status_flags_s &failsafe_flags);
|
||||
|
||||
bool checkPosVelValidity(const hrt_abstime &now, const bool data_valid, const float data_accuracy,
|
||||
const float required_accuracy,
|
||||
const hrt_abstime &data_timestamp_us, hrt_abstime &last_fail_time_us,
|
||||
const bool was_valid) const;
|
||||
|
||||
|
||||
uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
|
||||
uORB::Subscription _estimator_status_sub{ORB_ID(estimator_status)};
|
||||
uORB::Subscription _estimator_status_flags_sub{ORB_ID(estimator_status_flags)};
|
||||
uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
|
||||
|
||||
uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
|
||||
uORB::Subscription _vehicle_local_position_sub{ORB_ID(vehicle_local_position)};
|
||||
uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)};
|
||||
uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
|
||||
uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)};
|
||||
|
||||
hrt_abstime _last_gpos_fail_time_us{0}; ///< Last time that the global position validity recovery check failed (usec)
|
||||
hrt_abstime _last_lpos_fail_time_us{0}; ///< Last time that the local position validity recovery check failed (usec)
|
||||
hrt_abstime _last_lpos_relaxed_fail_time_us{0}; ///< Last time that the relaxed local position validity recovery check failed (usec)
|
||||
hrt_abstime _last_lvel_fail_time_us{0}; ///< Last time that the local velocity validity recovery check failed (usec)
|
||||
|
||||
// variables used to check for navigation failure after takeoff
|
||||
hrt_abstime _time_last_innov_pass{0}; ///< last time velocity and position innovations passed
|
||||
hrt_abstime _time_last_innov_fail{0}; ///< last time velocity and position innovations failed
|
||||
bool _nav_test_passed{false}; ///< true if the post takeoff navigation test has passed
|
||||
bool _nav_test_failed{false}; ///< true if the post takeoff navigation test has failed
|
||||
|
||||
static constexpr hrt_abstime GPS_VALID_TIME{3_s};
|
||||
systemlib::Hysteresis _vehicle_gps_position_valid{false};
|
||||
|
||||
bool _position_reliant_on_optical_flow{false};
|
||||
|
||||
DEFINE_PARAMETERS_CUSTOM_PARENT(HealthAndArmingCheckBase,
|
||||
(ParamInt<px4::params::SYS_MC_EST_GROUP>) _param_sys_mc_est_group,
|
||||
(ParamInt<px4::params::SENS_IMU_MODE>) _param_sens_imu_mode,
|
||||
(ParamInt<px4::params::COM_ARM_MAG_STR>) _param_com_arm_mag_str,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_HGT>) _param_com_arm_ekf_hgt,
|
||||
@@ -68,6 +112,10 @@ private:
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_POS>) _param_com_arm_ekf_pos,
|
||||
(ParamFloat<px4::params::COM_ARM_EKF_YAW>) _param_com_arm_ekf_yaw,
|
||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_com_arm_wo_gps,
|
||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps
|
||||
(ParamBool<px4::params::SYS_HAS_GPS>) _param_sys_has_gps,
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPH>) _param_com_pos_fs_eph,
|
||||
(ParamFloat<px4::params::COM_POS_FS_EPV>) _param_com_pos_fs_epv,
|
||||
(ParamFloat<px4::params::COM_VEL_FS_EVH>) _param_com_vel_fs_evh,
|
||||
(ParamInt<px4::params::COM_POS_FS_DELAY>) _param_com_pos_fs_delay
|
||||
)
|
||||
};
|
||||
|
||||
@@ -64,13 +64,24 @@ void ModeChecks::checkAndReport(const Context &context, Report &reporter)
|
||||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_attitude);
|
||||
}
|
||||
|
||||
NavModes local_position_modes = NavModes::None;
|
||||
|
||||
if (!reporter.failsafeFlags().local_position_valid && reporter.failsafeFlags().mode_req_local_position != 0) {
|
||||
local_position_modes = (NavModes)reporter.failsafeFlags().mode_req_local_position;
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().local_position_valid_relaxed
|
||||
&& reporter.failsafeFlags().mode_req_local_position_relaxed != 0) {
|
||||
local_position_modes = local_position_modes | (NavModes)reporter.failsafeFlags().mode_req_local_position_relaxed;
|
||||
}
|
||||
|
||||
if (local_position_modes != NavModes::None) {
|
||||
/* EVENT
|
||||
*/
|
||||
reporter.armingCheckFailure((NavModes)reporter.failsafeFlags().mode_req_local_position, health_component_t::system,
|
||||
reporter.armingCheckFailure(local_position_modes, health_component_t::system,
|
||||
events::ID("check_modes_local_pos"),
|
||||
events::Log::Error, "No valid local position estimate");
|
||||
reporter.clearCanRunBits((NavModes)reporter.failsafeFlags().mode_req_local_position);
|
||||
reporter.clearCanRunBits(local_position_modes);
|
||||
}
|
||||
|
||||
if (!reporter.failsafeFlags().global_position_valid && reporter.failsafeFlags().mode_req_global_position != 0) {
|
||||
|
||||
@@ -45,6 +45,7 @@ void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
|
||||
flags.mode_req_angular_velocity = 0;
|
||||
flags.mode_req_attitude = 0;
|
||||
flags.mode_req_local_position = 0;
|
||||
flags.mode_req_local_position_relaxed = 0;
|
||||
flags.mode_req_global_position = 0;
|
||||
flags.mode_req_local_alt = 0;
|
||||
flags.mode_req_mission = 0;
|
||||
@@ -63,7 +64,7 @@ void getModeRequirements(uint8_t vehicle_type, vehicle_status_flags_s &flags)
|
||||
// NAVIGATION_STATE_POSCTL
|
||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_angular_velocity);
|
||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_attitude);
|
||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position);
|
||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_local_position_relaxed);
|
||||
|
||||
if (vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING) {
|
||||
setRequirement(vehicle_status_s::NAVIGATION_STATE_POSCTL, flags.mode_req_global_position);
|
||||
|
||||
Reference in New Issue
Block a user