AP_NavEKF3: use GPS yaw if available and enabled

this allows a suitable GPS to be used as an external yaw source
This commit is contained in:
Andrew Tridgell 2017-11-11 14:29:48 +11:00
parent 0c4f92d4c1
commit b56914b879
1 changed files with 6 additions and 0 deletions

View File

@ -616,6 +616,12 @@ void NavEKF3_core::readGpsData()
frontend->logging.log_gps = true; frontend->logging.log_gps = true;
// if the GPS has yaw data then input that as well
float yaw_deg, yaw_accuracy_deg;
if (AP::gps().gps_yaw_deg(yaw_deg, yaw_accuracy_deg)) {
writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), gpsDataNew.time_ms, 2);
}
} else { } else {
// report GPS fix status // report GPS fix status
gpsCheckStatus.bad_fix = true; gpsCheckStatus.bad_fix = true;