You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
104 lines
2.8 KiB
104 lines
2.8 KiB
6 years ago
|
#include <stdio.h>
|
||
|
|
#include <stdlib.h>
|
||
|
|
#include <string.h>
|
||
|
|
#include <signal.h>
|
||
|
|
#include <unistd.h>
|
||
|
|
#include <sched.h>
|
||
|
|
#include <sys/time.h>
|
||
|
|
#include <sys/cdefs.h>
|
||
|
|
#include <sys/types.h>
|
||
|
|
#include <sys/time.h>
|
||
|
|
#include <assert.h>
|
||
|
|
#include <math.h>
|
||
|
|
#include <ctime>
|
||
|
|
#include <chrono>
|
||
|
|
#include <map>
|
||
|
|
#include <vector>
|
||
|
|
#include <iostream>
|
||
|
|
|
||
|
|
#include <zmq.h>
|
||
|
|
#include <capnp/serialize.h>
|
||
|
|
#include "cereal/gen/cpp/log.capnp.h"
|
||
|
|
|
||
|
|
#include "common/params.h"
|
||
|
|
#include "common/swaglog.h"
|
||
|
|
#include "common/timing.h"
|
||
|
|
#include "common/util.h"
|
||
|
|
#include "ublox_msg.h"
|
||
|
|
|
||
|
|
using namespace ublox;
|
||
|
|
|
||
|
|
void write_file(std::string fpath, uint8_t *data, int len) {
|
||
|
|
FILE* f = fopen(fpath.c_str(), "wb");
|
||
|
|
if (!f) {
|
||
|
|
std::cout << "Open " << fpath << " failed" << std::endl;
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
fwrite(data, len, 1, f);
|
||
|
|
fclose(f);
|
||
|
|
}
|
||
|
|
|
||
|
|
static size_t len = 0U;
|
||
|
|
static size_t consumed = 0U;
|
||
|
|
static uint8_t *data = NULL;
|
||
|
|
static int save_idx = 0;
|
||
|
|
static std::string prefix;
|
||
|
|
static void *gps_sock, *ublox_gnss_sock;
|
||
|
|
|
||
|
|
int poll_ubloxraw_msg(void *gpsLocationExternal, void *ubloxGnss, void *subscriber, zmq_msg_t *msg) {
|
||
|
|
gps_sock = gpsLocationExternal;
|
||
|
|
ublox_gnss_sock = ubloxGnss;
|
||
|
|
size_t consuming = min(len - consumed, 128);
|
||
|
|
if(consumed < len) {
|
||
|
|
// create message
|
||
|
|
capnp::MallocMessageBuilder msg_builder;
|
||
|
|
cereal::Event::Builder event = msg_builder.initRoot<cereal::Event>();
|
||
|
|
event.setLogMonoTime(nanos_since_boot());
|
||
|
|
auto ublox_raw = event.initUbloxRaw(consuming);
|
||
|
|
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
|
||
|
|
auto words = capnp::messageToFlatArray(msg_builder);
|
||
|
|
auto bytes = words.asBytes();
|
||
|
|
zmq_msg_init_size (msg, bytes.size());
|
||
|
|
memcpy (zmq_msg_data(msg), (void *)bytes.begin(), bytes.size());
|
||
|
|
consumed += consuming;
|
||
|
|
return 1;
|
||
|
|
} else
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
|
||
|
|
int send_gps_event(uint8_t msg_cls, uint8_t msg_id, void *s, const void *buf, size_t len, int flags) {
|
||
|
|
if(msg_cls == CLASS_NAV && msg_id == MSG_NAV_PVT)
|
||
|
|
assert(s == gps_sock);
|
||
|
|
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_RAW)
|
||
|
|
assert(s == ublox_gnss_sock);
|
||
|
|
else if(msg_cls == CLASS_RXM && msg_id == MSG_RXM_SFRBX)
|
||
|
|
assert(s == ublox_gnss_sock);
|
||
|
|
else
|
||
|
|
assert(0);
|
||
|
|
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, len);
|
||
|
|
save_idx ++;
|
||
|
|
return len;
|
||
|
|
}
|
||
|
|
|
||
|
|
int main(int argc, char** argv) {
|
||
|
|
if(argc < 3) {
|
||
|
|
printf("Format: ubloxd_test stream_file_path save_prefix\n");
|
||
|
|
return 0;
|
||
|
|
}
|
||
|
|
// Parse 11360 msgs, generate 9452 events
|
||
|
|
data = (uint8_t *)read_file(argv[1], &len);
|
||
|
|
if(data == NULL) {
|
||
|
|
LOGE("Read file %s failed\n", argv[1]);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
prefix = argv[2];
|
||
|
|
ubloxd_main(poll_ubloxraw_msg, send_gps_event);
|
||
|
|
free(data);
|
||
|
|
printf("Generated %d cereal events\n", save_idx);
|
||
|
|
if(save_idx != 9452) {
|
||
|
|
printf("Event count error: %d\n", save_idx);
|
||
|
|
return -1;
|
||
|
|
}
|
||
|
|
return 0;
|
||
|
|
}
|