locationd: Commissue fix (#23951)
* check allAliveAndValid only at filter init * revert cereal and bugfix test * rename filterValid to filterInitialized * bump cereal * correct err * update refspull/23969/merge
parent
d7c758d4bf
commit
dfc1292614
2
cereal
2
cereal
|
@ -1 +1 @@
|
|||
Subproject commit ad2fe885dab99896908b88e765a5f720bfd79b3b
|
||||
Subproject commit e20113a5551810da1afdc5eed5dda3bd37dacfdd
|
|
@ -8,10 +8,10 @@ extern "C" {
|
|||
}
|
||||
|
||||
void localizer_get_message_bytes(Localizer *localizer, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size)
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size)
|
||||
{
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK).asChars();
|
||||
kj::ArrayPtr<char> arr = localizer->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, msgValid).asChars();
|
||||
assert(buff_size >= arr.size());
|
||||
memcpy(buff, arr.begin(), arr.size());
|
||||
}
|
||||
|
|
|
@ -457,15 +457,16 @@ void Localizer::handle_msg(const cereal::Event::Reader& log) {
|
|||
}
|
||||
|
||||
kj::ArrayPtr<capnp::byte> Localizer::get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK)
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid)
|
||||
{
|
||||
cereal::Event::Builder evt = msg_builder.initEvent();
|
||||
evt.setLogMonoTime(logMonoTime);
|
||||
evt.setValid(inputsOK);
|
||||
evt.setValid(msgValid);
|
||||
cereal::LiveLocationKalman::Builder liveLoc = evt.initLiveLocationKalman();
|
||||
this->build_live_location(liveLoc);
|
||||
liveLoc.setSensorsOK(sensorsOK);
|
||||
liveLoc.setGpsOK(gpsOK);
|
||||
liveLoc.setInputsOK(inputsOK);
|
||||
return msg_builder.toBytes();
|
||||
}
|
||||
|
||||
|
@ -497,19 +498,21 @@ int Localizer::locationd_thread() {
|
|||
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
|
||||
|
||||
uint64_t cnt = 0;
|
||||
bool filterInitialized = false;
|
||||
|
||||
while (!do_exit) {
|
||||
sm.update();
|
||||
if (sm.allAliveAndValid()){
|
||||
if (filterInitialized){
|
||||
for (const char* service : service_list) {
|
||||
if (sm.updated(service)){
|
||||
if (sm.updated(service) && sm.valid(service)){
|
||||
const cereal::Event::Reader log = sm[service];
|
||||
this->handle_msg(log);
|
||||
}
|
||||
}
|
||||
} else {
|
||||
filterInitialized = sm.allAliveAndValid();
|
||||
}
|
||||
|
||||
|
||||
if (sm.updated("cameraOdometry")) {
|
||||
uint64_t logMonoTime = sm["cameraOdometry"].getLogMonoTime();
|
||||
bool inputsOK = sm.allAliveAndValid();
|
||||
|
@ -517,7 +520,7 @@ int Localizer::locationd_thread() {
|
|||
bool gpsOK = this->isGpsOK();
|
||||
|
||||
MessageBuilder msg_builder;
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK);
|
||||
kj::ArrayPtr<capnp::byte> bytes = this->get_message_bytes(msg_builder, logMonoTime, inputsOK, sensorsOK, gpsOK, filterInitialized);
|
||||
pm.send("liveLocationKalman", bytes.begin(), bytes.size());
|
||||
|
||||
if (cnt % 1200 == 0 && gpsOK) { // once a minute
|
||||
|
|
|
@ -36,7 +36,7 @@ public:
|
|||
void determine_gps_mode(double current_time);
|
||||
|
||||
kj::ArrayPtr<capnp::byte> get_message_bytes(MessageBuilder& msg_builder, uint64_t logMonoTime,
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK);
|
||||
bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid);
|
||||
void build_live_location(cereal::LiveLocationKalman::Builder& fix);
|
||||
|
||||
Eigen::VectorXd get_position_geodetic();
|
||||
|
|
|
@ -24,7 +24,7 @@ class TestLocationdLib(unittest.TestCase):
|
|||
def setUp(self):
|
||||
header = '''typedef ...* Localizer_t;
|
||||
Localizer_t localizer_init();
|
||||
void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, char *buff, size_t buff_size);
|
||||
void localizer_get_message_bytes(Localizer_t localizer, uint64_t logMonoTime, bool inputsOK, bool sensorsOK, bool gpsOK, bool msgValid, char *buff, size_t buff_size);
|
||||
void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t size);'''
|
||||
|
||||
self.ffi = FFI()
|
||||
|
@ -40,8 +40,8 @@ void localizer_handle_msg_bytes(Localizer_t localizer, const char *data, size_t
|
|||
bytstr = msg_builder.to_bytes()
|
||||
self.lib.localizer_handle_msg_bytes(self.localizer, self.ffi.from_buffer(bytstr), len(bytstr))
|
||||
|
||||
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True):
|
||||
self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
|
||||
def localizer_get_msg(self, t=0, inputsOK=True, sensorsOK=True, gpsOK=True, msgValid=True):
|
||||
self.lib.localizer_get_message_bytes(self.localizer, t, inputsOK, sensorsOK, gpsOK, msgValid, self.ffi.addressof(self.msg_buff, 0), self.buff_size)
|
||||
return log.Event.from_bytes(self.ffi.buffer(self.msg_buff), nesting_limit=self.buff_size // 8)
|
||||
|
||||
def test_liblocalizer(self):
|
||||
|
|
|
@ -1 +1 @@
|
|||
37aa2e3afedc8364d2bbecc5f1b7ed4efec52e30
|
||||
e4a770725fd7240388e1cadf6baf9ee456f65aaf
|
Loading…
Reference in New Issue