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;
}
示例#2
0
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);
    }
}
示例#3
0
文件: AP_IMU_INS.cpp 项目: cuav/MAAT
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();
    }
}
示例#4
0
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();
}
示例#6
0
文件: AP_IMU_INS.cpp 项目: cuav/MAAT
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);
}
示例#7
0
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;
}
示例#9
0
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();
    }
}
示例#13
0
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();
}