sadmen
/
openpilot_comma
mirror of https://github.com/commaai/openpilot.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Revert: Locationd: enable laikad (#29687)

Browse Source 
* Revert: Locationd: enable laikad

* update ref
pull/29698/head
Harald Schäfer 2 years ago committed by GitHub
parent 5aed2e01e8
commit 55b391509d
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
6 changed files with 155 additions and 32 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      selfdrive/locationd/liblocationd.cc
  2. 129
      selfdrive/locationd/locationd.cc
  3. 13
      selfdrive/locationd/locationd.h
  4. 29
      selfdrive/locationd/test/test_locationd.py
  5. 12
      selfdrive/test/process_replay/process_replay.py
  6. 2
      selfdrive/test/process_replay/ref_commit

2
selfdrive/locationd/liblocationd.cc
Unescape View File

@ -4,7 +4,7 @@ extern "C" {
typedef Localizer* Localizer_t;
Localizer *localizer_init(bool has_ublox) {
return new Localizer();
return new Localizer(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM);
}
void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,

129
selfdrive/locationd/locationd.cc
Unescape View File

@ -15,6 +15,7 @@ const double ACCEL_SANITY_CHECK = 100.0; // m/s^2
const double ROTATION_SANITY_CHECK = 10.0; // rad/s
const double TRANS_SANITY_CHECK = 200.0; // m/s
const double CALIB_RPY_SANITY_CHECK = 0.5; // rad (+- 30 deg)
const double ALTITUDE_SANITY_CHECK = 10000; // m
const double MIN_STD_SANITY_CHECK = 1e-5; // m or rad
const double VALID_TIME_SINCE_RESET = 1.0; // s
const double VALID_POS_STD = 50.0; // m
@ -24,11 +25,15 @@ const double INPUT_INVALID_THRESHOLD = 5.0; // same as reset tracker
const double DECAY = 0.99995; // same as reset tracker
const double MAX_FILTER_REWIND_TIME = 0.8; // s
// TODO: GPS sensor time offsets are empirically calculated
// They should be replaced with synced time from a real clock
const double GPS_QUECTEL_SENSOR_TIME_OFFSET = 0.630; // s
const double GPS_UBLOX_SENSOR_TIME_OFFSET = 0.095; // s
const float GPS_POS_STD_THRESHOLD = 50.0;
const float GPS_VEL_STD_THRESHOLD = 5.0;
const float GPS_POS_ERROR_RESET_THRESHOLD = 300.0;
const float GPS_POS_STD_RESET_THRESHOLD = 10.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 1.0;
const float GPS_POS_STD_RESET_THRESHOLD = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
@ -67,7 +72,7 @@ static VectorXd rotate_std(const MatrixXdr& rot_matrix, const VectorXd& std_in)
return rotate_cov(rot_matrix, std_in.array().square().matrix().asDiagonal()).diagonal().array().sqrt();
}
Localizer::Localizer() {
Localizer::Localizer(LocalizerGnssSource gnss_source) {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
@ -81,6 +86,7 @@ Localizer::Localizer() {
VectorXd ecef_pos = this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START);
this->converter = std::make_unique<LocalCoord>((ECEF) { .x = ecef_pos[0], .y = ecef_pos[1], .z = ecef_pos[2] });
this->configure_gnss_source(gnss_source);
}
void Localizer::build_live_location(cereal::LiveLocationKalman::Builder& fix) {
@ -293,6 +299,64 @@ 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, 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);
bool gps_accuracy_insane = ((log.getVerticalAccuracy() <= 0) || (log.getSpeedAccuracy() <= 0) || (log.getBearingAccuracyDeg() <= 0));
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) {
//this->gps_valid = false;
this->determine_gps_mode(current_time);
return;
}
double sensor_time = current_time - sensor_time_offset;
// Process message
//this->gps_valid = true;
this->gps_mode = true;
Geodetic geodetic = { log.getLatitude(), log.getLongitude(), log.getAltitude() };
this->converter = std::make_unique<LocalCoord>(geodetic);
VectorXd ecef_pos = this->converter->ned2ecef({ 0.0, 0.0, 0.0 }).to_vector();
VectorXd ecef_vel = this->converter->ned2ecef({ log.getVNED()[0], log.getVNED()[1], log.getVNED()[2] }).to_vector() - ecef_pos;
float ecef_pos_std = std::sqrt(this->gps_variance_factor * std::pow(log.getAccuracy(), 2) + this->gps_vertical_variance_factor * std::pow(log.getVerticalAccuracy(), 2));
MatrixXdr ecef_pos_R = Vector3d::Constant(std::pow(this->gps_std_factor * ecef_pos_std, 2)).asDiagonal();
MatrixXdr ecef_vel_R = Vector3d::Constant(std::pow(this->gps_std_factor * log.getSpeedAccuracy(), 2)).asDiagonal();
this->unix_timestamp_millis = log.getUnixTimestampMillis();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
VectorXd orientation_ecef = quat2euler(vector2quat(this->kf->get_x().segment<STATE_ECEF_ORIENTATION_LEN>(STATE_ECEF_ORIENTATION_START)));
VectorXd orientation_ned = ned_euler_from_ecef({ ecef_pos(0), ecef_pos(1), ecef_pos(2) }, orientation_ecef);
VectorXd orientation_ned_gps = Vector3d(0.0, 0.0, DEG2RAD(log.getBearingDeg()));
VectorXd orientation_error = (orientation_ned - orientation_ned_gps).array() - M_PI;
for (int i = 0; i < orientation_error.size(); i++) {
orientation_error(i) = std::fmod(orientation_error(i), 2.0 * M_PI);
if (orientation_error(i) < 0.0) {
orientation_error(i) += 2.0 * M_PI;
}
orientation_error(i) -= M_PI;
}
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_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(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->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 });
}
void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log) {
if (!log.getPositionECEF().getValid() || !log.getVelocityECEF().getValid()) {
@ -301,22 +365,21 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
}
double sensor_time = log.getMeasTime() * 1e-9;
sensor_time -= this->gps_time_offset;
auto ecef_pos_v = log.getPositionECEF().getValue();
VectorXd ecef_pos = Vector3d(ecef_pos_v[0], ecef_pos_v[1], ecef_pos_v[2]);
auto ecef_pos_std = log.getPositionECEF().getStd();
VectorXd ecef_pos_var = Vector3d(ecef_pos_std[0], ecef_pos_std[1], ecef_pos_std[2]).array().square().matrix();
double ecef_pos_std_norm = std::sqrt(ecef_pos_var.sum());
MatrixXdr ecef_pos_R = ecef_pos_var.asDiagonal();
// indexed at 0 cause all std values are the same MAE
auto ecef_pos_std = log.getPositionECEF().getStd()[0];
MatrixXdr ecef_pos_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_pos_std, 2)).asDiagonal();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
auto ecef_vel_std = log.getVelocityECEF().getStd();
VectorXd ecef_vel_var = Vector3d(ecef_vel_std[0], ecef_vel_std[1], ecef_vel_std[2]).array().square().matrix();
double ecef_vel_std_norm = std::sqrt(ecef_vel_var.sum());
MatrixXdr ecef_vel_R = ecef_vel_var.asDiagonal();
// indexed at 0 cause all std values are the same MAE
auto ecef_vel_std = log.getVelocityECEF().getStd()[0];
MatrixXdr ecef_vel_R = Vector3d::Constant(pow(this->gps_std_factor*ecef_vel_std, 2)).asDiagonal();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@ -339,13 +402,13 @@ 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)));
if (ecef_pos_std_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) {
if (ecef_pos_std > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD;
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
@ -354,7 +417,7 @@ void Localizer::handle_gnss(double current_time, const cereal::GnssMeasurements:
this->orientation_reset_count = 0;
}
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std_norm < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
// always reset on first gps message and if the location is off but the accuracy is high
LOGE("Locationd vs gnssMeasurement position difference too large, kalman reset");
this->reset_kalman(NAN, initial_pose_ecef_quat, ecef_pos, ecef_vel, ecef_pos_R, ecef_vel_R);
@ -524,8 +587,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.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()) {
@ -586,12 +653,38 @@ void Localizer::determine_gps_mode(double current_time) {
}
}
void Localizer::configure_gnss_source(const LocalizerGnssSource &source) {
this->gnss_source = source;
if (source == LocalizerGnssSource::UBLOX) {
this->gps_std_factor = 10.0;
this->gps_variance_factor = 1.0;
this->gps_vertical_variance_factor = 1.0;
this->gps_time_offset = GPS_UBLOX_SENSOR_TIME_OFFSET;
} else {
this->gps_std_factor = 2.0;
this->gps_variance_factor = 0.0;
this->gps_vertical_variance_factor = 3.0;
this->gps_time_offset = GPS_QUECTEL_SENSOR_TIME_OFFSET;
}
}
int Localizer::locationd_thread() {
const std::initializer_list<const char *> service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration",
LocalizerGnssSource source;
const char* gps_location_socket;
if (Params().getBool("UbloxAvailable")) {
source = LocalizerGnssSource::UBLOX;
gps_location_socket = "gpsLocationExternal";
} else {
source = LocalizerGnssSource::QCOM;
gps_location_socket = "gpsLocation";
}
this->configure_gnss_source(source);
const std::initializer_list<const char *> service_list = {gps_location_socket, "cameraOdometry", "liveCalibration",
"carState", "carParams", "accelerometer", "gyroscope"};
// TODO: remove carParams once we're always sending at 100Hz
SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"});
SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;

13
selfdrive/locationd/locationd.h
Unescape View File

@ -22,10 +22,13 @@
#define POSENET_STD_HIST_HALF 20
enum LocalizerGnssSource {
UBLOX, QCOM
};
class Localizer {
public:
Localizer();
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
int locationd_thread();
@ -53,6 +56,7 @@ public:
void handle_msg_bytes(const char *data, const size_t size);
void handle_msg(const cereal::Event::Reader& log);
void handle_sensor(double current_time, const cereal::SensorEventData::Reader& log);
void handle_gps(double current_time, const cereal::GpsLocationData::Reader& log, const double sensor_time_offset);
void handle_gnss(double current_time, const cereal::GnssMeasurements::Reader& log);
void handle_car_state(double current_time, const cereal::CarState::Reader& log);
void handle_cam_odo(double current_time, const cereal::CameraOdometry::Reader& log);
@ -81,8 +85,15 @@ private:
double first_valid_log_time = NAN;
double ttff = NAN;
double last_gps_msg = 0;
LocalizerGnssSource gnss_source;
bool observation_timings_invalid = false;
std::map<std::string, double> observation_values_invalid;
bool standstill = true;
int32_t orientation_reset_count = 0;
float gps_std_factor;
float gps_variance_factor;
float gps_vertical_variance_factor;
double gps_time_offset;
void configure_gnss_source(const LocalizerGnssSource &source);
};

29
selfdrive/locationd/test/test_locationd.py
Unescape View File

@ -14,7 +14,7 @@ from openpilot.selfdrive.manager.process_config import managed_processes
class TestLocationdProc(unittest.TestCase):
LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
LLD_MSGS = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
'accelerometer', 'gyroscope', 'magnetometer']
def setUp(self):
@ -35,14 +35,25 @@ class TestLocationdProc(unittest.TestCase):
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
if name == "gnssMeasurements":
msg.gnssMeasurements.measTime = t
msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
msg.gnssMeasurements.positionECEF.std = [0,0,0]
msg.gnssMeasurements.positionECEF.valid = True
msg.gnssMeasurements.velocityECEF.value = []
msg.gnssMeasurements.velocityECEF.std = [0,0,0]
msg.gnssMeasurements.velocityECEF.valid = True
if name == "gpsLocationExternal":
msg.gpsLocationExternal.flags = 1
msg.gpsLocationExternal.verticalAccuracy = 1.0
msg.gpsLocationExternal.speedAccuracy = 1.0
msg.gpsLocationExternal.bearingAccuracyDeg = 1.0
msg.gpsLocationExternal.vNED = [0.0, 0.0, 0.0]
msg.gpsLocationExternal.latitude = float(self.lat)
msg.gpsLocationExternal.longitude = float(self.lon)
msg.gpsLocationExternal.unixTimestampMillis = t * 1e6
msg.gpsLocationExternal.altitude = float(self.alt)
#if name == "gnssMeasurements":
# msg.gnssMeasurements.measTime = t
# msg.gnssMeasurements.positionECEF.value = [self.x , self.y, self.z]
# msg.gnssMeasurements.positionECEF.std = [0,0,0]
# msg.gnssMeasurements.positionECEF.valid = True
# msg.gnssMeasurements.velocityECEF.value = []
# msg.gnssMeasurements.velocityECEF.std = [0,0,0]
# msg.gnssMeasurements.velocityECEF.valid = True
elif name == 'cameraOdometry':
msg.cameraOdometry.rot = [0.0, 0.0, 0.0]
msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0]

