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

Locationd: switch to laikad (#29278)

Browse Source 
* use_laika

* Update test

* fix spacing

* fix proc replay

* small fix

* Try correct std

* cast correctly

* experimental

* simpler

* update reset thresholds

* improve

* Ready for live running

* outlier reject

* update laika

* add fine speeds

* more experiments

* fixes

* all in laika

* more small tweaks

* outlier rejection not needed

* fix conflict

* no more gpslocation

* update test

* refactor to laika

* runs again

* Update ref
pull/29522/head
Harald Schäfer 2 years ago committed by GitHub
parent 0c4b91975b
commit fd3b8a3d75
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
9 changed files with 57 additions and 195 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
      laika_repo
  2. 57
      selfdrive/locationd/laikad.py
  3. 2
      selfdrive/locationd/liblocationd.cc
  4. 129
      selfdrive/locationd/locationd.cc
  5. 13
      selfdrive/locationd/locationd.h
  6. 4
      selfdrive/locationd/test/test_laikad.py
  7. 29
      selfdrive/locationd/test/test_locationd.py
  8. 12
      selfdrive/test/process_replay/process_replay.py
  9. 2
      selfdrive/test/process_replay/ref_commit

2
laika_repo

@ -1 +1 @@
Subproject commit 65901a1e6c7c7940d8c97027d3edaca7d0132253
Subproject commit 6c79c964748ab930bb3699c06025bb2b0565cbea

57
selfdrive/locationd/laikad.py
Unescape View File

@ -18,7 +18,8 @@ from laika.downloader import DownloadFailed
from laika.ephemeris import EphemerisType, GPSEphemeris, GLONASSEphemeris, ephemeris_structs, parse_qcom_ephem
from laika.gps_time import GPSTime
from laika.helpers import ConstellationId, get_sv_id
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox, read_raw_qcom
from laika.raw_gnss import GNSSMeasurement, correct_measurements, process_measurements, read_raw_ublox
from laika.raw_gnss import gps_time_from_qcom_report, get_measurements_from_qcom_reports
from laika.opt import calc_pos_fix, get_posfix_sympy_fun, calc_vel_fix, get_velfix_sympy_func
from openpilot.selfdrive.locationd.models.constants import GENERATED_DIR, ObservationKind
from openpilot.selfdrive.locationd.models.gnss_kf import GNSSKalman
@ -102,9 +103,10 @@ class Laikad:
self.use_qcom = use_qcom
self.first_log_time = None
self.ttff = -1
self.measurement_lag = 0.630 if self.use_qcom else 0.095
# qcom specific stuff
self.qcom_reports_received = 1
self.qcom_reports_received = 4
self.qcom_reports = []
def load_cache(self):
@ -161,47 +163,31 @@ class Laikad:
def get_lsq_fix(self, t, measurements):
if self.last_fix_t is None or abs(self.last_fix_t - t) > 0:
min_measurements = 5 if any(p.constellation_id == ConstellationId.GLONASS for p in measurements) else 4
position_solution, pr_residuals, pos_std = calc_pos_fix(measurements, self.posfix_functions, min_measurements=min_measurements)
if len(position_solution) < 3:
return None
position_estimate = position_solution[:3]
position_std_residual = np.median(np.abs(pr_residuals))
position_std = np.median(np.abs(pos_std))/10
position_std = max(position_std_residual, position_std) * np.ones(3)
position_std = pos_std[:3]
velocity_solution, prr_residuals, vel_std = calc_vel_fix(measurements, position_estimate, self.velfix_function, min_measurements=min_measurements)
if len(velocity_solution) < 3:
return None
velocity_estimate = velocity_solution[:3]
velocity_std_residual = np.median(np.abs(prr_residuals))
velocity_std = np.median(np.abs(vel_std))/10
velocity_std = max(velocity_std, velocity_std_residual) * np.ones(3)
velocity_std = vel_std[:3]
return position_estimate, position_std, velocity_estimate, velocity_std
def gps_time_from_qcom_report(self, gnss_msg):
report = gnss_msg.drMeasurementReport
if report.source == log.QcomGnss.MeasurementSource.gps:
report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
elif report.source == log.QcomGnss.MeasurementSource.sbas:
report_time = GPSTime(report.gpsWeek, report.gpsMilliseconds / 1000.0)
elif report.source == log.QcomGnss.MeasurementSource.glonass:
report_time = GPSTime.from_glonass(report.glonassYear,
report.glonassDay,
report.glonassMilliseconds / 1000.0)
else:
raise NotImplementedError(f'Unknownconstellation {report.source}')
return report_time
def is_good_report(self, gnss_msg):
if gnss_msg.which() == 'drMeasurementReport' and self.use_qcom:
if gnss_msg.which() in ['drMeasurementReport', 'measurementReport'] and self.use_qcom:
# TODO: Understand and use remaining unknown constellations
try:
if gnss_msg.which() == 'drMeasurementReport':
constellation_id = ConstellationId.from_qcom_source(gnss_msg.drMeasurementReport.source)
else:
constellation_id = ConstellationId.from_qcom_source(gnss_msg.measurementReport.source)
good_constellation = constellation_id in [ConstellationId.GPS, ConstellationId.SBAS, ConstellationId.GLONASS]
report_time = self.gps_time_from_qcom_report(gnss_msg)
report_time = gps_time_from_qcom_report(gnss_msg)
except NotImplementedError:
return False
# Garbage timestamps with week > 32767 are sometimes sent by module.
@ -216,21 +202,20 @@ class Laikad:
def read_report(self, gnss_msg):
if self.use_qcom:
# QCOM reports are per constellation, so we need to aggregate them
report = gnss_msg.drMeasurementReport
report_time = self.gps_time_from_qcom_report(gnss_msg)
# Additionally, the pseudoranges are broken in the measurementReports
# and the doppler filteredSpeed is broken in the drMeasurementReports
report_time = gps_time_from_qcom_report(gnss_msg)
if report_time - self.last_report_time > 0:
self.qcom_reports_received = max(1, len(self.qcom_reports))
self.qcom_reports = [report]
self.qcom_reports = [gnss_msg]
else:
self.qcom_reports.append(report)
self.qcom_reports.append(gnss_msg)
self.last_report_time = report_time
new_meas = []
if len(self.qcom_reports) == self.qcom_reports_received:
for report in self.qcom_reports:
new_meas.extend(read_raw_qcom(report))
new_meas = get_measurements_from_qcom_reports(self.qcom_reports)
else:
new_meas = []
else:
report = gnss_msg.measurementReport
self.last_report_time = GPSTime(report.gpsWeek, report.rcvTow)
@ -300,7 +285,7 @@ class Laikad:
def process_gnss_msg(self, gnss_msg, gnss_mono_time: int, block=False):
out_msg = messaging.new_message("gnssMeasurements")
t = gnss_mono_time * 1e-9
msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time}
msg_dict: Dict[str, Any] = {"measTime": gnss_mono_time - int(1e9 * self.measurement_lag)}
if self.first_log_time is None:
self.first_log_time = 1e-9 * gnss_mono_time
if self.is_ephemeris(gnss_msg):

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(has_ublox ? LocalizerGnssSource::UBLOX : LocalizerGnssSource::QCOM);
return new Localizer();
}
void localizer_get_message_bytes(Localizer *localizer, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid,

129
selfdrive/locationd/locationd.cc
Unescape View File

@ -13,7 +13,6 @@ 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
@ -23,15 +22,11 @@ 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 = 2.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 0.5;
const float GPS_POS_STD_RESET_THRESHOLD = 10.0;
const float GPS_VEL_STD_RESET_THRESHOLD = 1.0;
const float GPS_ORIENTATION_ERROR_RESET_THRESHOLD = 1.0;
const int GPS_ORIENTATION_ERROR_RESET_CNT = 3;
@ -70,7 +65,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(LocalizerGnssSource gnss_source) {
Localizer::Localizer() {
this->kf = std::make_unique<LiveKalman>();
this->reset_kalman();
@ -84,7 +79,6 @@ Localizer::Localizer(LocalizerGnssSource gnss_source) {
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) {
@ -300,64 +294,6 @@ 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()) {
@ -366,21 +302,22 @@ 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]);
// 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_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();
auto ecef_vel_v = log.getVelocityECEF().getValue();
VectorXd ecef_vel = Vector3d(ecef_vel_v[0], ecef_vel_v[1], ecef_vel_v[2]);
// 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();
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();
double gps_est_error = (this->kf->get_x().segment<STATE_ECEF_POS_LEN>(STATE_ECEF_POS_START) - ecef_pos).norm();
@ -403,13 +340,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 > GPS_POS_STD_THRESHOLD || ecef_vel_std > GPS_VEL_STD_THRESHOLD) {
if (ecef_pos_std_norm > GPS_POS_STD_THRESHOLD || ecef_vel_std_norm > GPS_VEL_STD_THRESHOLD) {
this->determine_gps_mode(current_time);
return;
}
// prevent jumping gnss measurements (covered lots, standstill...)
bool orientation_reset = ecef_vel_std < GPS_VEL_STD_RESET_THRESHOLD;
bool orientation_reset = ecef_vel_std_norm < GPS_VEL_STD_RESET_THRESHOLD;
orientation_reset &= orientation_error.norm() > GPS_ORIENTATION_ERROR_RESET_THRESHOLD;
orientation_reset &= !this->standstill;
if (orientation_reset) {
@ -419,7 +356,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 < GPS_POS_STD_RESET_THRESHOLD) || this->last_gps_msg == 0) {
if ((gps_est_error > GPS_POS_ERROR_RESET_THRESHOLD && ecef_pos_std_norm < 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);
@ -588,12 +525,8 @@ 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.isGnssMeasurements()) {
this->handle_gnss(t, log.getGnssMeasurements());
} else if (log.isCarState()) {
this->handle_car_state(t, log.getCarState());
} else if (log.isCameraOdometry()) {
@ -655,38 +588,12 @@ void Localizer::determine_gps_mode(double current_time) {
}
}
void Localizer::configure_gnss_source(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() {
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",
const std::initializer_list<const char *> service_list = {"gnssMeasurements", "cameraOdometry", "liveCalibration",
"carState", "carParams", "accelerometer", "gyroscope"};
// TODO: remove carParams once we're always sending at 100Hz
SubMaster sm(service_list, {}, nullptr, {gps_location_socket, "carParams"});
SubMaster sm(service_list, {}, nullptr, {"gnssMeasurements", "carParams"});
PubMaster pm({"liveLocationKalman"});
uint64_t cnt = 0;

13
selfdrive/locationd/locationd.h
Unescape View File

@ -21,13 +21,10 @@
#define POSENET_STD_HIST_HALF 20
enum LocalizerGnssSource {
UBLOX, QCOM
};
class Localizer {
public:
Localizer(LocalizerGnssSource gnss_source = LocalizerGnssSource::UBLOX);
Localizer();
int locationd_thread();
@ -55,7 +52,6 @@ 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);
@ -84,15 +80,8 @@ 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(LocalizerGnssSource source);
};

4
selfdrive/locationd/test/test_laikad.py
Unescape View File

@ -177,8 +177,8 @@ class TestLaikad(unittest.TestCase):
laikad = Laikad(auto_update=True, valid_ephem_types=EphemerisType.NAV, use_qcom=use_qcom)
# Disable fetch_orbits to test NAV only
correct_msgs = verify_messages(logs, laikad)
correct_msgs_expected = 56 if use_qcom else 560
valid_fix_expected = 56 if use_qcom else 560
correct_msgs_expected = 55 if use_qcom else 560
valid_fix_expected = 55 if use_qcom else 560
self.assertEqual(correct_msgs_expected, len(correct_msgs))
self.assertEqual(valid_fix_expected, len([m for m in correct_msgs if m.gnssMeasurements.positionECEF.valid]))

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 = ['gpsLocationExternal', 'cameraOdometry', 'carState', 'liveCalibration',
LLD_MSGS = ['gnssMeasurements', 'cameraOdometry', 'carState', 'liveCalibration',
'accelerometer', 'gyroscope', 'magnetometer']
def setUp(self):
@ -35,25 +35,14 @@ class TestLocationdProc(unittest.TestCase):
msg = messaging.new_message(name)
except capnp.lib.capnp.KjException:
msg = messaging.new_message(name, 0)
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
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,13 +433,6 @@ 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",
@ -494,12 +487,11 @@ CONFIGS = [
ProcessConfig(
proc_name="locationd",
pubs=[
"cameraOdometry", "accelerometer", "gyroscope", "gpsLocationExternal",
"liveCalibration", "carState", "carParams", "gpsLocation"
"cameraOdometry", "accelerometer", "gyroscope", "gnssMeasurements",
"liveCalibration", "carState", "carParams"
],
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 @@
72e3d7b660ee92f5adcc249112cf04c703f4bf9e
89db4d10a4eb0bfd0ea0ff83525e5cf358f5f690
Write Preview
Loading…
Cancel
Save