AP_InertialNav: add fallback numeric vert velocity

[robertlong13]

This PR adds the ability for AP_InertialNav to fall back to a filtered numeric derivative of altitude when measuring vertical velocity whenever the AHRS refuses a request for velocities. This prevents the vertical velocity from simply freezing when GPS is lost on Plane.

Related to this issue: https://discuss.ardupilot.org/t/near-crash-in-qhover-after-gps-loss/107723/3

I also intend to make another PR to prevent DCM fallback (like Copter does) when hovering. But wanted to start with this one first.

This does cause some noise to feed into the altitude control, but this is vastly superior to the current behavior.

Download Before/After Logs

I am also planning to test this on a physical aircraft this week.

[IamPete1]

We do have a derivative filter class that does the derivative and the filtering.

On the whole I don't think this going to work well in all cases. This changes the feedback path for the altitude control loops. So we may find that there is much more noise leading to a velocity oscillation or much more phase lag leading to a position oscillation.

That the issue that this is trying to fix is not present on copter suggests this change is not really necessary.

I think we should try the other fix first and only come back to this if we find its absolutely required.

[robertlong13]

I have vastly simplified this PR based on recommendations from Tridge.

Now, when 3D velocity is not available (or during high vibration as previously), it calls get_vert_pos_rate_D, and I added baro climbrate to DCM's implementation of get_vert_pos_rate_D

[IamPete1]

This means you could be updating X and Y from a bad ekf estimate. You should only update the Z component.

[tridge]

we could zero the xy components

[IamPete1]

I don't think zeroing is a good idea, it will result in agresive flying, it would be very bad if we get intermittent values.

We could decay to zero I suppose.

Ultimately i think we just need to stop using X and Y if they go bad.

[robertlong13]

This means you could be updating X and Y from a bad ekf estimate. You should only update the Z component.

Nothing is changed in how this handles EKF velocities. The EKF will always cause velned_ok to be true.

ardupilot/libraries/AP_AHRS/AP_AHRS.cpp

Lines 3452 to 3458 in b867747

	// return a ground velocity in meters/second, North/East/Down
	// order. Must only be called if have_inertial_nav() is true
	bool AP_AHRS::get_velocity_NED(Vector3f &vec) const
	{
	vec = state.velocity_NED;
	return state.velocity_NED_ok;
	}

ardupilot/libraries/AP_AHRS/AP_AHRS.cpp

Line 345 in b867747

state.velocity_NED_ok = _get_velocity_NED(state.velocity_NED);

ardupilot/libraries/AP_AHRS/AP_AHRS.cpp

Lines 1417 to 1427 in 422d7ce

	#if HAL_NAVEKF2_AVAILABLE
	case EKFType::TWO:
	EKF2.getVelNED(vec);
	return true;
	#endif
	#if HAL_NAVEKF3_AVAILABLE
	case EKFType::THREE:
	EKF3.getVelNED(vec);
	return true;
	#endif

This current behavior, both in master and this PR, is that the x and y components will be stuck at whatever the last good update was to the AP_AHRS::state.velocity_NED variable, and I think that's mostly fine. Position hold hovers are going to do bad things. We can make those modes do less-bad things, but that's another PR.

[IamPete1]

Sure, but this relies on the AHRS behaving in a certain way. We might change that in the future and then hit this untested code path. For example we might stop filling in the vector if returning false which would be seemingly fine because no-one should be using a invalid estimate, then we hit this odd case.

Better to have a function which is obliviously the same without having to look into outside libs.

Something like:

    // get the velocity relative to the local earth frame
    Vector3f velNED;
    const bool velned_ok = _ahrs_ekf.get_velocity_NED(velNED);
    if (velned_ok) {
        _velocity_cm = velNED * 100.0; // convert to cm/s
        _velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU
    }
    float rate_z;
    if ((!velned_ok || high_vibes) && _ahrs_ekf.get_vert_pos_rate_D(rate_z)) {
        _velocity_cm.z = rate_z * -100.0;
    }

[robertlong13]

I have folded in Peter's suggestions.

I have also added some handling for the VFR_HUD climb rate so that this fallback behavior can be reported to the GCS. Which was necessary for the autotest that I created.

Not sure if we really want the autotest but it's there.

[IamPete1]

I think this looks good.

[peterbarker]

Would be better to sense the change in state here rather than a simple delay. These types of sleeps lead to race conditions.