12
selfdrive/test/process_replay/process_replay.py
Unescape View File

@ -433,6 +433,13 @@ def laikad_config_pubsub_callback(params, cfg, lr):
cfg.main_pub_drained = True
def locationd_config_pubsub_callback(params, cfg, lr):
ublox = params.get_bool("UbloxAvailable")
sub_keys = ({"gpsLocation", } if ublox else {"gpsLocationExternal", })
cfg.pubs = set(cfg.pubs) - sub_keys
CONFIGS = [
ProcessConfig(
proc_name="controlsd",
@ -487,11 +494,12 @@ CONFIGS = [
ProcessConfig(
proc_name="locationd",
pubs=[
"cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements",
"liveCalibration", "carState", "carParams"
"cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
"liveCalibration", "carState", "carParams", "gpsLocation"
],
subs=["liveLocationKalman"],
ignore=["logMonoTime", "valid"],
config_callback=locationd_config_pubsub_callback,
tolerance=NUMPY_TOLERANCE,
),
ProcessConfig(

2
selfdrive/test/process_replay/ref_commit
Unescape View File

@ -1 +1 @@
3fd85c41e61044a86bf364b9a7a0470e5758f231
20022e4a1b5f5df343465180a3865b0c78890544
Write Preview
Loading…
Cancel
Save