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