void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) { // remember the sample rate _sample_rate = sample_rate; if (_gyro_count == 0 && _accel_count == 0) { // detect available backends. Only called once _detect_backends(); } _product_id = 0; // FIX // initialise accel scale if need be. This is needed as we can't // give non-zero default values for vectors in AP_Param for (uint8_t i=0; i<get_accel_count(); i++) { if (_accel_scale[i].get().is_zero()) { _accel_scale[i].set(Vector3f(1,1,1)); } } // remember whether we have 3D calibration so this can be used for // AHRS health check_3D_calibration(); if (WARM_START != style) { // do cold-start calibration for gyro only _init_gyro(); } switch (sample_rate) { case RATE_50HZ: _sample_period_usec = 20000; break; case RATE_100HZ: _sample_period_usec = 10000; break; case RATE_200HZ: _sample_period_usec = 5000; break; case RATE_400HZ: default: _sample_period_usec = 2500; break; } // establish the baseline time between samples _delta_time = 0; _next_sample_usec = 0; _last_sample_usec = 0; _have_sample = false; }
// Default init method // bool Compass::init() { if (_compass_count == 0) { // detect available backends. Only called once _detect_backends(); } if (_compass_count != 0) { // get initial health status hal.scheduler->delay(100); read(); } return true; }