static int ir_divider(void)
{
if (g_state == st_idle)
return 0;
if (g_state == st_started)
return 1;
if (g_state == st_setup || is_calibrating())
return 10;
return WD_HZ;
}
__interrupt void watchdog_timer(void)
{
// Routine maintenance tasks
int ir_div = ir_divider();
int r = wc_update(&g_wc);
if (r && g_state == st_started) {
display_set_dp(1);
display_bin(g_wc.d);
if (r > WC_DIGITS)
g_timeout = 1;
}
display_refresh();
if (is_calibrating()) {
P1OUT |= CALIB_LED;
} else {
P1OUT &= ~CALIB_LED;
}
if (g_no_ir) {
g_no_ir = 0;
++g_no_ir_gen;
if (is_calibrating())
beep(ir_div * 2);
}
if (g_beep) {
if (g_beep > 0)
--g_beep;
P1OUT |= BEEP_BIT;
} else {
P1OUT &= ~BEEP_BIT;
}
if (ir_div) {
if (++g_ir_timer >= ir_div) {
g_ir_timer -= ir_div;
g_ir_burst = IR_BURST_PULSES * 2;
}
}
__low_power_mode_off_on_exit();
}
bool
Compass::start_calibration(uint8_t i, bool retry, bool autosave, float delay)
{
if (healthy(i)) {
if (!is_calibrating() && delay > 0.5f) {
AP_Notify::events.initiated_compass_cal = 1;
}
if (i == get_primary()) {
_calibrator[i].set_tolerance(8);
} else {
_calibrator[i].set_tolerance(16);
}
_calibrator[i].start(retry, autosave, delay);
return true;
} else {
return false;
}
}
bool
Compass::accept_calibration(uint8_t i)
{
CompassCalibrator& cal = _calibrator[i];
uint8_t cal_status = cal.get_status();
if (cal_status == COMPASS_CAL_SUCCESS) {
Vector3f ofs, diag, offdiag;
cal.get_calibration(ofs, diag, offdiag);
cal.clear();
set_and_save_offsets(i, ofs);
set_and_save_diagonals(i,diag);
set_and_save_offdiagonals(i,offdiag);
if (!is_calibrating()) {
AP_Notify::events.compass_cal_saved = 1;
}
return true;
} else {
return false;
}
}
