Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-13 19:40:31 +03:00
Code Issues Wiki Activity

corrected magnetometer alignment bug found by CrashPilot1000 - during mag calibration, axes must be swapped per sensor alignment, which didn't happen.

Browse source 
git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@336 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-06-12 02:45:56 +00:00
parent 5de9dc47e0
commit 58d362d33e
3 changed files with 22 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

22
src/drv_hmc5883l.c
View file

@ -17,7 +17,9 @@
#define HMC_POS_BIAS 1 #define HMC_POS_BIAS 1
#define HMC_NEG_BIAS 2 #define HMC_NEG_BIAS 2
bool hmc5883lDetect(void) static int8_t sensor_align[3];
bool hmc5883lDetect(int8_t *align)
{ {
bool ack = false; bool ack = false;
uint8_t sig = 0; uint8_t sig = 0;
@ -26,6 +28,8 @@ bool hmc5883lDetect(void)
if (!ack || sig != 'H') if (!ack || sig != 'H')
return false; return false;
memcpy(sensor_align, align, 3);
return true; return true;
} }
@ -117,10 +121,20 @@ void hmc5883lInit(float *calibrationGain)
void hmc5883lRead(int16_t *magData) void hmc5883lRead(int16_t *magData)
{ {
uint8_t buf[6]; uint8_t buf[6];
int16_t mag[3];
int i;
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf); i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
magData[0] = buf[0] << 8 | buf[1]; mag[0] = ((int16_t)((uint16_t) buf[0] << 8) + buf[1]);
magData[1] = buf[2] << 8 | buf[3]; mag[1] = ((int16_t)((uint16_t) buf[2] << 8) + buf[3]);
magData[2] = buf[4] << 8 | buf[5]; mag[2] = ((int16_t)((uint16_t) buf[4] << 8) + buf[5]);
for (i = 0; i < 3; i++) {
int8_t axis = sensor_align[i];
if (axis > 0)
magData[axis - 1] = mag[i];
else
magData[-axis - 1] = -mag[i];
}
} }

2
src/drv_hmc5883l.h
View file

@ -1,5 +1,5 @@
#pragma once #pragma once
bool hmc5883lDetect(void); bool hmc5883lDetect(int8_t *align);
void hmc5883lInit(float *calibrationGain); void hmc5883lInit(float *calibrationGain);
void hmc5883lRead(int16_t *magData); void hmc5883lRead(int16_t *magData);

12
src/sensors.c
View file

@ -107,7 +107,7 @@ retry:
gyro.init(); gyro.init();
#ifdef MAG #ifdef MAG
if (!hmc5883lDetect()) if (!hmc5883lDetect(mcfg.align[ALIGN_MAG]))
sensorsClear(SENSOR_MAG); sensorsClear(SENSOR_MAG);
#endif #endif
@ -402,19 +402,13 @@ void Gyro_getADC(void)
} }
#ifdef MAG #ifdef MAG
static float magCal[3] = { 1.0, 1.0, 1.0 }; // gain for each axis, populated at sensor init static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0; static uint8_t magInit = 0;
static void Mag_getRawADC(void) static void Mag_getRawADC(void)
{ {
// MAG driver will align itself, so no need to alignSensors()
hmc5883lRead(magADC); hmc5883lRead(magADC);
// Default mag orientation is -2, -3, 1 or
// no way? is THIS finally the proper orientation?? (by GrootWitBaas)
// magADC[ROLL] = rawADC[2]; // X
// magADC[PITCH] = -rawADC[0]; // Y
// magADC[YAW] = -rawADC[1]; // Z
alignSensors(ALIGN_MAG, magADC);
} }
void Mag_init(void) void Mag_init(void)