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

bresch · 2020-04-17T13:17:26Z

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.

The ekf reset without a correct compensation of the setpoint can lead to a crash.

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.

MaEtUgR

I have some questions, maybe we can quickly discuss.

MaEtUgR · 2020-04-17T13:44:59Z

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

 {
-	_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;


Ah clever so you went around the problem that the counters got initialized on task start with the new frame by not initializing them before the update and the first update now always initializes them given the reset counter is never exactly zero 👍

Can we be sure the reset counter is never zero when there's a valid estimate?

Don't you then get an e.g. _ekfResetHandlerPositionZ() and _smoothing.setCurrentPosition(_position(2)) after every task switch which would override taking over the setpoint from the last task?

Don't you then get an e.g. _ekfResetHandlerPositionZ() and _smoothing.setCurrentPosition(_position(2)) after every task switch which would override taking over the setpoint from the last task?

Ouch, yes, this is correct and not something we want. I need to find an other solution, thanks...

Now, the previous value of the counters is transferred to from the previous to the next FlightTask using a get/set function

MaEtUgR · 2020-04-17T14:08:51Z

src/lib/flight_tasks/tasks/Transition/FlightTaskTransition.cpp

@@ -60,6 +60,7 @@ void FlightTaskTransition::checkSetpoints(vehicle_local_position_setpoint_s &set

 bool FlightTaskTransition::update()
 {
+	FlightTask::update();


FlightTaskOffboard and FlightTaskOrbit need to call the base update as well.
Also to make it fully correct you need to logically and with the return value like for example here: https://github.com/PX4/Firmware/blob/2fca03e1bda313f86fd96d05130e830b9a2ee220/src/lib/flight_tasks/tasks/Manual/FlightTaskManual.cpp#L47

Thanks, I added it for FlightTaskOffboard, but orbit inherits from Altitude, so it's not required

bresch · 2020-04-20T06:49:56Z

@MaEtUgR Should be good now. I tested in SITL that the reset works when EKF resets and that the counter is saved when you switch between tasks

MaEtUgR

Looks correct to me, I just had some minor comments.

MaEtUgR · 2020-04-21T17:07:55Z

src/lib/flight_tasks/FlightTasks.hpp

@@ -81,6 +81,12 @@ class FlightTasks
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();

+	/**
+	 * Get the reset counters from the current task


The name says that but I would probably ask myself what reset counters.

Suggested change

* Get the reset counters from the current task

* Get the estimation reference frame reset counters from the current task

?

MaEtUgR · 2020-04-21T17:08:26Z

src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

-	_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();


In principle you could leave the _checkEkfResetCounters() call in the update Initialize. They would just be checked twice on activation then... works either way.

MaEtUgR · 2020-04-21T17:09:11Z

src/lib/flight_tasks/tasks/FlightTask/FlightTask.hpp

@@ -113,6 +121,9 @@ class FlightTask : public ModuleParams
 	 */
 	const vehicle_local_position_setpoint_s getPositionSetpoint();

+	const ekf_reset_counters_s getResetCounters() const { return _reset_counters; }
+	void setResetCounters(ekf_reset_counters_s counters) { _reset_counters = counters; }


Suggested change

void setResetCounters(ekf_reset_counters_s counters) { _reset_counters = counters; }

void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }

MaEtUgR · 2020-04-21T17:09:28Z

src/lib/flight_tasks/tasks/Offboard/FlightTaskOffboard.cpp

@@ -153,7 +155,7 @@ bool FlightTaskOffboard::update()
 		_position_setpoint.setNaN(); // Don't require any position/velocity setpoints
 		_velocity_setpoint.setNaN();
 		_acceleration_setpoint = Vector3f(0.f, 0.f, 100.f); // High downwards acceleration to make sure there's no thrust
-		return true;
+		return ret;


omg we should to get rid of these early returns, not here.

Yes, it's horrible :|

MaEtUgR

Everything addressed, thanks 👍

bresch added bug Multirotor 🛸 priority-critical labels Apr 17, 2020

bresch added this to the Release v1.11.0 milestone Apr 17, 2020

bresch requested review from dagar and MaEtUgR April 17, 2020 13:17

bresch self-assigned this Apr 17, 2020

MaEtUgR reviewed Apr 17, 2020

View reviewed changes

bresch changed the title ~~FlightTask: Fix ekf2 reset race condition during task switch~~ [WIP]FlightTask: Fix ekf2 reset race condition during task switch Apr 17, 2020

weekly-digest bot mentioned this pull request Apr 19, 2020

Weekly Digest (12 April, 2020 - 19 April, 2020) #14700

Closed

bresch requested a review from MaEtUgR April 20, 2020 06:50

bresch changed the title ~~[WIP]FlightTask: Fix ekf2 reset race condition during task switch~~ FlightTask: Fix ekf2 reset race condition during task switch Apr 20, 2020

bresch force-pushed the pr-ekf-reset-fix branch from d1e6f4b to ca96289 Compare April 20, 2020 07:07

bresch added 3 commits April 20, 2020 09:07

FlightTask: Transfer estimator reset counters from previous to next task

ca96289

FlightTask: call parent update function in offborad task

18bf120

FlightTask: in update function, include returned value of parent class

f85571e

MaEtUgR reviewed Apr 21, 2020

View reviewed changes

Address Maetugr review

2e5cdb6

MaEtUgR approved these changes Apr 22, 2020

View reviewed changes

bresch merged commit be2bb4a into master Apr 22, 2020

bresch deleted the pr-ekf-reset-fix branch April 22, 2020 11:18

weekly-digest bot mentioned this pull request Apr 26, 2020

Weekly Digest (19 April, 2020 - 26 April, 2020) #14761

Closed

MaEtUgR mentioned this pull request Feb 20, 2025

MC: Fix yaw jump when switching from stabilized to another mode #24297

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

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

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

bresch commented Apr 17, 2020

MaEtUgR left a comment

MaEtUgR Apr 17, 2020

MaEtUgR Apr 17, 2020

MaEtUgR Apr 17, 2020

bresch Apr 17, 2020

bresch Apr 20, 2020

MaEtUgR Apr 17, 2020

bresch Apr 20, 2020

bresch commented Apr 20, 2020

MaEtUgR left a comment

MaEtUgR Apr 21, 2020

MaEtUgR Apr 21, 2020

MaEtUgR Apr 21, 2020

MaEtUgR Apr 21, 2020

bresch Apr 22, 2020

MaEtUgR left a comment

		{
		_reset_counters.xy = _sub_vehicle_local_position.get().xy_reset_counter;

	* Get the reset counters from the current task
	* Get the estimation reference frame reset counters from the current task

	void setResetCounters(ekf_reset_counters_s counters) { _reset_counters = counters; }
	void setResetCounters(const ekf_reset_counters_s &counters) { _reset_counters = counters; }

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

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

Conversation

bresch commented Apr 17, 2020

MaEtUgR left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bresch commented Apr 20, 2020

MaEtUgR left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

MaEtUgR left a comment

Choose a reason for hiding this comment