void
AP_InertialSensor::init_accel(void (*flash_leds_cb)(bool on))
{
    _init_accel(flash_leds_cb);

    // save calibration
    _save_parameters();
}
void
AP_InertialSensor::init_accel()
{
    _init_accel();

    // save calibration
    _save_parameters();
}
void
AP_InertialSensor::init_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on))
{
    _init_accel(delay_cb, flash_leds_cb);

    // save calibration
    _save_parameters();
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
                                        float &trim_roll,
                                        float &trim_pitch)
{
    Vector3f samples[6];
    Vector3f new_offsets;
    Vector3f new_scaling;
    Vector3f orig_offset;
    Vector3f orig_scale;

    // backup original offsets and scaling
    orig_offset = _accel_offset.get();
    orig_scale = _accel_scale.get();

    // clear accelerometer offsets and scaling
    _accel_offset = Vector3f(0,0,0);
    _accel_scale = Vector3f(1,1,1);

    // capture data from 6 positions
    for (int8_t i=0; i<6; i++) {
        const prog_char_t *msg;

        // display message to user
        switch ( i ) {
            case 0:
                msg = PSTR("level");
                break;
            case 1:
                msg = PSTR("on its LEFT side");
                break;
            case 2:
                msg = PSTR("on its RIGHT side");
                break;
            case 3:
                msg = PSTR("nose DOWN");
                break;
            case 4:
                msg = PSTR("nose UP");
                break;
            default:    // default added to avoid compiler warning
            case 5:
                msg = PSTR("on its BACK");
                break;
        }
        interact->printf_P(
                PSTR("Place APM %S and press any key.\n"), msg);

        // wait for user input
        interact->blocking_read();

        // clear out any existing samples from ins
        update();

        // wait until we have 32 samples
        while( num_samples_available() < 32 * SAMPLE_UNIT ) {
            hal.scheduler->delay(10);
        }

        // read samples from ins
        update();

        // capture sample
        samples[i] = get_accel();
    }

    // run the calibration routine
    bool success = _calibrate_accel(samples, new_offsets, new_scaling);

    interact->printf_P(PSTR("Offsets: %.2f %.2f %.2f\n"),
                       new_offsets.x, new_offsets.y, new_offsets.z);
    interact->printf_P(PSTR("Scaling: %.2f %.2f %.2f\n"),
                       new_scaling.x, new_scaling.y, new_scaling.z);

    if (success) {
        interact->printf_P(PSTR("Calibration successful\n"));

        // set and save calibration
        _accel_offset.set(new_offsets);
        _accel_scale.set(new_scaling);
        _save_parameters();

        // calculate the trims as well and pass back to caller
        _calculate_trim(samples[0], trim_roll, trim_pitch);

        return true;
    }

    interact->printf_P(PSTR("Calibration FAILED\n"));
    // restore original scaling and offsets
    _accel_offset.set(orig_offset);
    _accel_scale.set(orig_scale);
    return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(void (*flash_leds_cb)(bool on),
                            AP_InertialSensor_UserInteract* interact)
{
    Vector3f samples[6];
    Vector3f new_offsets;
    Vector3f new_scaling;
    Vector3f orig_offset;
    Vector3f orig_scale;

    // backup original offsets and scaling
    orig_offset = _accel_offset.get();
    orig_scale = _accel_scale.get();

    // clear accelerometer offsets and scaling
    _accel_offset = Vector3f(0,0,0);
    _accel_scale = Vector3f(1,1,1);

    // capture data from 6 positions
    for (int8_t i=0; i<6; i++) {
        const prog_char_t *msg;

        // display message to user
        switch ( i ) {
            case 0:
                msg = PSTR("level");
                break;
            case 1:
                msg = PSTR("on it's left side");
                break;
            case 2:
                msg = PSTR("on it's right side");
                break;
            case 3:
                msg = PSTR("nose down");
                break;
            case 4:
                msg = PSTR("nose up");
                break;
            default:    // default added to avoid compiler warning
            case 5:
                msg = PSTR("on it's back");
                break;
        }
        interact->printf_P(
                PSTR("Place APM %S and press any key.\n"), msg);

        // wait for user input
        interact->blocking_read();

        // clear out any existing samples from ins
        update();

        // wait until we have 32 samples
        while( num_samples_available() < 32 * SAMPLE_UNIT ) {
            hal.scheduler->delay(10);
        }

        // read samples from ins
        update();

        // capture sample
        samples[i] = get_accel();
    }

    // run the calibration routine
    if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
        interact->printf_P(PSTR("Calibration successful\n"));

        // set and save calibration
        _accel_offset.set(new_offsets);
        _accel_scale.set(new_scaling);
        _save_parameters();
        return true;
    }

    interact->printf_P(
            PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
             new_offsets.x, new_offsets.y, new_offsets.z,
             new_scaling.x, new_scaling.y, new_scaling.z);
    // restore original scaling and offsets
    _accel_offset.set(orig_offset);
    _accel_scale.set(orig_scale);
    return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
                                        float &trim_roll,
                                        float &trim_pitch)
{
    uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
    Vector3f samples[INS_MAX_INSTANCES][6];
    Vector3f new_offsets[INS_MAX_INSTANCES];
    Vector3f new_scaling[INS_MAX_INSTANCES];
    Vector3f orig_offset[INS_MAX_INSTANCES];
    Vector3f orig_scale[INS_MAX_INSTANCES];
    uint8_t num_ok = 0;

    for (uint8_t k=0; k<num_accels; k++) {
        // backup original offsets and scaling
        orig_offset[k] = _accel_offset[k].get();
        orig_scale[k]  = _accel_scale[k].get();

        // clear accelerometer offsets and scaling
        _accel_offset[k] = Vector3f(0,0,0);
        _accel_scale[k] = Vector3f(1,1,1);
    }

    // capture data from 6 positions
    for (uint8_t i=0; i<6; i++) {
        const prog_char_t *msg;

        // display message to user
        switch ( i ) {
            case 0:
                msg = PSTR("level");
                break;
            case 1:
                msg = PSTR("on its LEFT side");
                break;
            case 2:
                msg = PSTR("on its RIGHT side");
                break;
            case 3:
                msg = PSTR("nose DOWN");
                break;
            case 4:
                msg = PSTR("nose UP");
                break;
            default:    // default added to avoid compiler warning
            case 5:
                msg = PSTR("on its BACK");
                break;
        }
        interact->printf_P(
                PSTR("Place vehicle %S and press any key.\n"), msg);

        // wait for user input
        if (!interact->blocking_read()) {
            //No need to use interact->printf_P for an error, blocking_read does this when it fails
            goto failed;
        }

        // clear out any existing samples from ins
        update();

        // average 32 samples
        for (uint8_t k=0; k<num_accels; k++) {
            samples[k][i] = Vector3f();
        }
        uint8_t num_samples = 0;
        while (num_samples < 32) {
            wait_for_sample();
            // read samples from ins
            update();
            // capture sample
            for (uint8_t k=0; k<num_accels; k++) {
                samples[k][i] += get_accel(k);
            }
            hal.scheduler->delay(10);
            num_samples++;
        }
        for (uint8_t k=0; k<num_accels; k++) {
            samples[k][i] /= num_samples;
        }
    }

    // run the calibration routine
    for (uint8_t k=0; k<num_accels; k++) {
        bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k]);

        interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
                           (unsigned)k,
                           new_offsets[k].x, new_offsets[k].y, new_offsets[k].z);
        interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
                           (unsigned)k,
                           new_scaling[k].x, new_scaling[k].y, new_scaling[k].z);
        if (success) num_ok++;
    }

    if (num_ok == num_accels) {
        interact->printf_P(PSTR("Calibration successful\n"));

        for (uint8_t k=0; k<num_accels; k++) {
            // set and save calibration
            _accel_offset[k].set(new_offsets[k]);
            _accel_scale[k].set(new_scaling[k]);
        }
        _save_parameters();

        check_3D_calibration();

        // calculate the trims as well from primary accels and pass back to caller
        _calculate_trim(samples[0][0], trim_roll, trim_pitch);

        return true;
    }

