void xsens_event_task( void )
{
while (Xsens1Link(ChAvailable()) && !xsens_msg_received[0]) {
parse_xsens_msg(0, Xsens1Link(Getch()));
}
/*
while (Xsens2Link(ChAvailable()) && !xsens_msg_received[1])
parse_xsens_msg(1, Xsens2Link(Getch()));
*/
for (int i = 0; i < XSENS_COUNT; i++) {
if (xsens_msg_received[i]) {
// first make room
while (xsens_msg_buf_count[i] >= XSENS_MSG_BUF) {
// throwing away old stuff
xsens_msg_buf_ci[i] = (xsens_msg_buf_ci[i] + 1 ) % XSENS_MSG_BUF;
xsens_msg_buf_count[i]--;
}
xsens_msg_buf_pi[i] = (xsens_msg_buf_pi[i] + 1 ) % XSENS_MSG_BUF;
xsens_msg_buf_count[i]++;
//xsens_parse_msg(i);
xsens_msg_received[i] = FALSE;
}
}
}
void ins_xsens_event(void)
{
xsens_event();
if (xsens.msg_received) {
parse_xsens_msg();
handle_ins_msg();
xsens.msg_received = false;
}
}
