locationd: fix writing LastGPSPosition interval (#23895)
parent
aabd9a0dff
commit
f4c822e8c6
|
@ -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…
Reference in New Issue