mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-23 11:07:36 +08:00
commander: move estimator checks to arming check
This commit is contained in:
Beat Küng
@@ -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()
|
||||
|
||||
Reference in New Issue
Block a user