diff --git a/selfdrive/controls/controlsd.py b/selfdrive/controls/controlsd.py index 8094030731..bb135071f2 100755 --- a/selfdrive/controls/controlsd.py +++ b/selfdrive/controls/controlsd.py @@ -381,6 +381,8 @@ class Controls: if not (self.sm['liveParameters'].sensorValid or self.sm['liveLocationKalman'].sensorsOK) and not NOSENSOR: if self.sm.frame > 5 / DT_CTRL: # Give locationd some time to receive all the inputs self.events.add(EventName.sensorDataInvalid) + if not self.sm['liveLocationKalman'].inputsOK and not NOSENSOR: + self.events.add(EventName.localizerMalfunction) if not self.sm['liveLocationKalman'].posenetOK: self.events.add(EventName.posenetInvalid) if not self.sm['liveLocationKalman'].deviceStable: @@ -420,8 +422,6 @@ class Controls: if self.sm['modelV2'].frameDropPerc > 20: self.events.add(EventName.modeldLagging) - if self.sm['liveLocationKalman'].excessiveResets: - self.events.add(EventName.localizerMalfunction) def data_sample(self): """Receive data from sockets and update carState""" diff --git a/selfdrive/controls/lib/events.py b/selfdrive/controls/lib/events.py index 1095c880cb..6f41224475 100755 --- a/selfdrive/controls/lib/events.py +++ b/selfdrive/controls/lib/events.py @@ -576,11 +576,9 @@ EVENTS: Dict[int, Dict[str, Union[Alert, AlertCallbackType]]] = { ET.PERMANENT: NormalPermanentAlert("GPS Malfunction", "Likely Hardware Issue"), }, - # When the GPS position and localizer diverge the localizer is reset to the - # current GPS position. This alert is thrown when the localizer is reset - # more often than expected. EventName.localizerMalfunction: { - # ET.PERMANENT: NormalPermanentAlert("Sensor Malfunction", "Hardware Malfunction"), + ET.NO_ENTRY: NoEntryAlert("Localizer Malfunction"), + ET.SOFT_DISABLE: soft_disable_alert("Localizer Malfunction"), }, # ********** events that affect controls state transitions ********** diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index 401de0cfdd..600ba46853 100644 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -21,9 +21,11 @@ const double VALID_TIME_SINCE_RESET = 1.0; // s const double VALID_POS_STD = 50.0; // m const double MAX_RESET_TRACKER = 5.0; const double SANE_GPS_UNCERTAINTY = 1500.0; // m -const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker -const double DECAY = 0.99995; // same as reset tracker +const double INPUT_INVALID_THRESHOLD = 0.5; // same as reset tracker +const double RESET_TRACKER_DECAY = 0.99995; +const double DECAY = 0.9993; // ~10 secs to resume after a bad input const double MAX_FILTER_REWIND_TIME = 0.8; // s +const double YAWRATE_CROSS_ERR_CHECK_FACTOR = 30; // TODO: GPS sensor time offsets are empirically calculated // They should be replaced with synced time from a real clock @@ -256,7 +258,13 @@ void Localizer::handle_sensor(double current_time, const cereal::SensorEventData if (log.getSensor() == SENSOR_GYRO_UNCALIBRATED && log.getType() == SENSOR_TYPE_GYROSCOPE_UNCALIBRATED) { auto v = log.getGyroUncalibrated().getV(); auto meas = Vector3d(-v[2], -v[1], -v[0]); - if (meas.norm() < ROTATION_SANITY_CHECK) { + + VectorXd gyro_bias = this->kf->get_x().segment(STATE_GYRO_BIAS_START); + float gyro_camodo_yawrate_err = std::abs((meas[2] - gyro_bias[2]) - this->camodo_yawrate_distribution[0]); + float gyro_camodo_yawrate_err_threshold = YAWRATE_CROSS_ERR_CHECK_FACTOR * this->camodo_yawrate_distribution[1]; + bool gyro_valid = gyro_camodo_yawrate_err < gyro_camodo_yawrate_err_threshold; + + if ((meas.norm() < ROTATION_SANITY_CHECK) && gyro_valid) { this->kf->predict_and_observe(sensor_time, OBSERVATION_PHONE_GYRO, { meas }); this->observation_values_invalid["gyroscope"] *= DECAY; } else { @@ -483,6 +491,7 @@ void Localizer::handle_cam_odo(double current_time, const cereal::CameraOdometry this->kf->predict_and_observe(current_time, OBSERVATION_CAMERA_ODO_TRANSLATION, { trans_device }, { trans_device_cov }); this->observation_values_invalid["cameraOdometry"] *= DECAY; + this->camodo_yawrate_distribution = Vector2d(rot_device[2], rotate_std(this->device_from_calib, rot_calib_std)[2]); } void Localizer::handle_live_calib(double current_time, const cereal::LiveCalibrationData::Reader& log) { @@ -538,7 +547,7 @@ void Localizer::time_check(double current_time) { void Localizer::update_reset_tracker() { // reset tracker is tuned to trigger when over 1reset/10s over 2min period if (this->is_gps_ok()) { - this->reset_tracker *= DECAY; + this->reset_tracker *= RESET_TRACKER_DECAY; } else { this->reset_tracker = 0.0; } diff --git a/selfdrive/locationd/locationd.h b/selfdrive/locationd/locationd.h index f3069048cd..47c8bf5627 100644 --- a/selfdrive/locationd/locationd.h +++ b/selfdrive/locationd/locationd.h @@ -94,6 +94,7 @@ private: float gps_variance_factor; float gps_vertical_variance_factor; double gps_time_offset; + Eigen::VectorXd camodo_yawrate_distribution = Eigen::Vector2d(0.0, 10.0); // mean, std void configure_gnss_source(const LocalizerGnssSource &source); }; diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index f5e098910f..9e0f4d7b3d 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -649c512f95d9ad80e7c8c4d852cfa5f468a28fd5 \ No newline at end of file +89e571c56fb6c053177cd28c3e0d688c641b46d5 \ No newline at end of file