Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-14 03:50:02 +03:00
Code Issues Wiki Activity

added improved mag calibration from http://www.multiwii.com/forum/viewtopic.php?p=13334#p13334 (thanks EOSBandi)

Browse source 
new hex file only works with Java GUI 20120504-dev :( WinGUI is still not updated. Use latest release version on downloads page if want to use WinGUI.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@153 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop 2012-05-06 09:58:03 +00:00
parent 0d7460960e
commit 941f2f1762
9 changed files with 2684 additions and 2495 deletions
Download patch file Download diff file Expand all files Collapse all files

60
src/sensors.c
View file

@ -15,6 +15,7 @@ extern uint16_t AccInflightCalibrationSavetoEEProm;
extern uint16_t AccInflightCalibrationActive;
extern uint16_t batteryWarningVoltage;
extern uint8_t batteryCellCount;
extern float magneticDeclination;
sensor_t acc; // acc access functions
sensor_t gyro; // gyro access functions
@ -29,6 +30,8 @@ void sensorsAutodetect(void)
// AfroFlight32 i2c sensors
void sensorsAutodetect(void)
{
int16_t deg, min;
drv_adxl345_config_t acc_params;
// configure parameters for ADXL345 driver
@ -61,6 +64,11 @@ void sensorsAutodetect(void)
gyro.init();
// todo: this is driver specific :(
mpu3050Config(cfg.gyro_lpf);
// calculate magnetic declination
deg = cfg.magDeclination / 100;
min = cfg.magDeclination % 100;
magneticDeclination = deg + ((float)min * (1.0f / 60.0f));
}
#endif
@ -306,15 +314,59 @@ static void Mag_getRawADC(void)
void Mag_init(void)
{
uint8_t numAttempts = 0, good_count = 0;
bool success = false;
uint8_t calibration_gain = 0x60; // HMC5883
uint16_t expected_x = 766; // default values for HMC5883
uint16_t expected_yz = 713;
float gain_multiple = 660.0f / 1090.0f; // adjustment for runtime vs calibration gain
float cal[3];
// initial calibration
hmc5883lInit();
delay(100);
Mag_getRawADC();
delay(10);
magCal[0] = 0;
magCal[1] = 0;
magCal[2] = 0;
while (success == false && numAttempts < 20 && good_count < 5) {
// record number of attempts at initialisation
numAttempts++;
// enter calibration mode
hmc5883lCal(calibration_gain);
delay(100);
Mag_getRawADC();
delay(10);
cal[0] = fabsf(expected_x / (float)magADC[ROLL]);
cal[1] = fabsf(expected_yz / (float)magADC[PITCH]);
cal[2] = fabsf(expected_yz / (float)magADC[ROLL]);
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++;
magCal[0] += cal[0];
magCal[1] += cal[1];
magCal[2] += cal[2];
}
}
if (good_count >= 5) {
magCal[0] = magCal[0] * gain_multiple / (float)good_count;
magCal[1] = magCal[1] * gain_multiple / (float)good_count;
magCal[2] = magCal[2] * gain_multiple / (float)good_count;
success = true;
} else {
/* best guess */
magCal[0] = 1.0f;
magCal[1] = 1.0f;
magCal[2] = 1.0f;
}
#if 0
magCal[ROLL] = 1160.0f / abs(magADC[ROLL]);
magCal[PITCH] = 1160.0f / abs(magADC[PITCH]);
magCal[YAW] = 1080.0f / abs(magADC[YAW]);
#endif
hmc5883lFinishCal();
magInit = 1;
@ -366,7 +418,7 @@ void Mag_getADC(void)
} else {
tCal = 0;
for (axis = 0; axis < 3; axis++)
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2;
cfg.magZero[axis] = ((magZeroTempMax[axis] - magZeroTempMin[axis]) / 2 - magZeroTempMax[axis]); // Calculate offsets
writeParams(1);
}
}