/* calculate the trim_roll and trim_pitch. This is used for redoing the trim without needing a full accel cal */ bool AP_InertialSensor::calibrate_trim(float &trim_roll, float &trim_pitch) { Vector3f level_sample; // exit immediately if calibration is already in progress if (_calibrating) { return false; } _calibrating = true; 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 Vector3f samp; samp = get_accel(0); level_sample += samp; if (!get_accel_health(0)) { goto failed; } hal.scheduler->delay(update_dt_milliseconds); num_samples++; } level_sample /= num_samples; if (!_calculate_trim(level_sample, trim_roll, trim_pitch)) { goto failed; } _calibrating = false; return true; failed: _calibrating = false; return false; }
bool AP_InertialSensor_VRBRAIN::update(void) { if (!wait_for_sample(100)) { return false; } // get the latest sample from the sensor drivers _get_sample(); for (uint8_t k=0; k<_num_accel_instances; k++) { _previous_accel[k] = _accel[k]; _accel[k] = _accel_in[k]; _accel[k].rotate(_board_orientation); _accel[k].x *= _accel_scale[k].get().x; _accel[k].y *= _accel_scale[k].get().y; _accel[k].z *= _accel_scale[k].get().z; _accel[k] -= _accel_offset[k]; } for (uint8_t k=0; k<_num_gyro_instances; k++) { _gyro[k] = _gyro_in[k]; _gyro[k].rotate(_board_orientation); _gyro[k] -= _gyro_offset[k]; } if (_last_filter_hz != _mpu6000_filter) { _set_filter_frequency(_mpu6000_filter); _last_filter_hz = _mpu6000_filter; } _have_sample_available = false; return true; }
bool AP_InertialSensor_L3GD20::update( void ) { // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); _num_samples = _sum_count; _gyro_sum.zero(); _sum_count = 0; hal.scheduler->resume_timer_procs(); _gyro[0].rotate(_board_orientation); _gyro[0] *= _gyro_scale / _num_samples; _gyro[0] -= _gyro_offset[0]; // if (_last_filter_hz != _L3GD20_filter) { // if (_spi_sem->take(10)) { // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // _set_filter_register(_L3GD20_filter, 0); // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); // _error_count = 0; // _spi_sem->give(); // } // } return true; }
/* update gyro and accel values from backends */ void AP_InertialSensor::update(void) { // during initialisation update() may be called without // wait_for_sample(), and a wait is implied wait_for_sample(); if (!_hil_mode) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { // mark sensors unhealthy and let update() in each backend // mark them healthy via _rotate_and_offset_gyro() and // _rotate_and_offset_accel() _gyro_healthy[i] = false; _accel_healthy[i] = false; } for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } // adjust health status if a sensor has a non-zero error count // but another sensor doesn't. bool have_zero_accel_error_count = false; bool have_zero_gyro_error_count = false; for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i] && _accel_error_count[i] == 0) { have_zero_accel_error_count = true; } if (_gyro_healthy[i] && _gyro_error_count[i] == 0) { have_zero_gyro_error_count = true; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i] && _gyro_error_count[i] != 0 && have_zero_gyro_error_count) { // we prefer not to use a gyro that has had errors _gyro_healthy[i] = false; } if (_accel_healthy[i] && _accel_error_count[i] != 0 && have_zero_accel_error_count) { // we prefer not to use a accel that has had errors _accel_healthy[i] = false; } } // set primary to first healthy accel and gyro for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i]) { _primary_gyro = i; break; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i]) { _primary_accel = i; break; } } } _have_sample = false; }
bool AP_InertialSensor_MPU6000::update( void ) { int32_t sum[7]; float count_scale; Vector3f accel_scale = _accel_scale.get(); // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); /** ATOMIC SECTION w/r/t TIMER PROCESS */ { for (int i=0; i<7; i++) { sum[i] = _sum[i]; _sum[i] = 0; } _num_samples = _count; _count = 0; } hal.scheduler->resume_timer_procs(); count_scale = 1.0f / _num_samples; _gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]], _gyro_data_sign[1] * sum[_gyro_data_index[1]], _gyro_data_sign[2] * sum[_gyro_data_index[2]]); _gyro.rotate(_board_orientation); _gyro *= _gyro_scale * count_scale; _gyro -= _gyro_offset; _accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]], _accel_data_sign[1] * sum[_accel_data_index[1]], _accel_data_sign[2] * sum[_accel_data_index[2]]); _accel.rotate(_board_orientation); _accel *= count_scale * MPU6000_ACCEL_SCALE_1G; _accel.x *= accel_scale.x; _accel.y *= accel_scale.y; _accel.z *= accel_scale.z; _accel -= _accel_offset; _temp = _temp_to_celsius(sum[_temp_data_index] * count_scale); if (_last_filter_hz != _mpu6000_filter) { if (_spi_sem->take(10)) { _set_filter_register(_mpu6000_filter, 0); _spi_sem->give(); } } return true; }
bool AP_InertialSensor_MPU9250::update( void ) { // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } _previous_accel[0] = _accel[0]; // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); _num_samples = _sum_count; _accel_sum.zero(); _gyro_sum.zero(); _sum_count = 0; hal.scheduler->resume_timer_procs(); _gyro[0].rotate(_board_orientation); _gyro[0] *= _gyro_scale / _num_samples; _gyro[0] -= _gyro_offset[0]; _accel[0].rotate(_board_orientation); _accel[0] *= MPU9250_ACCEL_SCALE_1G / _num_samples; // rotate for bbone default _accel[0].rotate(ROTATION_ROLL_180_YAW_90); _gyro[0].rotate(ROTATION_ROLL_180_YAW_90); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF // PXF has an additional YAW 180 _accel[0].rotate(ROTATION_YAW_180); _gyro[0].rotate(ROTATION_YAW_180); #endif Vector3f accel_scale = _accel_scale[0].get(); _accel[0].x *= accel_scale.x; _accel[0].y *= accel_scale.y; _accel[0].z *= accel_scale.z; _accel[0] -= _accel_offset[0]; if (_last_filter_hz != _mpu6000_filter) { if (_spi_sem->take(10)) { _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _set_filter_register(_mpu6000_filter, 0); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _error_count = 0; _spi_sem->give(); } } return true; }
// TODO finish bool AP_InertialSensor_LSM9DS0::_update( void ) { // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } imu._previous_accel[0] = imu._accel[0]; // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); imu._gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); imu._accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); // _mag[0] = Vector3f(_mag_sum.x, _mag_sum.y, _mag_sum.z); // TODO divide num_samples _num_samples_g = _sum_count_g; _num_samples_xm = _sum_count_xm; _accel_sum.zero(); _gyro_sum.zero(); _sum_count_g = 0; _sum_count_xm = 0; hal.scheduler->resume_timer_procs(); imu._gyro[0].rotate(imu._board_orientation); imu._gyro[0] *= _gRes / _num_samples_g; imu._gyro[0] -= imu._gyro_offset[0]; imu._accel[0].rotate(imu._board_orientation); imu._accel[0] *= _aRes / _num_samples_xm; Vector3f accel_scaling = imu._accel_scale[0].get(); imu._accel[0].x *= accel_scaling.x; imu._accel[0].y *= accel_scaling.y; imu._accel[0].z *= accel_scaling.z; imu._accel[0] -= imu._accel_offset[0]; // // Configure mag // _mag[0] *= _mRes / _num_samples_xm; // if (_last_filter_hz != _mpu6000_filter) { // if (_spi_sem->take(10)) { // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // _set_filter_register(_mpu6000_filter, 0); // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); // _error_count = 0; // _spi_sem->give(); // } // } return true; }
bool AP_InertialSensor_ITG3200::update( void ) { int32_t sum[7]; float count_scale; Vector3f accel_scale = _accel_scale.get(); // wait for at least 1 sample wait_for_sample(); // disable interrupts for mininum time hal.scheduler->suspend_timer_procs(); /** ATOMIC SECTION w/r/t TIMER PROCESS */ { for (int i=0; i<7; i++) { sum[i] = _sum[i]; _sum[i] = 0; } _num_samples = _count; _count = 0; } hal.scheduler->resume_timer_procs(); count_scale = 1.0f / _num_samples; _gyro = Vector3f(_gyro_data_sign[0] * sum[_gyro_data_index[0]], _gyro_data_sign[1] * sum[_gyro_data_index[1]], _gyro_data_sign[2] * sum[_gyro_data_index[2]]); _gyro.rotate(_board_orientation); _gyro *= _gyro_scale * count_scale; _gyro -= _gyro_offset; _accel = Vector3f(_accel_data_sign[0] * sum[_accel_data_index[0]], _accel_data_sign[1] * sum[_accel_data_index[1]], _accel_data_sign[2] * sum[_accel_data_index[2]]); _accel.rotate(_board_orientation); _accel *= count_scale * _accel_scale_1G; _accel.x *= accel_scale.x; _accel.y *= accel_scale.y; _accel.z *= accel_scale.z; _accel -= _accel_offset; _temp = _temp_to_celsius(sum[_temp_data_index] * count_scale); return true; }
bool AP_InertialSensor_MPU6000_Ext::update( void ) { // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } _previous_accel[0] = _accel[0]; // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); _gyro[0] = Vector3f(_gyro_sum.x, _gyro_sum.y, _gyro_sum.z); _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); _num_samples = _sum_count; _accel_sum.zero(); _gyro_sum.zero(); _temp[0] = _temp_sum; _temp_sum = 0; _sum_count = 0; hal.scheduler->resume_timer_procs(); _gyro[0].rotate(_board_orientation); _gyro[0] *= _gyro_scale / _num_samples; _gyro[0] -= _gyro_offset[0]; _accel[0].rotate(_board_orientation); _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; Vector3f accel_scale = _accel_scale[0].get(); _accel[0].x *= accel_scale.x; _accel[0].y *= accel_scale.y; _accel[0].z *= accel_scale.z; _accel[0] -= _accel_offset[0]; _temp[0] = _temp_to_celsius(_temp[0] / _num_samples); if (_last_filter_hz != _mpu6000_filter) { if (_spi_sem->take(10)) { _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); _set_filter_register(_mpu6000_filter, 0); _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); _error_count = 0; _spi_sem->give(); } } return true; }
bool AP_InertialSensor_VRBRAIN::update( void ) { // wait for at least 1 sample if (!wait_for_sample(100)) { return false; } _previous_accel[0] = _accel[0]; // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); _accel[0] = _accel_filtered; _accel_samples = 0; _gyro[0] = _gyro_filtered; _gyro_samples = 0; _temp[0] = _temp_filtered; hal.scheduler->resume_timer_procs(); _accel[0].rotate(_board_orientation); _accel[0] *= MPU6000_ACCEL_SCALE_1G; Vector3f accel_scale = _accel_scale[0].get(); _accel[0].x *= accel_scale.x; _accel[0].y *= accel_scale.y; _accel[0].z *= accel_scale.z; _accel[0] -= _accel_offset[0]; _gyro[0].rotate(_board_orientation); _gyro[0] *= _gyro_scale; _gyro[0] -= _gyro_offset[0]; _temp[0] = _temp_to_celsius(_temp[0]); if (_last_filter_hz != _mpu6000_filter) { _set_filter_frequency(_mpu6000_filter); _last_filter_hz = _mpu6000_filter; } return true; }
/* update the accel and gyro vectors */ bool AP_InertialSensor_MPU9250::update( void ) { if (!wait_for_sample(1000)) { return false; } _previous_accel[0] = _accel[0]; // pull the data from the timer shared data buffer uint8_t idx = _shared_data_idx; _gyro[0] = _shared_data[idx]._gyro_filtered; _accel[0] = _shared_data[idx]._accel_filtered; _gyro[0].rotate(_board_orientation); _gyro[0] *= GYRO_SCALE; _gyro[0] -= _gyro_offset[0]; _accel[0].rotate(_board_orientation); _accel[0] *= MPU9250_ACCEL_SCALE_1G; // rotate for bbone default _accel[0].rotate(ROTATION_ROLL_180_YAW_90); _gyro[0].rotate(ROTATION_ROLL_180_YAW_90); #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_PXF // PXF has an additional YAW 180 _accel[0].rotate(ROTATION_YAW_180); _gyro[0].rotate(ROTATION_YAW_180); #endif Vector3f accel_scale = _accel_scale[0].get(); _accel[0].x *= accel_scale.x; _accel[0].y *= accel_scale.y; _accel[0].z *= accel_scale.z; _accel[0] -= _accel_offset[0]; if (_last_filter_hz != _mpu6000_filter) { _set_filter(_mpu6000_filter); _last_filter_hz = _mpu6000_filter; } _have_sample_available = false; return true; }
bool AP_InertialSensor_LSM303D::update( void ) { // wait for at least 1 sample if (!wait_for_sample(1000)) { return false; } // disable timer procs for mininum time hal.scheduler->suspend_timer_procs(); _accel[0] = Vector3f(_accel_sum.x, _accel_sum.y, _accel_sum.z); // _mag[0] = Vector3f(_mag_sum.x, _mag_sum.y, _mag_sum.z); _num_samples = _sum_count; _accel_sum.zero(); _mag_sum.zero(); _sum_count = 0; hal.scheduler->resume_timer_procs(); _accel[0].rotate(_board_orientation); // TODO change this for the corresponding value // _accel[0] *= MPU6000_ACCEL_SCALE_1G / _num_samples; // Vector3f accel_scale = _accel_scale[0].get(); // _accel[0].x *= accel_scale.x; // _accel[0].y *= accel_scale.y; // _accel[0].z *= accel_scale.z; // _accel[0] -= _accel_offset[0]; // TODO similarly put mag values in _mag and scale them // if (_last_filter_hz != _LSM303D_filter) { // if (_spi_sem->take(10)) { // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_LOW); // _set_filter_register(_LSM303D_filter, 0); // _spi->set_bus_speed(AP_HAL::SPIDeviceDriver::SPI_SPEED_HIGH); // _error_count = 0; // _spi_sem->give(); // } // } return true; }
// 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(); // average 32 samples samples[i] = Vector3f(); uint8_t num_samples = 0; while (num_samples < 32) { if (!wait_for_sample(1000)) { interact->printf_P(PSTR("Failed to get INS sample\n")); return false; } // read samples from ins update(); // capture sample samples[i] += get_accel(); hal.scheduler->delay(10); num_samples++; } samples[i] /= num_samples; } // 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; }
/* update gyro and accel values from backends */ void AP_InertialSensor::update(void) { // during initialisation update() may be called without // wait_for_sample(), and a wait is implied wait_for_sample(); if (!_hil_mode) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { // mark sensors unhealthy and let update() in each backend // mark them healthy via _publish_gyro() and // _publish_accel() _gyro_healthy[i] = false; _accel_healthy[i] = false; _delta_velocity_valid[i] = false; _delta_angle_valid[i] = false; } for (uint8_t i=0; i<_backend_count; i++) { _backends[i]->update(); } // clear accumulators for (uint8_t i = 0; i < INS_MAX_INSTANCES; i++) { _delta_velocity_acc[i].zero(); _delta_velocity_acc_dt[i] = 0; _delta_angle_acc[i].zero(); _delta_angle_acc_dt[i] = 0; } if (!_startup_error_counts_set) { for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { _accel_startup_error_count[i] = _accel_error_count[i]; _gyro_startup_error_count[i] = _gyro_error_count[i]; } if (_startup_ms == 0) { _startup_ms = AP_HAL::millis(); } else if (AP_HAL::millis()-_startup_ms > 2000) { _startup_error_counts_set = true; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_error_count[i] < _accel_startup_error_count[i]) { _accel_startup_error_count[i] = _accel_error_count[i]; } if (_gyro_error_count[i] < _gyro_startup_error_count[i]) { _gyro_startup_error_count[i] = _gyro_error_count[i]; } } // adjust health status if a sensor has a non-zero error count // but another sensor doesn't. bool have_zero_accel_error_count = false; bool have_zero_gyro_error_count = false; for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i] && _accel_error_count[i] <= _accel_startup_error_count[i]) { have_zero_accel_error_count = true; } if (_gyro_healthy[i] && _gyro_error_count[i] <= _gyro_startup_error_count[i]) { have_zero_gyro_error_count = true; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i] && _gyro_error_count[i] > _gyro_startup_error_count[i] && have_zero_gyro_error_count) { // we prefer not to use a gyro that has had errors _gyro_healthy[i] = false; } if (_accel_healthy[i] && _accel_error_count[i] > _accel_startup_error_count[i] && have_zero_accel_error_count) { // we prefer not to use a accel that has had errors _accel_healthy[i] = false; } } // set primary to first healthy accel and gyro for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_gyro_healthy[i] && _use[i]) { _primary_gyro = i; break; } } for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) { if (_accel_healthy[i] && _use[i]) { _primary_accel = i; break; } } } _have_sample = 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; }