failed:
    interact->printf_P(PSTR("Calibration FAILED\n"));
    // restore original scaling and offsets
    for (uint8_t k=0; k<num_accels; k++) {
        _accel_offset[k].set(orig_offset[k]);
        _accel_scale[k].set(orig_scale[k]);
    }
    return false;
}
// calibrate_accel - perform accelerometer calibration including providing user
// instructions and feedback Gauss-Newton accel calibration routines borrowed
// from Rolfe Schmidt blog post describing the method:
// http://chionophilous.wordpress.com/2011/10/24/accelerometer-calibration-iv-1-implementing-gauss-newton-on-an-atmega/
// original sketch available at
// http://rolfeschmidt.com/mathtools/skimetrics/adxl_gn_calibration.pde
bool AP_InertialSensor::calibrate_accel(AP_InertialSensor_UserInteract* interact,
                                        float &trim_roll,
                                        float &trim_pitch)
{
    uint8_t num_accels = min(get_accel_count(), INS_MAX_INSTANCES);
    Vector3f samples[INS_MAX_INSTANCES][6];
    Vector3f new_offsets[INS_MAX_INSTANCES];
    Vector3f new_scaling[INS_MAX_INSTANCES];
    Vector3f orig_offset[INS_MAX_INSTANCES];
    Vector3f orig_scale[INS_MAX_INSTANCES];
    uint8_t num_ok = 0;

    // exit immediately if calibration is already in progress
    if (_calibrating) {
        return false;
    }

    _calibrating = true;

    /*
      we do the accel calibration with no board rotation. This avoids
      having to rotate readings during the calibration
    */
    enum Rotation saved_orientation = _board_orientation;
    _board_orientation = ROTATION_NONE;

    for (uint8_t k=0; k<num_accels; k++) {
        // backup original offsets and scaling
        orig_offset[k] = _accel_offset[k].get();
        orig_scale[k]  = _accel_scale[k].get();

        // clear accelerometer offsets and scaling
        _accel_offset[k] = Vector3f(0,0,0);
        _accel_scale[k] = Vector3f(1,1,1);
    }

    memset(samples, 0, sizeof(samples));

    // capture data from 6 positions
    for (uint8_t i=0; i<6; i++) {
        const prog_char_t *msg;

        // display message to user
        switch ( i ) {
            case 0:
                msg = PSTR("level");
                break;
            case 1:
                msg = PSTR("on its LEFT side");
                break;
            case 2:
                msg = PSTR("on its RIGHT side");
                break;
            case 3:
                msg = PSTR("nose DOWN");
                break;
            case 4:
                msg = PSTR("nose UP");
                break;
            default:    // default added to avoid compiler warning
            case 5:
                msg = PSTR("on its BACK");
                break;
        }
        interact->printf_P(
                PSTR("Place vehicle %S and press any key.\n"), msg);

        // wait for user input
        if (!interact->blocking_read()) {
            //No need to use interact->printf_P for an error, blocking_read does this when it fails
            goto failed;
        }

        const uint8_t update_dt_milliseconds = (uint8_t)(1000.0f/get_sample_rate()+0.5f);

        // wait 100ms for ins filter to rise
        for (uint8_t k=0; k<100/update_dt_milliseconds; k++) {
            wait_for_sample();
            update();
            hal.scheduler->delay(update_dt_milliseconds);
        }

        uint32_t num_samples = 0;
        while (num_samples < 400/update_dt_milliseconds) {
            wait_for_sample();
            // read samples from ins
            update();
            // capture sample
            for (uint8_t k=0; k<num_accels; k++) {
                Vector3f samp;
                if(get_delta_velocity(k,samp) && _delta_velocity_dt[k] > 0) {
                    samp /= _delta_velocity_dt[k];
                } else {
                    samp = get_accel(k);
                }
                samples[k][i] += samp;
                if (!get_accel_health(k)) {
                    interact->printf_P(PSTR("accel[%u] not healthy"), (unsigned)k);
                    goto failed;
                }
            }
            hal.scheduler->delay(update_dt_milliseconds);
            num_samples++;
        }
        for (uint8_t k=0; k<num_accels; k++) {
            samples[k][i] /= num_samples;
        }
    }

    // run the calibration routine
    for (uint8_t k=0; k<num_accels; k++) {
        if (!_check_sample_range(samples[k], saved_orientation, interact)) {
            interact->printf_P(PSTR("Insufficient accel range"));
            continue;
        }

        bool success = _calibrate_accel(samples[k], new_offsets[k], new_scaling[k], saved_orientation);

        interact->printf_P(PSTR("Offsets[%u]: %.2f %.2f %.2f\n"),
                            (unsigned)k,
                           (double)new_offsets[k].x, (double)new_offsets[k].y, (double)new_offsets[k].z);
        interact->printf_P(PSTR("Scaling[%u]: %.2f %.2f %.2f\n"),
                           (unsigned)k,
                           (double)new_scaling[k].x, (double)new_scaling[k].y, (double)new_scaling[k].z);
        if (success) num_ok++;
    }

    if (num_ok == num_accels) {
        interact->printf_P(PSTR("Calibration successful\n"));

        for (uint8_t k=0; k<num_accels; k++) {
            // set and save calibration
            _accel_offset[k].set(new_offsets[k]);
            _accel_scale[k].set(new_scaling[k]);
        }
        for (uint8_t k=num_accels; k<INS_MAX_INSTANCES; k++) {
            // clear unused accelerometer's scaling and offsets
            _accel_offset[k] = Vector3f(0,0,0);
            _accel_scale[k] = Vector3f(0,0,0);
        }
        _save_parameters();

        /*
          calculate the trims as well from primary accels 
          We use the original board rotation for this sample
        */
        Vector3f level_sample = samples[0][0];
        level_sample.rotate(saved_orientation);

        _calculate_trim(level_sample, trim_roll, trim_pitch);

        _board_orientation = saved_orientation;

        _calibrating = false;
        return true;
    }

failed:
    interact->printf_P(PSTR("Calibration FAILED\n"));
    // restore original scaling and offsets
    for (uint8_t k=0; k<num_accels; k++) {
        _accel_offset[k].set(orig_offset[k]);
        _accel_scale[k].set(orig_scale[k]);
    }
    _board_orientation = saved_orientation;
    _calibrating = false;
    return false;
}
// perform accelerometer calibration including providing user instructions and feedback
bool AP_InertialSensor::calibrate_accel(void (*delay_cb)(unsigned long t), void (*flash_leds_cb)(bool on), void (*send_msg)(const prog_char_t *, ...))
{
    Vector3f samples[6];
    Vector3f new_offsets;
    Vector3f new_scaling;
    Vector3f orig_offset;
    Vector3f orig_scale;

    // backup original offsets and scaling
    orig_offset = _accel_offset.get();
    orig_scale = _accel_scale.get();

    // clear accelerometer offsets and scaling
    _accel_offset = Vector3f(0,0,0);
    _accel_scale = Vector3f(1,1,1);

    // capture data from 6 positions
    for (int8_t i=0; i<6; i++) {
        const prog_char_t *msg;

        // display message to user
        switch ( i ) {
            case 0:
                msg = PSTR("level");
                break;
            case 1:
                msg = PSTR("on it's left side");
                break;
            case 2:
                msg = PSTR("on it's right side");
                break;
            case 3:
                msg = PSTR("nose down");
                break;
            case 4:
                msg = PSTR("nose up");
                break;
            case 5:
                msg = PSTR("on it's back");
                break;
        }
        if (send_msg == NULL) {
            Serial.printf_P(PSTR("USER: Place APM %S and press any key.\n"), msg);
        }else{
            send_msg(PSTR("USER: Place APM %S and press any key.\n"), msg);
        }

        // wait for user input
        while( !Serial.available() ) {
            delay_cb(20);
        }
        // clear unput buffer
        while( Serial.available() ) {
            Serial.read();
        }

        // clear out any existing samples from ins
        update();

        // wait until we have 32 samples
        while( num_samples_available() < 32 * SAMPLE_UNIT ) {
            delay_cb(1);
        }

        // read samples from ins
        update();

        // capture sample
        samples[i] = get_accel();
    }

    // run the calibration routine
    if( _calibrate_accel(samples, new_offsets, new_scaling) ) {
        if (send_msg == NULL) {
            Serial.printf_P(PSTR("Calibration successful\n"));
        }else{
            send_msg(PSTR("Calibration successful\n"));
        }

        // set and save calibration
        _accel_offset.set(new_offsets);
        _accel_scale.set(new_scaling);
        _save_parameters();
        return true;
    }

    if (send_msg == NULL) {
        Serial.printf_P(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
                        new_offsets.x, new_offsets.y, new_offsets.z,
                        new_scaling.x, new_scaling.y, new_scaling.z);
    } else {
        send_msg(PSTR("Calibration failed (%.1f %.1f %.1f %.1f %.1f %.1f)\n"),
                 new_offsets.x, new_offsets.y, new_offsets.z,
                 new_scaling.x, new_scaling.y, new_scaling.z);
    }
    // restore original scaling and offsets
    _accel_offset.set(orig_offset);
    _accel_scale.set(orig_scale);
    return false;
}