void AP_InertialSensor::init(uint16_t sample_rate) { // remember the sample rate _sample_rate = sample_rate; _loop_delta_t = 1.0f / sample_rate; if (_gyro_count == 0 && _accel_count == 0) { _start_backends(); } // 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)); } } // calibrate gyros unless gyro calibration has been disabled if (gyro_calibration_timing() != GYRO_CAL_NEVER) { _init_gyro(); } _sample_period_usec = 1000*1000UL / _sample_rate; // establish the baseline time between samples _delta_time = 0; _next_sample_usec = 0; _last_sample_usec = 0; _have_sample = false; }
void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), AP_PeriodicProcess * scheduler, FastSerial * serial) { _serial = serial; _product_id = _init_sensor(scheduler, sample_rate); // check scaling Vector3f accel_scale = _accel_scale.get(); if( accel_scale.x == 0 && accel_scale.y == 0 && accel_scale.z == 0 ) { accel_scale.x = accel_scale.y = accel_scale.z = 1.0; _accel_scale.set(accel_scale); } if (WARM_START != style) { if (_serial) _serial->printf_P(PSTR("Init Cold start")); // //TEO // //per comodità inserisco calibrazione accel // _init_accel(delay_cb, flash_leds_cb); // do cold-start calibration for gyro only _init_gyro(delay_cb, flash_leds_cb); } }
void AP_IMU_INS::init( Start_style style, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)//, /*AP_PeriodicProcess * scheduler*/ ) { //_product_id = _ins->init(scheduler); // if we are warm-starting, load the calibration data from EEPROM and go // if (WARM_START == style) { //_sensor_cal.load(); eeprom.read(_sensor_cal, _sensor_cal_rom, sizeof(_sensor_cal)); //eeprom_read_block(_sensor_cal_eeprom, _sensor_cal, sizeof(_sensor_cal)); } else { // do cold-start calibration for both accel and gyro // read accel cal eeprom.read(_sensor_cal, _sensor_cal_rom, sizeof(_sensor_cal)); _init_gyro(delay_cb, flash_leds_cb); // save calibration //_sensor_cal.save(); save(); } }
void AP_InertialSensor::init_gyro() { _init_gyro(); // save calibration _save_parameters(); }
void AP_InertialSensor::init_gyro(void (*flash_leds_cb)(bool on)) { _init_gyro(flash_leds_cb); // save calibration _save_parameters(); }
void AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { _init_gyro(delay_cb, flash_leds_cb); //_sensor_cal.save(); save(); FLASH_LEDS(false); }
void AP_InertialSensor::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { _init_gyro(delay_cb, flash_leds_cb); // save calibration _save_parameters(); }
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; }
void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) { _product_id = _init_sensor(sample_rate); // check scaling Vector3f accel_scale = _accel_scale.get(); if( accel_scale.x == 0 && accel_scale.y == 0 && accel_scale.z == 0 ) { accel_scale.x = accel_scale.y = accel_scale.z = 1.0; _accel_scale.set(accel_scale); } if (WARM_START != style) { // do cold-start calibration for gyro only _init_gyro(); } }
void AP_InertialSensor::init( Start_style style, Sample_rate sample_rate) { _product_id = _init_sensor(sample_rate); // check scaling 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)); } } if (WARM_START != style) { // do cold-start calibration for gyro only _init_gyro(); } }
void AP_InertialSensor::init( Start_style style, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), AP_PeriodicProcess * scheduler ) { _product_id = _init_sensor(scheduler); // check scaling Vector3f accel_scale = _accel_scale.get(); if( accel_scale.x == 0 && accel_scale.y == 0 && accel_scale.z == 0 ) { accel_scale.x = accel_scale.y = accel_scale.z = 1.0; _accel_scale.set(accel_scale); } if (WARM_START != style) { // do cold-start calibration for gyro only _init_gyro(delay_cb, flash_leds_cb); } }
void AP_IMU_INS::init( Start_style style, void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), AP_PeriodicProcess * scheduler ) { _product_id = _ins->init(scheduler); // if we are warm-starting, load the calibration data from EEPROM and go // if (WARM_START == style) { _sensor_cal.load(); } else { // do cold-start calibration for both accel and gyro _init_gyro(delay_cb, flash_leds_cb); // save calibration _sensor_cal.save(); } }
void AP_IMU_INS::init_gyro(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on)) { _init_gyro(delay_cb, flash_leds_cb); _sensor_cal.save(); }