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.
87 lines
2.0 KiB
87 lines
2.0 KiB
#include <stdio.h>
|
|
#include <iostream>
|
|
#include <cassert>
|
|
#include <algorithm>
|
|
|
|
#include "messaging.hpp"
|
|
#include "impl_zmq.hpp"
|
|
|
|
#include "common/params.h"
|
|
#include "common/swaglog.h"
|
|
#include "common/timing.h"
|
|
#include "common/util.h"
|
|
#include "ublox_msg.h"
|
|
|
|
using namespace ublox;
|
|
extern volatile sig_atomic_t do_exit;
|
|
|
|
void write_file(std::string fpath, uint8_t *to_write, int length) {
|
|
FILE* f = fopen(fpath.c_str(), "wb");
|
|
if (!f) {
|
|
std::cout << "Open " << fpath << " failed" << std::endl;
|
|
return;
|
|
}
|
|
fwrite(to_write, length, 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;
|
|
|
|
Message * poll_ubloxraw_msg(Poller * poller) {
|
|
assert(poller);
|
|
|
|
size_t consuming = std::min((int)(len - consumed), 128);
|
|
if(consumed < len) {
|
|
// create message
|
|
MessageBuilder msg_builder;
|
|
auto ublox_raw = msg_builder.initEvent().initUbloxRaw(consuming);
|
|
memcpy(ublox_raw.begin(), (void *)(data + consumed), consuming);
|
|
|
|
auto bytes = msg_builder.toBytes();
|
|
|
|
Message * msg = new ZMQMessage();
|
|
msg->init((char*)bytes.begin(), bytes.size());
|
|
consumed += consuming;
|
|
return msg;
|
|
} else {
|
|
do_exit = 1;
|
|
return NULL;
|
|
}
|
|
}
|
|
|
|
int send_gps_event(PubSocket *s, const void *buf, size_t length) {
|
|
assert(s);
|
|
write_file(prefix + "/" + std::to_string(save_idx), (uint8_t *)buf, length);
|
|
save_idx++;
|
|
return length;
|
|
}
|
|
|
|
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;
|
|
}
|
|
|