bool
RazorAHRS::_init_razor()
{
char in;
int result;
struct timeval t0, t1, t2;
const std::string synch_token = "#SYNCH";
const std::string new_line = "\r\n";
// start time
gettimeofday(&t0, nullptr);
// request synch token to see if Razor is really present
const std::string contact_synch_id = "00";
const std::string contact_synch_request = "#s" + contact_synch_id;
const std::string contact_synch_reply = synch_token + contact_synch_id + new_line;
write(_serial_port, contact_synch_request.data(), contact_synch_request.length());
gettimeofday(&t1, nullptr);
// set non-blocking I/O
if (!_set_nonblocking_io()) return false;
/* look for tracker */
while (true)
{
// try to read one byte from the port
result = read(_serial_port, &in, 1);
// one byte read
if (result > 0)
{
if (_read_token(contact_synch_reply, in))
break;
}
// no data available
else if (result == 0)
usleep(1000); // sleep 1ms
// error?
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (1).");
}
// check timeout
gettimeofday(&t2, nullptr);
if (elapsed_ms(t1, t2) > 200)
{
// 200ms elapsed since last request and no answer -> request synch again
// (this happens when DTR is connected and Razor resets on connect)
write(_serial_port, contact_synch_request.data(), contact_synch_request.length());
t1 = t2;
}
if (elapsed_ms(t0, t2) > _connect_timeout_ms)
// timeout -> tracker not present
throw std::runtime_error("Can not init: tracker does not answer.");
}
/* configure tracker */
// set correct binary output mode, enable continuous streaming, disable errors and
// request synch token. So we're good, no matter what state the tracker
// currently is in.
const std::string config_synch_id = "01";
const std::string config_synch_reply = synch_token + config_synch_id + new_line;
std::string config = "#o1#oe0#s" + config_synch_id;
if (_mode == YAW_PITCH_ROLL) config = "#ob" + config;
else if (_mode == ACC_MAG_GYR_RAW) config = "#osrb" + config;
else if (_mode == ACC_MAG_GYR_CALIBRATED) config = "#oscb" + config;
else throw std::runtime_error("Can not init: unknown 'mode' parameter.");
write(_serial_port, config.data(), config.length());
// set blocking I/O
// (actually semi-blocking, because VTIME is set)
if (!_set_blocking_io()) return false;
while (true)
{
// try to read one byte from the port
result = read(_serial_port, &in, 1);
// one byte read
if (result > 0)
{
if (_read_token(config_synch_reply, in))
break; // alrighty
}
// error?
else
{
if (errno != EAGAIN && errno != EINTR)
throw std::runtime_error("Can not read from serial port (2).");
}
}
// we keep using blocking I/O
//if (_set_blocking_io() == -1)
// return false;
return true;
}
static bool _read_params_bool(struct stream_data *s)
{
static struct notify_msg_param const notify_msg = {
.op = 'P',
.is_flag = 1,
};
char param[sizeof(PARAM_PREFIX_FLAG) + PARAM_KEY_SZ];
char *p;
unsigned char end_c;
notification_send(&s->notify, ¬ify_msg, sizeof notify_msg);
p = stpcpy(param, PARAM_PREFIX_FLAG);
if (!_read_token(s, p, PARAM_KEY_SZ, true, &end_c) || end_c != '\0') {
fprintf(stderr, "bad flag name\n");
return false;
}
if (setenv(param, "1", true) < 0) {
perror("setenv()");
return false;
}
return true;
}
static bool _read_params_keyval(struct stream_data *s)
{
static struct notify_msg_param const notify_msg = {
.op = 'P',
.is_flag = 0,
};
char key[sizeof(PARAM_PREFIX_KEYVAL) + PARAM_KEY_SZ];
char val[PARAM_VAL_SZ];
char *p;
unsigned char end_c;
notification_send(&s->notify, ¬ify_msg, sizeof notify_msg);
p = stpcpy(key, PARAM_PREFIX_KEYVAL);
if (!_read_token(s, p, PARAM_KEY_SZ, true, &end_c) || end_c != '=') {
fprintf(stderr, "bad key name\n");
return false;
}
if (!_read_token(s, val, sizeof val, false, &end_c) || end_c != '\0') {
fprintf(stderr, "bad value name\n");
return false;
}
if (setenv(key, val, true) < 0) {
perror("setenv()");
return false;
}
return true;
}
static bool stream_data_read_params(struct stream_data *s)
{
bool rc = false;
if (s->num_queued_char > 0) {
fprintf(stderr, "internal error: data already queued internally\n");
return false;
}
for (;;) {
unsigned char c;
ssize_t l = read(s->fd, &c, sizeof c);
if (l < 0) {
perror("read()");
rc = false;
break;
}
if (l == 0) {
rc = true;
break;
}
switch (c) {
case '+':
rc = _read_params_bool(s);
break;
case '?':
rc = _read_params_keyval(s);
break;
default:
s->queued_char = c;
s->num_queued_char = 1;
rc = true;
goto out;
}
if (!rc)
break;
}
out:
return rc;
}
static bool stream_data_read(struct stream_data *s, void *buf, size_t cnt,
bool ignore_eos)
{
size_t tlen = 0;
if (cnt > 0 && s->num_queued_char > 0) {
size_t l = MIN(s->num_queued_char, cnt);
memcpy(buf, &s->queued_char, l);
buf += l;
cnt -= l;
s->num_queued_char -= l;
}
while (cnt > 0) {
ssize_t l = read(s->fd, buf, cnt);
if (l > 0) {
cnt -= l;
buf += l;
tlen += l;
} else if (l < 0) {
perror("read()");
break;
} else {
if (tlen > 0 || !ignore_eos)
fprintf(stderr, "%s: EOS reached while reading\n",
__func__);
else
cnt = 0;
s->is_eos = true;
break;
}
}
return cnt == 0;
}
