EKF without baro sensor can stop fusion any height permanently

[lukegluke]

I'm working with ground rover without baro, only GPS. I'm using a workaround for EKF initialization without baro (#876), but I found another issue.

If GPS data get stopped, firstly (after 5 secs) we get into controlHeightSensorTimeouts() and trying to reset to baro, but without it we are staying on GPS height fusion (_control_status.flags.gps_hgt is still set).

PX4-ECL/EKF/control.cpp

Lines 832 to 843 in df7f261

	if (reset_to_baro) {
	startBaroHgtFusion();
	request_height_reset = true;
	failing_height_source = "gps";
	new_height_source = "baro";
	} else if (!_gps_hgt_intermittent) {
	request_height_reset = true;
	failing_height_source = "gps";
	new_height_source = "gps";
	}

Then (after 10 secs) we get into stopGpsFusion() where _control_status.flags.gps_hgt get cleared.
That's all. Now none of the height flags is set and controlHeightSensorTimeouts() can't do nothing and will not call startGpsHgtFusion() if GPS is available again. At the same timeGpsFusion for horizontal position is returning back normally.

Additional 'else' to controlHeightSensorTimeouts() like this fix the problem for my particular case:

} else if (_control_status.flags.ev_hgt) {
    ...
} else {
	if (_params.vdist_sensor_type == VDIST_SENSOR_GPS) {
		// check if GPS height is available
		const gpsSample &gps_init = _gps_buffer.get_newest();
		const bool gps_hgt_accurate = (gps_init.vacc < _params.req_vacc);

		// check for inertial sensing errors in the last BADACC_PROBATION seconds
		const bool prev_bad_vert_accel = isRecent(_time_bad_vert_accel, BADACC_PROBATION);

		// reset to GPS if adequate GPS data is available and the timeout cannot be blamed on IMU data
		const bool reset_to_gps = !_gps_hgt_intermittent &&
					((gps_hgt_accurate && !prev_bad_vert_accel) || _baro_hgt_faulty);

		if (reset_to_gps) {
			// set height sensor health
			_baro_hgt_faulty = true;


			startGpsHgtFusion();

			//reset sensor offset because height can drift quite far away
			_hgt_sensor_offset = 0.0f;

			request_height_reset = true;
			failing_height_source = "";
			new_height_source = "gps";

		}
	}
}

I suppose that issue is related to other height sources too. Also as I see even on systems with baro there still a small probability to never return to any height fusion if baro was faulty (_baro_hgt_faulty) on time of height sensor timeout.

For your note, @dagar and for addition to working on #931. Thanks!

[dagar]

FYI I kept the original baro related init changes from #931 in https://github.com/PX4/PX4-ECL/tree/pr-ekf_init_no_baro.

[lukegluke]

Yes, thanks. But I added also a waiting for _gps_buffer samples same as for baro. Because of not yet cleaned checks:

PX4-ECL/EKF/estimator_interface.cpp

Lines 189 to 190 in df7f261

	// TODO: remove checks that are not timing related
	if (((gps.time_usec - _time_last_gps) > _min_obs_interval_us) && need_gps && gps.fix_type > 2) {

gps data in buffer at least 3d :)

[dagar]

Yes, we should do this for all the cases as Paul described in #931 (comment).

@lukegluke do you have a flight log testing the GPS change with no baro?

[lukegluke]

@lukegluke do you have a flight log testing the GPS change with no baro?

I don't fly just driving on ground :). If you want I can send by email links to logs I mentioned in #947 (comment), but I suppose it would not be very useful for you, they are recorded with older px4 and has modifications like for example lat, lon are double in deg, alt - float in m.

[dagar]

for example lat, lon are double in deg, alt - float in m.

Is this for a particular gps unit? I wouldn't mind making that change upstream.

[lukegluke]

@dagar I've already suggested this here: PX4/PX4-Autopilot#14447 (comment) and tried to note possible drawbacks and tasks to do. In our custom project we moved to doubles quite ago and working with general NMEA driver PX4/PX4-GPSDrivers#58, but it is not a big deal to correct all other drivers.
But we are using just a part of px4 framework and only for rovers so can't be responsible for all boards and can't correct or make tests, also as I understand QGC uses the same gps driver repository, so it also should be corrected simultaneously by someone.

I could try to make a separate patch when I have time if you need it as reference point.

[priseborough]

If GPS is selected as the primary height source and it stops and there is baro data, it will continue to try and reset to GPS. Not sure what you expect EKF to do if the only source of height data stops given we do not have a no height mode available. We would need to add support for a no height mode or switch to using a 'fake' height measurement and set z and velz validity to false in the local position and global position message to stop control loops trying to use the data.

[lukegluke]

Not sure what you expect EKF to do if the only source of height data stops given we do not have a no height mode available.

I would expect that EKF without baro data try to reset to GPS if adequate GPS data is available back again.

[lalawinter]

I want to use gps, imu, barometer and magnetometer to estimate the position、speed and yaw of a drone, but I don't know how to use this library