diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 851556ebc7..eb67d7e15d 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -130,16 +130,16 @@ void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) { Vector3d nans = Vector3d(NAN, NAN, NAN); // write measurements to msg - init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, true); - init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, true); - init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, true); - init_measurement(fix.initVelocityNED(), ned_vel, nans, true); + init_measurement(fix.initPositionGeodetic(), fix_pos_geo_vec, nans, this->last_gps_fix > 0); + init_measurement(fix.initPositionECEF(), fix_ecef, fix_ecef_std, this->last_gps_fix > 0); + init_measurement(fix.initVelocityECEF(), vel_ecef, vel_ecef_std, this->last_gps_fix > 0); + init_measurement(fix.initVelocityNED(), ned_vel, nans, this->last_gps_fix > 0); init_measurement(fix.initVelocityDevice(), vel_device, vel_device_std, true); init_measurement(fix.initAccelerationDevice(), accDevice, accDeviceErr, true); - init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, true); - init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated); - init_measurement(fix.initOrientationNED(), orientation_ned, nans, true); - init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, true); + init_measurement(fix.initOrientationECEF(), orientation_ecef, orientation_ecef_std, this->last_gps_fix > 0); + init_measurement(fix.initCalibratedOrientationECEF(), calibrated_orientation_ecef, nans, this->calibrated && this->last_gps_fix > 0); + init_measurement(fix.initOrientationNED(), orientation_ned, nans, this->last_gps_fix > 0); + init_measurement(fix.initCalibratedOrientationNED(), calibrated_orientation_ned, nans, this->calibrated && this->last_gps_fix > 0); init_measurement(fix.initAngularVelocityDevice(), angVelocityDevice, angVelocityDeviceErr, true); init_measurement(fix.initVelocityCalibrated(), vel_calib, vel_calib_std, this->calibrated); init_measurement(fix.initAngularVelocityCalibrated(), ang_vel_calib, ang_vel_calib_std, this->calibrated); diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index a0c89e18b2..fd88ecaea3 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7480befa230cb020e5ddd3b4b86e2e8a589cde59 \ No newline at end of file +d2b1507589f05d0600fc19ec2570d831fe210ad5 \ No newline at end of file