// Read Sensor data
bool AP_Compass_HMC5843::read()
{
if (!healthy && !re_initialise()) {
return false;
}
if (!read_raw()) {
return false;
}
mag_x *= calibration[0];
mag_y *= calibration[1];
mag_z *= calibration[2];
last_update = millis(); // record time of update
// rotate and offset the magnetometer values
// XXX this could well be done in common code...
Vector3f rot_mag = _orientation_matrix.get() * Vector3f(mag_x,mag_y,mag_z);
rot_mag = rot_mag + _offset.get();
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
}
// Read Sensor data
bool AP_Compass_HMC5843::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
}
if (!healthy) {
if (hal.scheduler->millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
return false;
}
}
if (_accum_count == 0) {
accumulate();
if (!healthy || _accum_count == 0) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
return false;
}
}
mag_x = _mag_x_accum * calibration[0] / _accum_count;
mag_y = _mag_y_accum * calibration[1] / _accum_count;
mag_z = _mag_z_accum * calibration[2] / _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
rot_mag.rotate(_orientation);
rot_mag += _offset.get();
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
}
// Read Sensor data
void AP_Compass_HMC5843_Pirates::_update(uint32_t tnow)
{
if (tnow - _compass_timer < 13500) {
return; // wait for more than 13.5ms, 75Hz
}
_compass_timer = tnow;
if (!_updated) {
if (!healthy && !re_initialise()) {
return;
}
_updated = read_raw();
}
}
// Read Sensor data
void AP_Compass_HMC5843::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return;
}
if (_retry_time != 0) {
if (hal.scheduler->millis() < _retry_time) {
return;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
_bus->set_high_speed(false);
return;
}
}
if (_accum_count == 0) {
accumulate();
if (_retry_time != 0) {
_bus->set_high_speed(false);
return;
}
}
Vector3f field(_mag_x_accum * _scaling[0],
_mag_y_accum * _scaling[1],
_mag_z_accum * _scaling[2]);
field /= _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
// rotate to the desired orientation
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
field.rotate(ROTATION_YAW_90);
}
publish_field(field, _compass_instance);
_retry_time = 0;
}
// Read Sensor data
bool AP_Compass_HMC5843::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
}
if (!healthy) {
if (hal.scheduler->millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
return false;
}
}
if (_accum_count == 0) {
accumulate();
if (!healthy || _accum_count == 0) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c->setHighSpeed(false);
return false;
}
}
mag_x = _mag_x_accum * calibration[0] / _accum_count;
mag_y = _mag_y_accum * calibration[1] / _accum_count;
mag_z = _mag_z_accum * calibration[2] / _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
Vector3f rot_mag = Vector3f(mag_x,mag_y,mag_z);
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
rot_mag.rotate(ROTATION_YAW_90);
}
// apply default board orientation for this compass type. This is
// a noop on most boards
rot_mag.rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
rot_mag.rotate((enum Rotation)_orientation.get());
// add in board orientation from AHRS
rot_mag.rotate(_board_orientation);
rot_mag += _offset.get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset = _motor_compensation.get() * _thr_or_curr;
rot_mag += _motor_offset;
}else{
_motor_offset.x = 0;
_motor_offset.y = 0;
_motor_offset.z = 0;
}
mag_x = rot_mag.x;
mag_y = rot_mag.y;
mag_z = rot_mag.z;
healthy = true;
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
hal.scheduler->delay(10);
_i2c_sem = hal.i2c->get_semaphore();
if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
}
// determine if we are using 5843 or 5883L
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
!read_register(ConfigRegA, &_base_config)) {
healthy = false;
_i2c_sem->give();
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
_i2c_sem->give();
return false;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 20 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
hal.scheduler->delay(50);
// set gains
if (!write_register(ConfigRegB, calibration_gain) ||
!write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
hal.scheduler->delay(50);
if (!read_raw())
continue; // we didn't read valid values
hal.scheduler->delay(10);
float cal[3];
cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabsf(expected_yz / (float)_mag_z);
if (cal[0] > 0.7f && cal[0] < 1.3f &&
cal[1] > 0.7f && cal[1] < 1.3f &&
cal[2] > 0.7f && cal[2] < 1.3f) {
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0
/* useful for debugging */
Serial.print("mag_x: ");
Serial.print(_mag_x);
Serial.print(" mag_y: ");
Serial.print(_mag_y);
Serial.print(" mag_z: ");
Serial.println(_mag_z);
Serial.print("CalX: ");
Serial.print(calibration[0]/good_count);
Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count);
#endif
}
if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
_i2c_sem->give();
return false;
}
_i2c_sem->give();
_initialised = true;
// perform an initial read
healthy = true;
read();
return success;
}
// Public Methods //////////////////////////////////////////////////////////////
void AP_Compass_HMC5843_Pirates::init_hardware()
{
int numAttempts = 0, good_count = 0;
bool success = false;
byte calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_y = 715;
uint16_t expected_z = 715;
float gain_multiple = 1.0;
delay(10);
// determine if we are using 5843 or 5883L
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
! read_register(ConfigRegA, &_base_config)) {
healthy = false;
return;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
// MPNG: Datasheet for HMC5883L says: 766 for X,Y and 713 for Z, but in read_raw we exch Y and Z
expected_x = 766;
expected_y = 713;
expected_z = 766;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
return;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 20 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (! write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
delay(50);
// set gains
if (! write_register(ConfigRegB, calibration_gain) ||
! write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
delay(50);
if (!read_raw())
continue; // we didn't read valid values
delay(10);
float cal[3];
cal[0] = fabs(expected_x / (float)_raw_mag_x);
cal[1] = fabs(expected_y / (float)_raw_mag_y);
cal[2] = fabs(expected_z / (float)_raw_mag_z);
if (cal[0] > 0.7 && cal[0] < 1.3 &&
cal[1] > 0.7 && cal[1] < 1.3 &&
cal[2] > 0.7 && cal[2] < 1.3) {
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0
/* useful for debugging */
Serial.print("mag_x: ");
Serial.print(_raw_mag_x);
Serial.print(" mag_y: ");
Serial.print(_raw_mag_y);
Serial.print(" mag_z: ");
Serial.println(_raw_mag_z);
Serial.print("CalX: ");
Serial.print(calibration[0]/good_count);
Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count);
#endif
}
if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
delay(50);
// leave test mode
if (!re_initialise()) {
Serial.println("Fail to initialize compass");
return ;
}
delay(50);
_updated = read_raw(); // Read first right values
if (_updated) {
read();
}
healthy = true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
_bus_sem = _bus->get_semaphore();
hal.scheduler->suspend_timer_procs();
if (!_bus_sem || !_bus_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.console->printf_P(PSTR("HMC5843: Unable to get bus semaphore\n"));
goto fail_sem;
}
if (!_bus->configure()) {
hal.console->printf_P(PSTR("HMC5843: Could not configure the bus\n"));
goto errout;
}
if (!_detect_version()) {
hal.console->printf_P(PSTR("HMC5843: Could not detect version\n"));
goto errout;
}
if (_product_id == AP_COMPASS_TYPE_HMC5883L) {
calibration_gain = 0x60;
/*
note that the HMC5883 datasheet gives the x and y expected
values as 766 and the z as 713. Experiments have shown the x
axis is around 766, and the y and z closer to 713.
*/
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0f / 1090; // adjustment for runtime vs calibration gain
}
if (!_calibrate(calibration_gain, expected_x, expected_yz, gain_multiple)) {
hal.console->printf_P(PSTR("HMC5843: Could not calibrate sensor\n"));
goto errout;
}
// leave test mode
if (!re_initialise()) {
goto errout;
}
if (!_bus->start_measurements()) {
hal.console->printf_P(PSTR("HMC5843: Could not start measurements on bus\n"));
goto errout;
}
_initialised = true;
_bus_sem->give();
hal.scheduler->resume_timer_procs();
// perform an initial read
read();
#if 0
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
_scaling[0], _scaling[1], _scaling[2]);
#endif
_compass_instance = register_compass();
set_dev_id(_compass_instance, _product_id);
return true;
errout:
_bus_sem->give();
fail_sem:
hal.scheduler->resume_timer_procs();
return false;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
uint8_t calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
hal.scheduler->delay(10);
_i2c_sem = hal.i2c->get_semaphore();
if (!_i2c_sem->take(HAL_SEMAPHORE_BLOCK_FOREVER)) {
hal.scheduler->panic(PSTR("Failed to get HMC5843 semaphore"));
}
// determine if we are using 5843 or 5883L
_base_config = 0;
if (!write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
!read_register(ConfigRegA, &_base_config)) {
_healthy[0] = false;
_i2c_sem->give();
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
/*
note that the HMC5883 datasheet gives the x and y expected
values as 766 and the z as 713. Experiments have shown the x
axis is around 766, and the y and z closer to 713.
*/
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
_i2c_sem->give();
return false;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 25 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (!write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
hal.scheduler->delay(50);
// set gains
if (!write_register(ConfigRegB, calibration_gain) ||
!write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
hal.scheduler->delay(50);
if (!read_raw())
continue; // we didn't read valid values
hal.scheduler->delay(10);
float cal[3];
// hal.console->printf_P(PSTR("mag %d %d %d\n"), _mag_x, _mag_y, _mag_z);
cal[0] = fabsf(expected_x / (float)_mag_x);
cal[1] = fabsf(expected_yz / (float)_mag_y);
cal[2] = fabsf(expected_yz / (float)_mag_z);
// hal.console->printf_P(PSTR("cal=%.2f %.2f %.2f\n"), cal[0], cal[1], cal[2]);
// we throw away the first two samples as the compass may
// still be changing its state from the application of the
// strap excitation. After that we accept values in a
// reasonable range
if (numAttempts > 2 &&
cal[0] > 0.7f && cal[0] < 1.35f &&
cal[1] > 0.7f && cal[1] < 1.35f &&
cal[2] > 0.7f && cal[2] < 1.35f) {
// hal.console->printf_P(PSTR("cal=%.2f %.2f %.2f good\n"), cal[0], cal[1], cal[2]);
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0
/* useful for debugging */
hal.console->printf_P(PSTR("MagX: %d MagY: %d MagZ: %d\n"), (int)_mag_x, (int)_mag_y, (int)_mag_z);
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"), cal[0], cal[1], cal[2]);
#endif
}
if (good_count >= 5) {
/*
The use of gain_multiple below is incorrect, as the gain
difference between 2.5Ga mode and 1Ga mode is already taken
into account by the expected_x and expected_yz values. We
are not going to fix it however as it would mean all
APM1/APM2 users redoing their compass calibration. The
impact is that the values we report on APM1/APM2 are lower
than they should be (by a multiple of about 0.6). This
doesn't have any impact other than the learned compass
offsets
*/
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
_i2c_sem->give();
return false;
}
_i2c_sem->give();
_initialised = true;
// perform an initial read
_healthy[0] = true;
read();
#if 0
hal.console->printf_P(PSTR("CalX: %.2f CalY: %.2f CalZ: %.2f\n"),
calibration[0], calibration[1], calibration[2]);
#endif
return success;
}
// Read Sensor data
bool AP_Compass_HMC5843_EXT::read()
{
if (!_initialised) {
// someone has tried to enable a compass for the first time
// mid-flight .... we can't do that yet (especially as we won't
// have the right orientation!)
return false;
}
if (!_healthy[0]) {
if (hal.scheduler->millis() < _retry_time) {
return false;
}
if (!re_initialise()) {
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c2->setHighSpeed(false);
return false;
}
}
if (_accum_count == 0) {
accumulate();
if (!_healthy[0] || _accum_count == 0) {
// try again in 1 second, and set I2c clock speed slower
_retry_time = hal.scheduler->millis() + 1000;
hal.i2c2->setHighSpeed(false);
return false;
}
}
_field[0].x = _mag_x_accum * calibration[0] / _accum_count;
_field[0].y = _mag_y_accum * calibration[1] / _accum_count;
_field[0].z = _mag_z_accum * calibration[2] / _accum_count;
_accum_count = 0;
_mag_x_accum = _mag_y_accum = _mag_z_accum = 0;
last_update = hal.scheduler->micros(); // record time of update
// rotate to the desired orientation
if (product_id == AP_COMPASS_TYPE_HMC5883L) {
_field[0].rotate(ROTATION_YAW_90);
}
// apply default board orientation for this compass type. This is
// a noop on most boards
_field[0].rotate(MAG_BOARD_ORIENTATION);
// add user selectable orientation
_field[0].rotate((enum Rotation)_orientation.get());
if (!_external) {
// and add in AHRS_ORIENTATION setting if not an external compass
_field[0].rotate(_board_orientation);
}
_field[0] += _offset[0].get();
// apply motor compensation
if(_motor_comp_type != AP_COMPASS_MOT_COMP_DISABLED && _thr_or_curr != 0.0f) {
_motor_offset[0] = _motor_compensation[0].get() * _thr_or_curr;
_field[0] += _motor_offset[0];
}else{
_motor_offset[0].zero();
}
_healthy[0] = true;
return true;
}
// Public Methods //////////////////////////////////////////////////////////////
bool
AP_Compass_HMC5843::init()
{
int numAttempts = 0, good_count = 0;
bool success = false;
byte calibration_gain = 0x20;
uint16_t expected_x = 715;
uint16_t expected_yz = 715;
float gain_multiple = 1.0;
delay(10);
// determine if we are using 5843 or 5883L
if (! write_register(ConfigRegA, SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation) ||
! read_register(ConfigRegA, &_base_config)) {
healthy = false;
return false;
}
if ( _base_config == (SampleAveraging_8<<5 | DataOutputRate_75HZ<<2 | NormalOperation)) {
// a 5883L supports the sample averaging config
int old_product_id = product_id;
product_id = AP_COMPASS_TYPE_HMC5883L;
calibration_gain = 0x60;
expected_x = 766;
expected_yz = 713;
gain_multiple = 660.0 / 1090; // adjustment for runtime vs calibration gain
if (old_product_id != product_id) {
/* now we know the compass type we need to rotate the
* orientation matrix that we were given
*/
rotate_for_5883L(&_orientation_matrix);
}
} else if (_base_config == (NormalOperation | DataOutputRate_75HZ<<2)) {
product_id = AP_COMPASS_TYPE_HMC5843;
} else {
// not behaving like either supported compass type
return false;
}
calibration[0] = 0;
calibration[1] = 0;
calibration[2] = 0;
while ( success == 0 && numAttempts < 20 && good_count < 5)
{
// record number of attempts at initialisation
numAttempts++;
// force positiveBias (compass should return 715 for all channels)
if (! write_register(ConfigRegA, PositiveBiasConfig))
continue; // compass not responding on the bus
delay(50);
// set gains
if (! write_register(ConfigRegB, calibration_gain) ||
! write_register(ModeRegister, SingleConversion))
continue;
// read values from the compass
delay(50);
if (!read_raw())
continue; // we didn't read valid values
delay(10);
float cal[3];
cal[0] = fabs(expected_x / (float)mag_x);
cal[1] = fabs(expected_yz / (float)mag_y);
cal[2] = fabs(expected_yz / (float)mag_z);
if (cal[0] > 0.7 && cal[0] < 1.3 &&
cal[1] > 0.7 && cal[1] < 1.3 &&
cal[2] > 0.7 && cal[2] < 1.3) {
good_count++;
calibration[0] += cal[0];
calibration[1] += cal[1];
calibration[2] += cal[2];
}
#if 0
/* useful for debugging */
Serial.print("mag_x: ");
Serial.print(mag_x);
Serial.print(" mag_y: ");
Serial.print(mag_y);
Serial.print(" mag_z: ");
Serial.println(mag_z);
Serial.print("CalX: ");
Serial.print(calibration[0]/good_count);
Serial.print(" CalY: ");
Serial.print(calibration[1]/good_count);
Serial.print(" CalZ: ");
Serial.println(calibration[2]/good_count);
#endif
}
if (good_count >= 5) {
calibration[0] = calibration[0] * gain_multiple / good_count;
calibration[1] = calibration[1] * gain_multiple / good_count;
calibration[2] = calibration[2] * gain_multiple / good_count;
success = true;
} else {
/* best guess */
calibration[0] = 1.0;
calibration[1] = 1.0;
calibration[2] = 1.0;
}
// leave test mode
if (!re_initialise()) {
return false;
}
return success;
}
