Reset to baro failsafe option sets height estimation to zero

[cimbar]

Hi,
I use external vision source for flight in offboard mode. Lidar Lite is connected to pixhawk and i get distance sensor data from pixhawk and send it back inside of the external vision position data. Primary height source is vision but the height actually comes from distance sensor after filtering on companion computer. Lidar lite started to produce zero values at flight; therefore, height of the vision position became zero as well. Then, ecl/ekf ev hgt timeout - reset to baro failsafe was triggered and position estimation of px4 suddenly changed to zero and this involved improper flight in offboard mode. I think reset to baro option shouldn't suddenly change the height of the vehicle. The last height estimation should be considered after resetting to baro.

To Reproduce
Use primary height source as vision or range EKF2_HGT_MODE 2 or 3
Produce false zero measurements from height source

Expected behavior
ecl/ekf hgt timeout will be triggered and px4 resets the height estimation to baro.
My suggestion is last height estimations should be considered in new estimation with baro before resetting height source.

Log Files and Screenshots
https://review.px4.io/plot_app?log=be81e736-7987-4009-af89-bc90a8dc57f6

Additional context
wq:I2C3 low on stack warning was produced multiple times before this issue occurred.
Main reason of the failure comes from Lidar Lite hardware but px4 failsafe option is not useful for this circumstances.

[bresch]

When the estimator resets to a different height source, the related reset counter is increased by one and the delta (difference in speed or position) is sent.

PX4-Autopilot/msg/vehicle_local_position.msg

Lines 20 to 21 in 850d0bc

	float32 delta_z
	uint8 z_reset_counter

It is the responsibility of the controller to handle this step and absorb it in the setpoint as done here:

PX4-Autopilot/src/lib/flight_tasks/tasks/FlightTask/FlightTask.cpp

Lines 61 to 63 in 46f0388

	if (_sub_vehicle_local_position.get().z_reset_counter != _reset_counters.z) {
	_ekfResetHandlerPositionZ();
	_reset_counters.z = _sub_vehicle_local_position.get().z_reset_counter;

[cimbar]

so what's the cause of resetting to zero as show in the Local Pozition Z graph?

[bresch]

I did not inspect the log, but the reset is located here:
https://github.com/PX4/PX4-ECL/blob/03fed323ab950125114eb34309d3a79119e572ee/EKF/control.cpp#L870-L881

followed by
https://github.com/PX4/PX4-ECL/blob/03fed323ab950125114eb34309d3a79119e572ee/EKF/ekf_helper.cpp#L257

And the baro offset that should absorb most of the jump is updated here while in VIO height/range finder aiding mode:
https://github.com/PX4/PX4-ECL/blob/398fe159ce2b0078f2fd4c94bf4d5b02d7ac0f1f/EKF/ekf_helper.cpp#L1234-L1246

I let you close the issue when you find the root case.

[cimbar]

_baro_hgt_offset is also update in range or gps hgt mode
https://github.com/PX4/PX4-ECL/blob/398fe159ce2b0078f2fd4c94bf4d5b02d7ac0f1f/EKF/ekf_helper.cpp#L237-L250

but it is not updated in ev hgt mode
https://github.com/PX4/PX4-ECL/blob/398fe159ce2b0078f2fd4c94bf4d5b02d7ac0f1f/EKF/ekf_helper.cpp#L286-L302

may this the issue?

[bresch]

@cimbar This might be part of the issue, yes; that part is clearly missing.

However, looking at the log, there are two resets, not just one:

Given the timestamp, the first one seems to be the reset to baro (with a small change) and the 2nd one a reset to EV_height (message not logged) because the control_mode_flags does not indicate a baro fusion: the 28803 value means:

CS_TILT_ALIGN
CS_YAW_ALIGN
CS_IN_AIR
CS_EV_POS
CS_EV_YAW
CS_EV_HGT <---

It seems that the step the occurs on the 2nd reset, which is a reset to ev height. Since the reported ev height is clost to 0, it then makes sense that the estimated height jumps to ~0.

[cimbar]

Thanks for your investigation. In the second reset, external vision height is started to be used as primary height source again, without considering last baro fusion. It might be better to not reset the source more than ones in the same flight or there can be better options for these kind of failsafe situations.

[bresch]

@cimbar I think the EV starting condition is not strict enough; for GPS and range finder aiding for example, we have more checks ensuring the fusion does not start on bad data.

[bresch]

@cimbar I'm closing this PR as the issue has been identified and opened a new one to fix it:
PX4/PX4-ECL#970
I'll let you submit a PR with the required changes or comment suggestions in the new issue.