void update(uint64_t read_time) {
if (prev_sent == 0) prev_sent = last_sent;
if (last_time == 0) {
last_time = read_time;
prev_sent = last_sent;
speed = 0.0;
speeds.append(speed);
}
else if (read_time - last_time >= 10000) {
/*
double dt = (double)(read_time - last_time);
double dv = (double)(last_sent - prev_sent);
speed = (long)( dv / dt * 1000000.0) ;
*/
rate_len = findMovement(positions, 20, *position_history);
if (rate_len < *position_history) {
speed = 1000000.0 * rate(positions, (rate_len<4)? 4 : rate_len);
}
else speed = 0.0;
speeds.append(speed);
last_time = read_time;
prev_sent = last_sent;
}
}
