|
|
|
@ -354,7 +354,6 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R |
|
|
|
|
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R); |
|
|
|
|
} |
|
|
|
|
|
|
|
|
|
this->last_gps_msg = sensor_time; |
|
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_POS, { ecef_pos }, { ecef_pos_R }); |
|
|
|
|
this->kf->predict_and_observe(sensor_time, OBSERVATION_ECEF_VEL, { ecef_vel }, { ecef_vel_R }); |
|
|
|
|
} |
|
|
|
@ -589,12 +588,12 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) { |
|
|
|
|
this->handle_sensor(t, log.getAccelerometer()); |
|
|
|
|
} else if (log.isGyroscope()) { |
|
|
|
|
this->handle_sensor(t, log.getGyroscope()); |
|
|
|
|
} else if (log.isGpsLocation()) { |
|
|
|
|
this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET); |
|
|
|
|
} else if (log.isGpsLocationExternal()) { |
|
|
|
|
this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET); |
|
|
|
|
//} else if (log.isGnssMeasurements()) {
|
|
|
|
|
// this->handle_gnss(t, log.getGnssMeasurements());
|
|
|
|
|
//} else if (log.isGpsLocation()) {
|
|
|
|
|
// this->handle_gps(t, log.getGpsLocation(), GPS_QUECTEL_SENSOR_TIME_OFFSET);
|
|
|
|
|
//} else if (log.isGpsLocationExternal()) {
|
|
|
|
|
// this->handle_gps(t, log.getGpsLocationExternal(), GPS_UBLOX_SENSOR_TIME_OFFSET);
|
|
|
|
|
} else if (log.isGnssMeasurements()) { |
|
|
|
|
this->handle_gnss(t, log.getGnssMeasurements()); |
|
|
|
|
} else if (log.isCarState()) { |
|
|
|
|
this->handle_car_state(t, log.getCarState()); |
|
|
|
|
} else if (log.isCameraOdometry()) { |
|
|
|
|