void calibrate(const ComType com, const Calibrate *data) {
__disable_irq();
PIN_ENABLE.pio->PIO_SODR = PIN_ENABLE.mask;
uint32_t tick_sum = 0;
uint16_t tick_last = 0;
for(uint16_t freq = 0; freq <= FREQUENCY_VALUE_SUM_MAX; freq+=8) {
set_frequency(freq);
SLEEP_MS(1);
while(!(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask)) {
__NOP();
}
while(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask) {
__NOP();
}
tick_last = SysTick->VAL;
SLEEP_US(1);
while(!(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask)) {
uint16_t tick_next = SysTick->VAL;
if(tick_last < tick_next) {
tick_sum += FEEDBACK_TICK_MAX - tick_next + tick_last;
} else {
tick_sum += tick_last - tick_next;
}
tick_last = tick_next;
}
SLEEP_US(1);
do {
uint16_t tick_next = SysTick->VAL;
if(tick_last < tick_next) {
tick_sum += FEEDBACK_TICK_MAX - tick_next + tick_last;
} else {
tick_sum += tick_last - tick_next;
}
tick_last = tick_next;
} while(PIN_FEEDBACK.pio->PIO_PDSR & PIN_FEEDBACK.mask);
uint16_t real_freq = (BOARD_MCK+tick_sum/2)/tick_sum;
BC->frequency_match[freq/8] = real_freq;
tick_sum = 0;
}
PIN_ENABLE.pio->PIO_CODR = PIN_ENABLE.mask;
save_calibration();
__enable_irq();
CalibrateReturn cr;
cr.header = data->header;
cr.header.length = sizeof(CalibrateReturn);
cr.calibration = true;
BA->send_blocking_with_timeout(&cr, sizeof(CalibrateReturn), com);
}
void
Compass::set_and_save_calibration(uint8_t i, const Vector3f &offsets, const float &scalers_XY, const float &scalers_XZ)
{
// sanity check compass instance provided
if (i < COMPASS_MAX_INSTANCES) {
_offset[i].set(offsets);
_scale_XY[i].set(scalers_XY);
_scale_XZ[i].set(scalers_XZ);
save_calibration(i);
}
}
