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