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