static uint16_t wheel_predict(
odometry_wheel_t *wheel,
const uint32_t time_now)
{
if (wheel->samples[2].timestamp != time_now) {
float acc, vel, pos, tau1, tau2, dt1, dt2, dt;
int16_t dy1, dy2;
uint16_t prediction;
// Fit a parabola to the wheel's motion
dt1 = timestamp_duration_s(wheel->samples[0].timestamp,
wheel->samples[1].timestamp);
dt2 = timestamp_duration_s(wheel->samples[0].timestamp,
wheel->samples[2].timestamp);
dy1 = (int16_t) (wheel->samples[1].value - wheel->samples[0].value);
dy2 = (int16_t) (wheel->samples[2].value - wheel->samples[0].value);
// Avoid division by zero
if (dt1 <= MINIMUM_DELTA_T) {
dt2 = MINIMUM_DELTA_T;
}
if (dt2 <= MINIMUM_DELTA_T) {
dt2 = MINIMUM_DELTA_T;
}
tau1 = dy1 / (- dt1 * dt1 + dt1 * dt2);
tau2 = dy2 / (- dt2 * dt2 + dt1 * dt2);
acc = - tau1 - tau2;
vel = tau1 * dt2 + tau2 * dt1;
pos = wheel->samples[0].value;
// Predict using fitted parabola
dt = timestamp_duration_s(wheel->samples[0].timestamp, time_now);
prediction = pos + (int16_t) (vel * dt + acc * dt * dt);
return prediction;
} else {
return wheel->samples[2].value;
}
}
void odometry_base_update(
odometry_differential_base_t *robot,
const odometry_encoder_sample_t right_wheel_sample,
const odometry_encoder_sample_t left_wheel_sample)
{
wheel_update(&robot->right_wheel, right_wheel_sample);
wheel_update(&robot->left_wheel, left_wheel_sample);
// Choose wheel with most recent timestamp as reference
uint32_t time_now;
if (0.0f >= timestamp_duration_s(right_wheel_sample.timestamp,
left_wheel_sample.timestamp)) {
time_now = right_wheel_sample.timestamp;
} else {
time_now = left_wheel_sample.timestamp;
}
// Wheel prediction
float delta_right_wheel = wheel_get_delta_meter(&robot->right_wheel, time_now);
float delta_left_wheel = wheel_get_delta_meter(&robot->left_wheel, time_now);
// Base prediction
float dt = timestamp_duration_s(robot->time_last_estim, time_now);
float delta_fwd = 0.5f * (delta_right_wheel + delta_left_wheel);
float delta_rot = (delta_right_wheel - delta_left_wheel) / robot->wheelbase;
float cos_theta = cosf(robot->pose.theta + 0.5f * delta_rot);
float sin_theta = sinf(robot->pose.theta + 0.5f * delta_rot);
if (dt >= MINIMUM_DELTA_T) {
robot->velocity.x = delta_fwd * cos_theta / dt;
robot->velocity.y = delta_fwd * sin_theta / dt;
robot->velocity.omega = delta_rot / dt;
}
robot->pose.x += delta_fwd * cos_theta;
robot->pose.y += delta_fwd * sin_theta;
robot->pose.theta += delta_rot;
robot->time_last_estim = time_now;
}
bool rate_limiter_should_run(rate_limiter_t *rl)
{
bool should_run = true;
timestamp_t now = timestamp_get();
if (timestamp_duration_s(rl->last_update, now) >= 1.0f/parameter_scalar_read(&rl->rate)) {
should_run = true;
rl->last_update = now;
} else {
should_run = false;
}
return should_run;
}
