yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 09:22:18 +00:00
Code Issues Packages Projects Releases Wiki Activity

FlightTask: Fix ekf2 reset race condition during task switch (#14692)

Browse Source 
* FlightTask: Fix ekf2 reset race condition during task switch

During a loss of GPS data when using GPS as primary height source,
the height is reset to baro and the local position gets invalid at the
same time. This triggers a switch to altitude flight task and a setpoint
reset.
This combination of events had the effect to ignore the height reset,
the large sudden height error could create an abrupt change of altitude
or even a crash.
The ekf2 reset is now done at the beginning of each update call.
This commit is contained in:
Mathieu Bresciani
2020-04-22 13:18:35 +02:00
committed by GitHub
parent e82880d6d7
commit be2bb4a479
14 changed files with 85 additions and 42 deletions
Download Patch File Download Diff File Expand all files Collapse all files

12
src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp
View File

@@ -6,6 +6,7 @@ constexpr uint64_t FlightTask::_timeout;
// First index of empty_setpoint corresponds to time-stamp and requires a finite number.
const vehicle_local_position_setpoint_s FlightTask::empty_setpoint = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, NAN, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {NAN, NAN, NAN}, {}};
const ekf_reset_counters_s FlightTask::zero_reset_counters = {};
const vehicle_constraints_s FlightTask::empty_constraints = {0, NAN, NAN, NAN, NAN, NAN, NAN, NAN, false, {}};
const landing_gear_s FlightTask::empty_landing_gear_default_keep = {0, landing_gear_s::GEAR_KEEP, {}};
@@ -13,7 +14,6 @@ bool FlightTask::activate(vehicle_local_position_setpoint_s last_setpoint)
{
_resetSetpoints();
_setDefaultConstraints();
_initEkfResetCounters();
_time_stamp_activate = hrt_absolute_time();
_gear = empty_landing_gear_default_keep;
return true;
@@ -37,17 +37,13 @@ bool FlightTask::updateInitialize()
_evaluateVehicleLocalPosition();
_evaluateDistanceToGround();
_checkEkfResetCounters();
return true;
}
void FlightTask::_initEkfResetCounters()
bool FlightTask::update()
{
_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;
_reset_counters.vxy = _sub_vehicle_local_position.get().vxy_reset_counter;
_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;
_reset_counters.vz = _sub_vehicle_local_position.get().vz_reset_counter;
_reset_counters.quat = _sub_attitude.get().quat_reset_counter;
_checkEkfResetCounters();
return true;
}
void FlightTask::_checkEkfResetCounters()