diff --git a/cereal b/cereal index df60d9da00..f16d2a211b 160000 --- a/cereal +++ b/cereal @@ -1 +1 @@ -Subproject commit df60d9da003aaf504c4843220c22999ca8311fa4 +Subproject commit f16d2a211bd5d79b5094ac1abc22d7a5241df101 diff --git a/selfdrive/locationd/locationd.cc b/selfdrive/locationd/locationd.cc index b723878e9d..120171d4b5 100755 --- a/selfdrive/locationd/locationd.cc +++ b/selfdrive/locationd/locationd.cc @@ -262,7 +262,6 @@ void Localizer::input_fake_gps_observations(double current_time) { void Localizer::handle_gps(double current_time, const cereal::GpsLocationData::Reader& log) { // 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)); @@ -462,9 +461,9 @@ kj::ArrayPtr Localizer::get_message_bytes(MessageBuilder& msg_build { cereal::Event::Builder evt = msg_builder.initEvent(); evt.setLogMonoTime(logMonoTime); + evt.setValid(inputsOK); cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman(); this->build_live_location(liveLoc); - liveLoc.setInputsOK(inputsOK); liveLoc.setSensorsOK(sensorsOK); liveLoc.setGpsOK(gpsOK); return msg_builder.toBytes(); @@ -499,12 +498,15 @@ int Localizer::locationd_thread() { while (!do_exit) { sm.update(); - for (const char* service : service_list) { - if (sm.updated(service) && sm.valid(service)) { - const cereal::Event::Reader log = sm[service]; - this->handle_msg(log); + if (sm.allAliveAndValid()){ + for (const char* service : service_list) { + if (sm.updated(service)){ + const cereal::Event::Reader log = sm[service]; + this->handle_msg(log); + } } } + if (sm.updated("cameraOdometry")) { uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime(); diff --git a/selfdrive/locationd/paramsd.py b/selfdrive/locationd/paramsd.py index 0efb4d04be..44ffccdf1a 100755 --- a/selfdrive/locationd/paramsd.py +++ b/selfdrive/locationd/paramsd.py @@ -62,7 +62,7 @@ class ParamsLearner: yaw_rate_valid = yaw_rate_valid and abs(yaw_rate) < 1 # rad/s if self.active: - if msg.inputsOK and msg.posenetOK: + if msg.posenetOK: if yaw_rate_valid: self.kf.predict_and_observe(t, @@ -160,10 +160,11 @@ def main(sm=None, pm=None): while True: sm.update() - for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): - if sm.updated[which]: - t = sm.logMonoTime[which] * 1e-9 - learner.handle_log(t, which, sm[which]) + if sm.all_alive_and_valid(): + for which in sorted(sm.updated.keys(), key=lambda x: sm.logMonoTime[x]): + if sm.updated[which]: + t = sm.logMonoTime[which] * 1e-9 + learner.handle_log(t, which, sm[which]) if sm.updated['liveLocationKalman']: x = learner.kf.x @@ -198,6 +199,8 @@ def main(sm=None, pm=None): liveParameters.angleOffsetAverageStd = float(P[States.ANGLE_OFFSET]) liveParameters.angleOffsetFastStd = float(P[States.ANGLE_OFFSET_FAST]) + msg.valid = sm.all_alive_and_valid() + if sm.frame % 1200 == 0: # once a minute params = { 'carFingerprint': CP.carFingerprint, diff --git a/selfdrive/locationd/test/test_locationd.py b/selfdrive/locationd/test/test_locationd.py index 76f82efc10..515bd59431 100755 --- a/selfdrive/locationd/test/test_locationd.py +++ b/selfdrive/locationd/test/test_locationd.py @@ -4,10 +4,12 @@ import json import random import unittest import time +import capnp from cffi import FFI from cereal import log import cereal.messaging as messaging +from cereal.services import service_list from common.params import Params from selfdrive.manager.process_config import managed_processes @@ -103,13 +105,15 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t ret = self.localizer_get_msg() self.assertFalse(ret.liveLocationKalman.posenetOK) + class TestLocationdProc(unittest.TestCase): MAX_WAITS = 1000 + LLD_MSGS = {'gpsLocationExternal', 'cameraOdometry', 'carState', 'sensorEvents', 'liveCalibration'} def setUp(self): random.seed(123489234) - self.pm = messaging.PubMaster({'gpsLocationExternal', 'cameraOdometry'}) + self.pm = messaging.PubMaster(self.LLD_MSGS) managed_processes['locationd'].prepare() managed_processes['locationd'].start() @@ -127,43 +131,53 @@ class TestLocationdProc(unittest.TestCase): waits_left -= 1 time.sleep(0.0001) - def test_params_gps(self): - # first reset params - Params().put('LastGPSPosition', json.dumps({"latitude": 0.0, "longitude": 0.0, "altitude": 0.0})) - - lat = 30 + (random.random() * 10.0) - lon = -70 + (random.random() * 10.0) - alt = 5 + (random.random() * 10.0) + def get_fake_msg(self, name, t): + try: + msg = messaging.new_message(name) + except capnp.lib.capnp.KjException: + msg = messaging.new_message(name, 0) - for _ in range(1000): # because of kalman filter, send often - msg = messaging.new_message('gpsLocationExternal') - msg.logMonoTime = 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 = lat - msg.gpsLocationExternal.longitude = lon - msg.gpsLocationExternal.altitude = alt - self.send_msg(msg) - - for _ in range(250): # params is only written so often - msg = messaging.new_message('cameraOdometry') - msg.logMonoTime = 0 + msg.gpsLocationExternal.latitude = self.lat + msg.gpsLocationExternal.longitude = self.lon + msg.gpsLocationExternal.altitude = self.alt + elif name == 'cameraOdometry': msg.cameraOdometry.rot = [0.0, 0.0, 0.0] msg.cameraOdometry.rotStd = [0.0, 0.0, 0.0] msg.cameraOdometry.trans = [0.0, 0.0, 0.0] msg.cameraOdometry.transStd = [0.0, 0.0, 0.0] - self.send_msg(msg) + msg.logMonoTime = t + return msg + def test_params_gps(self): + # first reset params + Params().delete('LastGPSPosition') + + self.lat = 30 + (random.random() * 10.0) + self.lon = -70 + (random.random() * 10.0) + self.alt = 5 + (random.random() * 10.0) + self.fake_duration = 90 # secs + # get fake messages at the correct frequency, listed in services.py + fake_msgs = [] + for sec in range(self.fake_duration): + for name in self.LLD_MSGS: + for j in range(int(service_list[name].frequency)): + fake_msgs.append(self.get_fake_msg(name, int((sec + j / service_list[name].frequency) * 1e9))) + + for fake_msg in sorted(fake_msgs, key=lambda x: x.logMonoTime): + self.send_msg(fake_msg) time.sleep(1) # wait for async params write lastGPS = json.loads(Params().get('LastGPSPosition')) - self.assertAlmostEqual(lastGPS['latitude'], lat, places=3) - self.assertAlmostEqual(lastGPS['longitude'], lon, places=3) - self.assertAlmostEqual(lastGPS['altitude'], alt, places=3) + self.assertAlmostEqual(lastGPS['latitude'], self.lat, places=3) + self.assertAlmostEqual(lastGPS['longitude'], self.lon, places=3) + self.assertAlmostEqual(lastGPS['altitude'], self.alt, places=3) if __name__ == "__main__": diff --git a/selfdrive/test/process_replay/ref_commit b/selfdrive/test/process_replay/ref_commit index 5619bafff0..26fe9a59c2 100644 --- a/selfdrive/test/process_replay/ref_commit +++ b/selfdrive/test/process_replay/ref_commit @@ -1 +1 @@ -7e6072a254791e4106a15ecbf94c16f40d54b459 \ No newline at end of file +072ee2ba6bca1ea5943fef27b0cc764e40275568 \ No newline at end of file