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); } }