|
|
|
@ -282,13 +282,15 @@ kj::Array<capnp::word> UbloxMsgParser::gen_nav_data() { |
|
|
|
|
for(int i = 0; i < msg->numWords;i++) |
|
|
|
|
words.push_back(measurements[i].dwrd); |
|
|
|
|
|
|
|
|
|
if(subframeId == 1) { |
|
|
|
|
nav_frame_buffer[msg->gnssId][msg->svid] = subframes_map(); |
|
|
|
|
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; |
|
|
|
|
} else if(nav_frame_buffer[msg->gnssId][msg->svid].find(subframeId-1) != nav_frame_buffer[msg->gnssId][msg->svid].end()) |
|
|
|
|
nav_frame_buffer[msg->gnssId][msg->svid][subframeId] = words; |
|
|
|
|
if(nav_frame_buffer[msg->gnssId][msg->svid].size() == 5) { |
|
|
|
|
EphemerisData ephem_data(msg->svid, nav_frame_buffer[msg->gnssId][msg->svid]); |
|
|
|
|
subframes_map &map = nav_frame_buffer[msg->gnssId][msg->svid]; |
|
|
|
|
if (subframeId == 1) { |
|
|
|
|
map = subframes_map(); |
|
|
|
|
map[subframeId] = words; |
|
|
|
|
} else if (map.find(subframeId-1) != map.end()) { |
|
|
|
|
map[subframeId] = words; |
|
|
|
|
} |
|
|
|
|
if(map.size() == 5) { |
|
|
|
|
EphemerisData ephem_data(msg->svid, map); |
|
|
|
|
MessageBuilder msg_builder; |
|
|
|
|
auto eph = msg_builder.initEvent().initUbloxGnss().initEphemeris(); |
|
|
|
|
eph.setSvId(ephem_data.svId); |
|
|
|
|