locationd: fix writing LastGPSPosition interval (#23895)

old-commit-hash: f4c822e8c6
taco
Willem Melching 3 years ago committed by GitHub
parent dfd521325d
commit 3c3db89b68
  1. 5
      selfdrive/locationd/locationd.cc

@ -496,6 +496,8 @@ int Localizer::locationd_thread() {
PubMaster pm({ "liveLocationKalman" }); PubMaster pm({ "liveLocationKalman" });
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" }); SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
uint64_t cnt = 0;
while (!do_exit) { while (!do_exit) {
sm.update(); sm.update();
if (sm.allAliveAndValid()){ if (sm.allAliveAndValid()){
@ -518,7 +520,7 @@ int Localizer::locationd_thread() {
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);
pm.send("liveLocationKalman", bytes.begin(), bytes.size()); pm.send("liveLocationKalman", bytes.begin(), bytes.size());
if (sm.frame % 1200 == 0 && gpsOK) { // once a minute if (cnt % 1200 == 0 && gpsOK) { // once a minute
VectorXd posGeo = this->get_position_geodetic(); VectorXd posGeo = this->get_position_geodetic();
std::string lastGPSPosJSON = util::string_format( std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2)); "{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
@ -527,6 +529,7 @@ int Localizer::locationd_thread() {
Params().put("LastGPSPosition", gpsjson); Params().put("LastGPSPosition", gpsjson);
}, lastGPSPosJSON).detach(); }, lastGPSPosJSON).detach();
} }
cnt++;
} }
} }
return 0; return 0;

Loading…
Cancel
Save