spacecruft
/
nopenpilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity

locationd: fix writing LastGPSPosition interval (#23895)

pull/23898/head
Willem Melching 2022-03-02 15:16:19 +01:00 committed by GitHub
parent aabd9a0dff
commit f4c822e8c6
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
1 changed files with 4 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
selfdrive/locationd/locationd.cc
View File

@ -496,6 +496,8 @@ int Localizer::locationd_thread() {
PubMaster pm({ "liveLocationKalman" });
SubMaster sm(service_list, nullptr, { "gpsLocationExternal" });
uint64_t cnt = 0;
while (!do_exit) {
sm.update();
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);
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();
std::string lastGPSPosJSON = util::string_format(
"{\"latitude\": %.15f, \"longitude\": %.15f, \"altitude\": %.15f}", posGeo(0), posGeo(1), posGeo(2));
@ -527,6 +529,7 @@ int Localizer::locationd_thread() {
Params().put("LastGPSPosition", gpsjson);
}, lastGPSPosJSON).detach();
}
cnt++;
}
}
return 0;