Copter: No wind estimate when using EK3_SRC1_YAW = GPS

[TheRealKaufmann]

Bug report

Issue details
I am using a HERE 4 Dual GPS Configuration for Yaw and it works really well. However, when selecting EK3_SRC1_YAW = (GPS or GPS with Compass Fallback), there are no wind estimates shown in the wind_vel and wind_dir messages. Also, there is no wind estimate shown in the Log under VWN and VWE. The EK3_DRAG_BCOEF_* and EK3_DRAG_MCOEF parameters have been configures correctly. When selecting EK3_SRC1_YAW = Compass, the wind estimates are shown and available.

Version
ArduCopter 4.4.4

Platform
[ ] All
[ ] AntennaTracker
[ X ] Copter
[ ] Plane
[ ] Rover
[ ] Submarine

Airframe type
QUAD X

Hardware type
Cube Orange

Logs

[rmackay9]

Thanks for the report, I've added this to the Copter-4.5.0 issues list although I see you said the same issue appears with 4.4.

In any case, I'll try to reproduce the issue in SITL.

[TheRealKaufmann]

@rmackay9 Thank you! Could you please give me a hint where in the code this problem might arise?
I might be able to work it through, if it is only in the EKF. For us, this is a fairly critical thing since we want to take off on a large ship with strong winds soon.

[rmackay9]

This issue can be reproduced in SITL meaning that it is likely caused by a bug in the EKF. The steps to reproduce are below:

First confirm that wind estimates work with all default parameters

1. cd ardupilot/ArduCopter
2. ../Tools/autotest/sim_vehicle.py --map --console
3. param load ../Tools/autotest/default_params/copter-wind-estimates.parm
4. restart SITL
5. arm throttle
6. GUIDED
7. takeoff 20
8. status WIND (to show the direction and speed, note the non-zero values)

Next load the gps-for-yaw parameters and repeat the flight

10. param load ../Tools/autotest/default_params/copter-gps-for-yaw.parm
11. repeat steps 5, 6, 7 and 8 above and note that the WIND direction is always -180 and speed is 0

This bug exists in Copter-4.4 as well.

FYI @tridge, @priseborough

[TheRealKaufmann]

I think the problem arises from canEstimateWind being false in void NavEKF3_core::setWindMagStateLearningMode().
It is negative because finalInflightYawInit = false which traces back to void NavEKF3_core::recordYawReset() which is called mainly from void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order).

void NavEKF3_core::resetQuatStateYawOnly(ftype yaw, ftype yawVariance, rotationOrder order) is called from void NavEKF3_core::setYawFromMag(). This function is usually called from void NavEKF3_core::SelectMagFusion() which correctly exits before execution of void NavEKF3_core::setYawFromMag() since no compass is being used.

My temporary fix would be to set finalInflightYawInit in void NavEKF3_core::SelectMagFusion() when GPS-YAW is correctly initialized:

// Handle case where we are using GPS yaw sensor instead of a magnetomer
    if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == 
    AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
        bool have_fused_gps_yaw = false;
        if (storedYawAng.recall(yawAngDataDelayed,imuDataDelayed.time_ms)) {
            if (tiltAlignComplete && (!yawAlignComplete || yaw_source_reset)) {
                alignYawAngle(yawAngDataDelayed);
                yaw_source_reset = false;
                have_fused_gps_yaw = true;
                lastSynthYawTime_ms = imuSampleTime_ms;
                last_gps_yaw_fuse_ms = imuSampleTime_ms;
            } else if (tiltAlignComplete && yawAlignComplete) {
                have_fused_gps_yaw = fuseEulerYaw(yawFusionMethod::GPS);
                if (have_fused_gps_yaw) {
                    last_gps_yaw_fuse_ms = imuSampleTime_ms;
                    _**finalInflightYawInit = true;**_
                }
            }
            last_gps_yaw_ms = imuSampleTime_ms;
        } else if (tiltAlignComplete && !yawAlignComplete) {

What is your opinion on this @tridge, @priseborough, @rmackay9?

All the best
Tom

[rmackay9]

@TheRealKaufmann,

Thanks very much for the analysis. I've slightly fixed the formatting of your comment and I think you meant "recordYawResetsCompleted" instead of "recordYawReset()".

Yes, surely this is related to the "canEstimateWind" check here.

    const bool canEstimateWind = ((finalInflightYawInit && dragFusionEnabled) || assume_zero_sideslip()) &&
                                 !onGround &&
                                 PV_AidingMode != AID_NONE;

I wonder if maybe it is just a mixup between "finalInflightYawInit" and "yawAlignComplete" which can be seen about 10 lines lower? I don't really understand the difference between these two variables.

I've asked @priseborough his opinion. Perhaps @tridge will have an opinion too

[priseborough]

It should be calling recordYawResetsCompleted() when the EKF yaw is reset to the GPS yaw

I'll submit a PR.

[priseborough]

Ref #27636

[rmackay9]

Fixed in master and we can include it in an upcoming beta, thanks @priseborough