bool checkreturn pb_skip_varint(pb_istream_t *stream)
{
uint8_t byte;
do
{
if (!pb_read(stream, &byte, 1))
return false;
} while (byte & 0x80);
return true;
}
static bool read_string(pb_istream_t *stream, const pb_field_t *field,
void **arg) {
size_t len = stream->bytes_left;
char *str = malloc(len + 1);
if (!pb_read(stream, str, len)) return false;
str[len] = '\0';
printf("Server replied %s\n", str);
free(str);
return true;
}
static bool read_string(pb_istream_t *stream, const pb_field_t *field, void **arg)
{
uint8_t buf[16] = {0};
size_t len = stream->bytes_left;
if (len > sizeof(buf) - 1 || !pb_read(stream, buf, len))
return false;
TEST(strcmp((char*)buf, *arg) == 0);
return true;
}
void
pb_read_str(struct pbuf* pbuf, char* str, int maxlen, int* pos)
{
uint32_t strlen;
/* Read the string length */
pb_read_arrl(pbuf, &strlen, sizeof(strlen), pos);
assert(strlen < maxlen - 1);
/* Read the string and padding */
pb_read(pbuf, str, strlen, pos);
str[strlen] = '\0';
pb_alignl(pos);
}
static char *read_string(pb_file *pbf, int size) {
char *page;
if((page = (char *) malloc(size + 1))) {
pb_read(pbf, page, size);
page[size] = '\0';
}
else {
fprintf(stderr, "Malloc of page buffer failed.\n");
fprintf(stderr, "Error: %s\n", strerror(errno));
exit(1);
}
return page;
}
static char *new_filename(pb_file *pbf, ub4 len) {
char *filename;
if(!(filename = (char *) malloc(len + 1))) {
fprintf(stderr, "Malloc failed of filename\n");
fprintf(stderr, "Error: %s\n", strerror(errno));
}
pb_read(pbf, filename, len);
filename[len] = '\0';
#ifdef DEBUG
printf("filename is %s\n", filename);
#endif
return filename;
}
void task_main(void *pvParameters)
{
while(pb_read(pb2) == 0)
{
// wait for button pressed.
}
vTaskDelay(1000);
for (int speed = 1000; speed < 10000; speed = speed + 50)
{
motor_go_forward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_go_backward();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_right();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
motor_turn_left();
motor_speed_set(speed, motor_ch_all);
motor_start(motor_ch_all);
timer_delay_mil(1000);
motor_stop(motor_ch_all);
timer_delay_mil(500);
}
// pidMotorMoveFor1Cell(85);
while(1)
{
}
}
bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest)
{
uint8_t byte;
int bitpos = 0;
*dest = 0;
while (bitpos < 64 && pb_read(stream, &byte, 1))
{
*dest |= (uint64_t)(byte & 0x7F) << bitpos;
bitpos += 7;
if (!(byte & 0x80))
return true;
}
PB_RETURN_ERROR(stream, "varint overflow");
}
static int cont_grep(regex_t *exp, regex_t *nl_exp, int fd, pb_file *pbf, int options) {
int retflag = TRUE;
int i;
int j;
ub4 csize;
ub4 usize;
ub2 fnlen;
ub2 eflen;
ub2 flags;
ub2 method;
ub1 file_header[30];
char *filename;
char *str_stream;
regmatch_t *match_array;
regmatch_t *nl_offsets;
if(pb_read(pbf, (file_header + 4), 26) != 26) {
perror("read");
retflag = FALSE;
}
else {
decd_siz(&csize, &usize, &fnlen, &eflen, &flags, &method, file_header);
filename = new_filename(pbf, fnlen);
lseek(fd, eflen, SEEK_CUR);
if(filename[fnlen - 1] != '/') {
str_stream = (method == 8 || (flags & 0x0008)) ?
(char *) inflate_string(pbf, &csize, &usize) :
read_string(pbf, csize);
if(flags & 0x008) check_crc(pbf, str_stream, usize);
mk_ascii(str_stream, usize);
match_array = fnd_match(exp, str_stream, &i);
if((options & JG_PRINT_LINE_NUMBER) && i)
nl_offsets = fnd_match(nl_exp, str_stream, &j);
prnt_mtchs(exp, filename, str_stream, match_array, nl_offsets, i, j, options);
if(match_array) free(match_array);
free(str_stream);
}
free(filename);
retflag = TRUE;
}
return retflag;
}
static Bytef *hrd_inflate_str(pb_file *pbf, ub4 *csize, ub4 *usize) {
Bytef *out_buff;
Bytef *tmp;
Bytef in_buff[RDSZ];
unsigned int rdamt;
int i;
int zret;
i = 1;
out_buff = NULL;
zret = Z_OK;
while(zret != Z_STREAM_END && (rdamt = pb_read(pbf, in_buff, RDSZ)))
{
zs.avail_in = rdamt;
zs.avail_out = 0;
zs.next_in = in_buff;
do {
if((tmp = (Bytef *) realloc(out_buff, (RDSZ * i) + 1))) {
out_buff = tmp;
zs.next_out = &(out_buff[(RDSZ * (i - 1)) - zs.avail_out]);
zs.avail_out += RDSZ;
i++;
}
else {
fprintf(stderr, "Realloc of out_buff failed.\n");
fprintf(stderr, "Error: %s\n", strerror(errno));
exit(1);
}
} while((zret = inflate(&zs, 0)) == Z_OK);
report_str_error(zret);
}
pb_push(pbf, zs.next_in, zs.avail_in);
out_buff[(RDSZ * (i - 1)) - zs.avail_out] = '\0';
*usize = zs.total_out;
*csize = zs.total_in;
inflateReset(&zs);
return out_buff;
}
bool checkreturn pb_decode_varint(pb_istream_t *stream, uint64_t *dest)
{
uint8_t byte;
uint8_t bitpos = 0;
uint64_t result = 0;
do
{
if (bitpos >= 64)
PB_RETURN_ERROR(stream, "varint overflow");
if (!pb_read(stream, &byte, 1))
return false;
result |= (uint64_t)(byte & 0x7F) << bitpos;
bitpos = (uint8_t)(bitpos + 7);
} while (byte & 0x80);
*dest = result;
return true;
}
static void check_crc(pb_file *pbf, const char *stream, ub4 usize) {
ub4 crc;
ub4 lcrc;
ub1 scratch[16];
crc = crc32(crc, NULL, 0);
crc = crc32(crc, (const unsigned char *)stream, usize);
if(pb_read(pbf, scratch, 16) != 16) {
perror("read");
exit(1);
}
if(UNPACK_UB4(scratch, 0) != 0x08074b50) {
fprintf(stderr, "Error! Missing data descriptor!\n");
exit(1);
}
lcrc = UNPACK_UB4(scratch, 4);
if(crc != lcrc){
fprintf(stderr, "Error! CRCs do not match! Got %x, expected %x\n",
crc, lcrc);
exit(1);
}
}
static Bytef *ez_inflate_str(pb_file *pbf, ub4 csize, ub4 usize) {
Bytef *out_buff;
Bytef *in_buff;
unsigned int rdamt;
if((zs.next_in = in_buff = (Bytef *) malloc(csize))) {
if((zs.next_out = out_buff = (Bytef *) malloc(usize + 1))) {
if((rdamt = pb_read(pbf, zs.next_in, csize)) == csize) {
zs.avail_in = csize;
zs.avail_out = usize;
report_str_error(inflate(&zs, 0));
free(in_buff);
inflateReset(&zs);
out_buff[usize] = '\0';
}
else {
fprintf(stderr, "Read failed on input file.\n");
fprintf(stderr, "Tried to read %u but read %u instead.\n", csize, rdamt);
free(in_buff);
free(out_buff);
exit(1);
}
}
else {
fprintf(stderr, "Malloc of out_buff failed.\n");
fprintf(stderr, "Error: %s\n", strerror(errno));
free(in_buff);
exit(1);
}
}
else {
fprintf(stderr, "Malloc of in_buff failed.\n");
fprintf(stderr, "Error: %s\n", strerror(errno));
exit(1);
}
return out_buff;
}
int read_data()
{
unsigned char buffer[BUFFER_SIZE];
oem4_header_t header;
int result, read_len, rd_cnt;
unsigned int crc;
pushback_reader_t efd;
oem4_bestpos_t bestpos;
oem4_bestvel_t bestvel;
gps_pos_novatel_t gps_pos;
gps_vel_novatel_t gps_vel;
struct timespec tloc;
static int is_init = 0;
//in case of read-errors from ser. device this function will be called several times in case of error from main.
//
if(is_init) {
erret(pb_reset(&efd));
}
else {
erret(pb_init(fd, BUFFER_SIZE*2, &efd));
is_init = 1;
}
memset(buffer, 0, sizeof(buffer));
memset(&gps_pos, 0, sizeof(gps_pos));
memset(&gps_vel, 0, sizeof(gps_vel));
rd_cnt = 0;
result = 0;
while(1)
{
// read sync
erret(pb_read(&efd, buffer, (unsigned int *)&read_len, OEM4_SYNC_SIZE));
if(read_len<=0) fprintf(stderr,"read timeout\r\n");
// read enough (3) sync bytes?
if (read_len != OEM4_SYNC_SIZE)
{
// no, pusback what we had read so far and try again...
erret(pb_unread(&efd, buffer, read_len));
continue;
}
// increment counter of read bytes
rd_cnt += read_len;
// sync found?
if (buffer[0] != OEM4_BIN_SYNC_HEADER[0] ||
buffer[1] != OEM4_BIN_SYNC_HEADER[1] ||
buffer[2] != OEM4_BIN_SYNC_HEADER[2])
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
// if no (partial) sync message is found, nothing is unread (rd_cnt - result) == 0
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
// everything is pushed back, therefore nothing is read
rd_cnt = 0;
continue;
}
// read head length
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, 1));
// nothing was read?
if (result == 0)
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt);
// push back and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
// assign header length
header.length = buffer[3];
// header too long for the buffer?
if(header.length + OEM4_SYNC_SIZE >= BUFFER_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// read header, keep in mind that four bytes of the header are allready read
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.length - 4));
// read less than expected?
if (read_len != header.length - 4)
{
// search for sync message or part of sync message
result = oem4_search_sync(buffer, rd_cnt + read_len);
// push back what we had and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
oem4_fill_header(&header, buffer);
// data too long for the buffer?
if ((header.message_length + header.length + OEM4_SYNC_SIZE + OEM4_CRC_SIZE) >= BUFFER_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// read data and CRC
erret(pb_read(&efd, &buffer[rd_cnt], (unsigned int *)&read_len, header.message_length + OEM4_CRC_SIZE));
// read less than expected?
if (read_len != header.message_length + OEM4_CRC_SIZE)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt + read_len);
// push back what we had and try again
erret(pb_unread(&efd, &buffer[result], rd_cnt + read_len - result));
rd_cnt = 0;
continue;
}
rd_cnt += read_len;
crc32_block(buffer, rd_cnt, &crc);
// CRC ok? (crc == 0)
if (crc)
{
// this is a serious error, search for sync
result = oem4_search_sync(buffer, rd_cnt);
// unread everything from the beginning of the sync message
erret(pb_unread(&efd, &buffer[result], rd_cnt - result));
rd_cnt = 0;
continue;
}
// decode message
switch(header.message_id)
{
case OEM4_BESTPOS:
if (!oem4_get_best_pos(&bestpos, &buffer[header.length], header.message_length))
{
#if AR_VERBOSE==1
oem4_dump_best_pos(&bestpos);
#endif
memcpy(&bestpos.gps_header, &header, sizeof(header));
create_gps_position(&bestpos, &gps_pos);
SHM_WriteSlot(config.ishm_pos_out, &gps_pos, sizeof(gps_pos));
clock_gettime(CLOCK_REALTIME, &tloc);
if(abs(tloc.tv_sec - gps_pos.gps_time.tv_sec) > 60*60*4 && gps_pos.solution_type>0)
clock_settime(CLOCK_REALTIME, &gps_pos.gps_time);
}
break;
case OEM4_BESTVEL:
if (!oem4_get_best_vel(&bestvel, &buffer[header.length], header.message_length))
{
memcpy(&bestvel.gps_header, &header, sizeof(header));
create_gps_velocity(&bestvel, &gps_vel);
SHM_WriteSlot(config.ishm_vel_out, &gps_vel, sizeof(gps_vel));
}
break;
}
rd_cnt = 0;
}
return 0;
}
int
proc_err(pb_t *ppb, int pc, paramv_t *pv)
{
register func_t *f;
register int i;
register ctx_t *ctx;
#ifdef xCTR2
if (tTf(6, 8))
lprintf("proc_err: new = %d\n", Ctx.ctx_new);
#endif
pb_prime(ppb, PB_ERR);
/*
** Scan back on the list of context dependencies.
** If we come to someone who can process this message,
** we go ahead and do it. We also take this
** opportunity to unwind the context list & call the
** cleanup functions.
*/
for (ctx = &Ctx; ctx != NULL; ctx = ctx->ctx_link) {
setprocname(ctx->ctx_name);
f = ctx->ctx_fn;
#ifdef xCTR2
if (tTf(6, 9))
lprintf("proc_err: unwinding %s: errfn=%x, ppb=%x, link=%x, resp=%d, fn=%x\n",
getprocname(), ctx->ctx_errfn, ctx->ctx_ppb,
ctx->ctx_link, ctx->ctx_resp, f);
#endif
/* Do the actual error processing. */
ppb->pb_proc = ctx->ctx_resp;
if (ctx->ctx_errfn != NULL)
i = (*ctx->ctx_errfn)(pc, pv);
else
i = -1;
#ifdef xCTR2
if (tTf(6, 11))
lprintf("proc_err: errcode %d\n", i);
#endif
if (i == 0)
break;
else if (i > 0) {
/* turn into nonfatal error */
ppb->pb_stat |= PB_INFO;
ppb->pb_proc = PB_FRONT;
} else {
/* call the cleanup function */
if (f != NULL && f->fn_active > 0) {
(*f->fn_cleanup)(1);
}
}
/* arrange to leave if parent not in this process */
if (ppb->pb_proc != Cm.cm_myproc) {
send_off(ppb, pc, pv);
pb_flush(ppb);
/* throw away dead contexts and exit */
break;
}
}
if (ctx == NULL) {
syserr("proc_err: no parent");
}
#ifdef xCTR3
MonPpb = getmonppb();
if (tTf(6, 12)) {
lprintf("proc_err: cleanup: ctx=%x, ->_link=%x, MonPpb = ", ctx, ctx->ctx_link);
pb_dump(MonPpb, TRUE);
}
#endif
/* pop contexts down to ctx and exit */
ctx = ctx->ctx_link;
while (Ctx.ctx_link != ctx) {
if (Ctx.ctx_link == NULL)
syserr("proc_err: underflow");
Ctx.ctx_new = TRUE;
resetp();
}
/*
** Flush input pipe.
** THIS CODE IS ONLY NEEDED TO MAKE READMON WORK, AND
** SHOULD BE REMOVED WHEN READMON GOES AWAY!!
*/
if (ctx == NULL) {
Cm.cm_input = Cm.cm_rinput;
MonPpb = getmonppb();
while (!BITISSET(PB_EOF, MonPpb->pb_stat)) {
pb_read(MonPpb);
}
MonPpb->pb_st = PB_UNKNOWN;
}
longjmp(Ctx.ctx_jbuf, 1);
}
int inflate_file(pb_file *pbf, int out_fd, struct zipentry *ze){
Bytef in_buff[RDSZ];
Bytef out_buff[RDSZ];
unsigned int rdamt;
int rtval;
ub4 crc = 0;
zs.avail_in = 0;
crc = crc32(crc, NULL, 0); /* initialize crc */
/* loop until we've consumed all the compressed data */
for(;;){
if(zs.avail_in == 0){
if((rdamt = pb_read(pbf, in_buff, RDSZ)) == 0)
break;
else if((int)rdamt < 0){
perror("read");
exit(1);
}
#ifdef DEBUG
printf("%d bytes read\n", rdamt);
#endif
zs.next_in = in_buff;
zs.avail_in = rdamt;
}
zs.next_out = out_buff;
zs.avail_out = RDSZ;
if((rtval = inflate(&zs, 0)) != Z_OK){
if(rtval == Z_STREAM_END){
#ifdef DEBUG
printf("end of stream\n");
#endif
if(zs.avail_out != RDSZ){
crc = crc32(crc, out_buff, (RDSZ - zs.avail_out));
if(out_fd >= 0)
if(write(out_fd, out_buff, (RDSZ - zs.avail_out)) !=
(int)(RDSZ - zs.avail_out)){
perror("write");
exit(1);
}
}
break;
} else {
fprintf(stderr, "Error inflating file! (%d)\n", rtval);
exit(1);
}
} else {
if(zs.avail_out != RDSZ){
crc = crc32(crc, out_buff, (RDSZ - zs.avail_out));
if(out_fd >= 0)
if(write(out_fd, out_buff, (RDSZ - zs.avail_out)) !=
(int)(RDSZ - zs.avail_out)){
perror("write");
exit(1);
}
zs.next_out = out_buff;
zs.avail_out = RDSZ;
}
}
}
#ifdef DEBUG
printf("done inflating\n");
#endif
#ifdef DEBUG
printf("%d bytes left over\n", zs.avail_in);
#endif
#ifdef DEBUG
printf("CRC is %x\n", crc);
#endif
ze->crc = crc;
pb_push(pbf, zs.next_in, zs.avail_in);
ze->usize = zs.total_out;
inflateReset(&zs);
return 0;
}
