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

disconnected magcal from core and put it all into hmc5883 driver. no need to keep track of it if driver does init by itself as well.

Browse source 
moved annexcode into mw.c instead of imu.c
hopefully didn't break anything.
NOT FLIGHT TESTED.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@405 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
timecop@gmail.com 2013-09-19 15:07:48 +00:00
parent ac3cee7788
commit 8a5157db46
5 changed files with 25 additions and 37 deletions
Download patch file Download diff file Expand all files Collapse all files

48
src/drv_hmc5883l.c
View file

@ -74,6 +74,7 @@
#define HMC_NEG_BIAS 2
static sensor_align_e magAlign = CW180_DEG;
static float magGain[3] = { 1.0f, 1.0f, 1.0f };
bool hmc5883lDetect(sensor_align_e align)
{
@ -90,10 +91,9 @@ bool hmc5883lDetect(sensor_align_e align)
return true;
}
void hmc5883lInit(float *calibrationGain)
void hmc5883lInit(void)
{
gpio_config_t gpio;
float magGain[3];
int16_t magADC[3];
int i;
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
@ -125,12 +125,12 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged rather than taking the max.
xyz_total[0] += magADC[0];
xyz_total[1] += magADC[1];
xyz_total[2] += magADC[2];
xyz_total[X] += magADC[X];
xyz_total[Y] += magADC[Y];
xyz_total[Z] += magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
@ -145,21 +145,21 @@ void hmc5883lInit(float *calibrationGain)
hmc5883lRead(magADC); // Get the raw values in case the scales have already been changed.
// Since the measurements are noisy, they should be averaged.
xyz_total[0] -= magADC[0];
xyz_total[1] -= magADC[1];
xyz_total[2] -= magADC[2];
xyz_total[X] -= magADC[X];
xyz_total[Y] -= magADC[Y];
xyz_total[Z] -= magADC[Z];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
if (-4096 >= min(magADC[X], min(magADC[Y], magADC[Z]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[0] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[0]);
magGain[1] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[1]);
magGain[2] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[2]);
magGain[X] = fabs(660.0f * HMC58X3_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
magGain[Y] = fabs(660.0f * HMC58X3_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
magGain[Z] = fabs(660.0f * HMC58X3_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
// leave test mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
@ -168,16 +168,9 @@ void hmc5883lInit(float *calibrationGain)
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[0] = 1.0f;
magGain[1] = 1.0f;
magGain[2] = 1.0f;
}
// if parameter was passed, give calibration values back
if (calibrationGain) {
calibrationGain[0] = magGain[0];
calibrationGain[1] = magGain[1];
calibrationGain[2] = magGain[2];
magGain[X] = 1.0f;
magGain[Y] = 1.0f;
magGain[Z] = 1.0f;
}
}
@ -187,9 +180,10 @@ void hmc5883lRead(int16_t *magData)
int16_t mag[3];
i2cRead(MAG_ADDRESS, MAG_DATA_REGISTER, 6, buf);
mag[X] = buf[0] << 8 | buf[1];
mag[Z] = buf[2] << 8 | buf[3];
mag[Y] = buf[4] << 8 | buf[5];
// During calibration, magGain is 1.0, so the read returns normal non-calibrated values.
// After calibration is done, magGain is set to calculated gain values.
mag[X] = (int16_t)(buf[0] << 8 | buf[1]) * magGain[X];
mag[Z] = (int16_t)(buf[2] << 8 | buf[3]) * magGain[Z];
mag[Y] = (int16_t)(buf[4] << 8 | buf[5]) * magGain[Y];
alignSensors(mag, magData, magAlign);
}

2
src/drv_hmc5883l.h
View file

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

1
src/imu.c
View file

@ -56,7 +56,6 @@ void computeIMU(void)
accADC[Y] = 0;
accADC[Z] = 0;
}
annexCode();
if (feature(FEATURE_GYRO_SMOOTHING)) {
static uint8_t Smoothing[3] = { 0, 0, 0 };

4
src/mw.c
View file

@ -84,7 +84,6 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
#define BREAKPOINT 1500
// this code is executed at each loop and won't interfere with control loop if it lasts less than 650 microseconds
void annexCode(void)
{
static uint32_t calibratedAccTime;
@ -147,7 +146,7 @@ void annexCode(void)
tmp2 = tmp / 100;
rcCommand[THROTTLE] = lookupThrottleRC[tmp2] + (tmp - tmp2 * 100) * (lookupThrottleRC[tmp2 + 1] - lookupThrottleRC[tmp2]) / 100; // [0;1000] -> expo -> [MINTHROTTLE;MAXTHROTTLE]
if(f.HEADFREE_MODE) {
if (f.HEADFREE_MODE) {
float radDiff = (heading - headFreeModeHold) * M_PI / 180.0f;
float cosDiff = cosf(radDiff);
float sinDiff = sinf(radDiff);
@ -777,6 +776,7 @@ void loop(void)
loopTime = currentTime + mcfg.looptime;
computeIMU();
annexCode();
// Measure loop rate just afer reading the sensors
currentTime = micros();
cycleTime = (int32_t)(currentTime - previousTime);

7
src/sensors.c
View file

@ -366,14 +366,13 @@ void Gyro_getADC(void)
}
#ifdef MAG
static float magCal[3] = { 1.0f, 1.0f, 1.0f }; // gain for each axis, populated at sensor init
static uint8_t magInit = 0;
void Mag_init(void)
{
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON;
hmc5883lInit(magCal);
hmc5883lInit();
LED1_OFF;
magInit = 1;
}
@ -392,10 +391,6 @@ int Mag_getADC(void)
// Read mag sensor
hmc5883lRead(magADC);
magADC[X] = magADC[X] * magCal[X];
magADC[Y] = magADC[Y] * magCal[Y];
magADC[Z] = magADC[Z] * magCal[Z];
if (f.CALIBRATE_MAG) {
tCal = t;
for (axis = 0; axis < 3; axis++) {