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