remove gps_valid flag

pull/26850/head
Kurt Nistelberger 3 years ago
parent d9d313d26d
commit 24003c1c02
  1. 10
      selfdrive/locationd/locationd.cc
  2. 1
      selfdrive/locationd/locationd.h

@ -310,7 +310,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
} }
if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) { if (gps_invalid_flag || gps_unreasonable || gps_accuracy_insane || gps_lat_lng_alt_insane || gps_vel_insane || gps_accuracy_insane_quectel) {
this->gps_valid = false; //this->gps_valid = false;
this->determine_gps_mode(current_time); this->determine_gps_mode(current_time);
return; return;
} }
@ -318,7 +318,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
double sensor_time = current_time - sensor_time_offset; double sensor_time = current_time - sensor_time_offset;
// Process message // Process message
this->gps_valid = true; //this->gps_valid = true;
this->gps_mode = true; this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() }; Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic); this->converter = std::make_unique<LocalCoord>(geodetic);
@ -359,8 +359,7 @@ void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::R
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) { void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
this->gps_valid = log.getPositionECEF().getValid() && log.getVelocityECEF().getValid(); if(!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
if (!this->gps_valid) {
this->determine_gps_mode(current_time); this->determine_gps_mode(current_time);
return; return;
} }
@ -408,7 +407,6 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps))); VectorXd initial_pose_ecef_quat = quat2vector(euler2quat(ecef_euler_from_ned({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ned_gps)));
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) { if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->gps_valid = false;
this->determine_gps_mode(current_time); this->determine_gps_mode(current_time);
return; return;
} }
@ -606,7 +604,7 @@ kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_build
} }
bool Localizer::is_gps_ok() { bool Localizer::is_gps_ok() {
return this->gps_valid && (this->kf->get_filter_time() - this->last_gps_msg) < 1.0; return (this->kf->get_filter_time() - this->last_gps_msg) < 1.0;
} }
bool Localizer::critical_services_valid(std::map<std::string, double> critical_services) { bool Localizer::critical_services_valid(std::map<std::string, double> critical_services) {

@ -78,7 +78,6 @@ private:
double reset_tracker = 0.0; double reset_tracker = 0.0;
bool device_fell = false; bool device_fell = false;
bool gps_mode = false; bool gps_mode = false;
bool gps_valid = false;
double last_gps_msg = 0; double last_gps_msg = 0;
bool ublox_available = true; bool ublox_available = true;
bool observation_timings_invalid = false; bool observation_timings_invalid = false;

Loading…
Cancel
Save