@ -20,6 +20,11 @@ const double VALID_POS_STD = 50.0; // m
const double MAX_RESET_TRACKER = 5.0 ;
const double SANE_GPS_UNCERTAINTY = 1500.0 ; // m
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_LOCATION_SENSOR_TIME_OFFSET = 0.630 ; // s
const double GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET = 0.095 ; // s
static VectorXd floatlist2vector ( const capnp : : List < float , capnp : : Kind : : PRIMITIVE > : : Reader & floatlist ) {
VectorXd res ( floatlist . size ( ) ) ;
for ( int i = 0 ; i < floatlist . size ( ) ; i + + ) {
@ -257,7 +262,7 @@ void Localizer::input_fake_gps_observations(double current_time) {
this - > kf - > predict_and_observe ( current_time , OBSERVATION_ECEF_VEL , { ecef_vel } , { ecef_vel_R } ) ;
}
void Localizer : : handle_gps ( double current_time , const cereal : : GpsLocationData : : Reader & log ) {
void Localizer : : handle_gps ( double current_time , const cereal : : GpsLocationData : : Reader & log , const double sensor_time_offset ) {
// ignore the message if the fix is invalid
bool gps_invalid_flag = ( log . getFlags ( ) % 2 = = 0 ) ;
bool gps_unreasonable = ( Vector2d ( log . getAccuracy ( ) , log . getVerticalAccuracy ( ) ) . norm ( ) > = SANE_GPS_UNCERTAINTY ) ;
@ -265,13 +270,15 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
bool gps_lat_lng_alt_insane = ( ( std : : abs ( log . getLatitude ( ) ) > 90 ) | | ( std : : abs ( log . getLongitude ( ) ) > 180 ) | | ( std : : abs ( log . getAltitude ( ) ) > ALTITUDE_SANITY_CHECK ) ) ;
bool gps_vel_insane = ( floatlist2vector ( log . getVNED ( ) ) . norm ( ) > TRANS_SANITY_CHECK ) ;
if ( gps_invalid_flag | | gps_unreasonable | | gps_accuracy_insane | | gps_lat_lng_alt_insane | | gps_vel_insane ) {
if ( gps_invalid_flag | | gps_unreasonable | | gps_accuracy_insane | | gps_lat_lng_alt_insane | | gps_vel_insane ) {
this - > determine_gps_mode ( current_time ) ;
return ;
}
double sensor_time = current_time - sensor_time_offset ;
// Process message
this - > last_gps_fix = current _time;
this - > last_gps_fix = sensor _time;
this - > gps_mode = true ;
Geodetic geodetic = { log . getLatitude ( ) , log . getLongitude ( ) , log . getAltitude ( ) } ;
this - > converter = std : : make_unique < LocalCoord > ( geodetic ) ;
@ -300,14 +307,14 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
if ( ecef_vel . norm ( ) > 5.0 & & orientation_error . norm ( ) > 1.0 ) {
LOGE ( " Locationd vs ubloxLocation orientation difference too large, kalman reset " ) ;
this - > reset_kalman ( NAN , initial_pose_ecef_quat , ecef_pos , ecef_vel , ecef_pos_R , ecef_vel_R ) ;
this - > kf - > predict_and_observe ( current _time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS , { initial_pose_ecef_quat } ) ;
this - > kf - > predict_and_observe ( sensor _time, OBSERVATION_ECEF_ORIENTATION_FROM_GPS , { initial_pose_ecef_quat } ) ;
} else if ( gps_est_error > 100.0 ) {
LOGE ( " Locationd vs ubloxLocation position difference too large, kalman reset " ) ;
this - > reset_kalman ( NAN , initial_pose_ecef_quat , ecef_pos , ecef_vel , ecef_pos_R , ecef_vel_R ) ;
}
this - > kf - > predict_and_observe ( current _time, OBSERVATION_ECEF_POS , { ecef_pos } , { ecef_pos_R } ) ;
this - > kf - > predict_and_observe ( current _time, OBSERVATION_ECEF_VEL , { ecef_vel } , { ecef_vel_R } ) ;
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 } ) ;
}
void Localizer : : handle_car_state ( double current_time , const cereal : : CarState : : Reader & log ) {
@ -443,9 +450,9 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
} else if ( log . isGyroscope ( ) ) {
this - > handle_sensor ( t , log . getGyroscope ( ) ) ;
} else if ( log . isGpsLocation ( ) ) {
this - > handle_gps ( t , log . getGpsLocation ( ) ) ;
this - > handle_gps ( t , log . getGpsLocation ( ) , GPS_LOCATION_SENSOR_TIME_OFFSET ) ;
} else if ( log . isGpsLocationExternal ( ) ) {
this - > handle_gps ( t , log . getGpsLocationExternal ( ) ) ;
this - > handle_gps ( t , log . getGpsLocationExternal ( ) , GPS_LOCATION_EXTERNAL_SENSOR_TIME_OFFSET ) ;
} else if ( log . isCarState ( ) ) {
this - > handle_car_state ( t , log . getCarState ( ) ) ;
} else if ( log . isCameraOdometry ( ) ) {
@ -497,9 +504,8 @@ int Localizer::locationd_thread() {
} else {
gps_location_socket = " gpsLocation " ;
}
const std : : initializer_list < const char * > service_list = { gps_location_socket ,
" cameraOdometry " , " liveCalibration " , " carState " , " carParams " ,
" accelerometer " , " gyroscope " } ;
const std : : initializer_list < const char * > service_list = { gps_location_socket , " cameraOdometry " , " liveCalibration " ,
" carState " , " carParams " , " accelerometer " , " gyroscope " } ;
PubMaster pm ( { " liveLocationKalman " } ) ;
// TODO: remove carParams once we're always sending at 100Hz