static void icuwidthcb(ICUDriver *icup) {
last_len_received[0] = ((float)icuGetWidthX(icup) / ((float)TIMER_FREQ / 1000.0));
float len = last_len_received[0] - pulse_start;
const float len_set = (pulse_end - pulse_start);
if (len > len_set) {
if (len < (len_set * 1.2)) {
len = len_set;
} else {
// Too long pulse. Most likely something is wrong.
len = -1.0;
}
} else if (len < 0.0) {
if ((len + pulse_start) > (pulse_start * 0.8)) {
len = 0.0;
} else {
// Too short pulse. Most likely something is wrong.
len = -1.0;
}
}
if (len >= 0.0) {
if (use_median_filter) {
float c = (len * 2.0 - len_set) / len_set;
static float c1 = 0.5;
static float c2 = 0.5;
float med = utils_middle_of_3(c, c1, c2);
c2 = c1;
c1 = c;
servo_pos[0] = med;
} else {
servo_pos[0] = (len * 2.0 - len_set) / len_set;
}
last_update_time = chVTGetSystemTime();
if (done_func) {
done_func();
}
}
}
static void icuwidthcb(ICUDriver *icup) {
palSetPad(GPIOI, GPIOI_ARD_D13);
last_width = icuGetWidthX(icup);
}
static void icuwidthcb(ICUDriver *icup) {
palSetPad(GPIOE, GPIOE_LED9_BLUE);
last_width = icuGetWidthX(icup);
}
static void icuwidthcb(ICUDriver *icup) {
palSetPad(GPIOB, GPIOB_LED3);
last_width = icuGetWidthX(icup);
}
static void echocb(ICUDriver *icup){
echo = icuGetWidthX(icup);
}
