mawei/StarPilot
1
0
Fork 0
mirror of https://github.com/firestar5683/StarPilot.git synced 2026-07-02 12:02:09 +08:00
Code Issues Packages Projects Releases Wiki Activity

Always publish from mapsd (#28581)

Browse Source 
old-commit-hash: 9d835020fc01cc6d9598388be7715c9412e3185b
This commit is contained in:
Mitchell Goff
2023-06-16 20:13:24 -07:00
committed by GitHub
parent 300071a27c
commit 88c95293cd
2 changed files with 11 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
selfdrive/navd/map_renderer.cc
+10 -5
View File
@@ -101,8 +101,7 @@ void MapRenderer::msgUpdate() {
auto pos = location.getPositionGeodetic();
auto orientation = location.getCalibratedOrientationNED();
bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && pos.getValid();
if (localizer_valid && (sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
if ((sm->rcv_frame("liveLocationKalman") % LLK_DECIMATION) == 0) {
float bearing = RAD2DEG(orientation.getValue()[2]);
updatePosition(get_point_along_line(pos.getValue()[0], pos.getValue()[1], bearing, MAP_OFFSET), bearing);
@@ -116,6 +115,7 @@ void MapRenderer::msgUpdate() {
if (!loaded()) {
LOGE("failed to render map after retry");
publish(0, false);
}
}
}
@@ -161,7 +161,7 @@ void MapRenderer::update() {
double end_t = millis_since_boot();
if ((vipc_server != nullptr) && loaded()) {
publish((end_t - start_t) / 1000.0);
publish((end_t - start_t) / 1000.0, true);
}
}
@@ -174,7 +174,7 @@ void MapRenderer::sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte>
pm->send("navThumbnail", msg);
}
void MapRenderer::publish(const double render_time) {
void MapRenderer::publish(const double render_time, const bool loaded) {
QImage cap = fbo->toImage().convertToFormat(QImage::Format_RGB888, Qt::AutoColor);
uint64_t ts = nanos_since_boot();
VisionBuf* buf = vipc_server->get_buffer(VisionStreamType::VISION_STREAM_MAP);
@@ -213,8 +213,13 @@ void MapRenderer::publish(const double render_time) {
}
// Send state msg
auto location = (*sm)["liveLocationKalman"].getLiveLocationKalman();
bool localizer_valid = (location.getStatus() == cereal::LiveLocationKalman::Status::VALID) && location.getPositionGeodetic().getValid();
MessageBuilder msg;
auto state = msg.initEvent().initMapRenderState();
auto evt = msg.initEvent();
auto state = evt.initMapRenderState();
evt.setValid(loaded && localizer_valid);
state.setLocationMonoTime((*sm)["liveLocationKalman"].getLogMonoTime());
state.setRenderTime(render_time);
state.setFrameId(frame_id);
selfdrive/navd/map_renderer.h
+1 -1
View File
@@ -34,7 +34,7 @@ private:
std::unique_ptr<VisionIpcServer> vipc_server;
std::unique_ptr<PubMaster> pm;
std::unique_ptr<SubMaster> sm;
void publish(const double render_time);
void publish(const double render_time, const bool loaded);
void sendThumbnail(const uint64_t ts, const kj::Array<capnp::byte> &buf);
QMapboxGLSettings m_settings;