rusefi
/
atbetaflight
mirror of https://github.com/rusefi/atbetaflight.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

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

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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
baseflight.uvproj
View File

@ -446,6 +446,11 @@
<FileType>1</FileType>
<FilePath>.\src\spektrum.c</FilePath>
</File>
<File>
<FileName>buzzer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\buzzer.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1201,6 +1206,11 @@
<FileType>1</FileType>
<FilePath>.\src\spektrum.c</FilePath>
</File>
<File>
<FileName>buzzer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\buzzer.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -1902,6 +1912,11 @@
<FileType>1</FileType>
<FilePath>.\src\spektrum.c</FilePath>
</File>
<File>
<FileName>buzzer.c</FileName>
<FileType>1</FileType>
<FilePath>.\src\buzzer.c</FilePath>
</File>
</Files>
</Group>
<Group>

5040
obj/baseflight.hex
View File

File diff suppressed because it is too large Load Diff

3
src/config.c
View File

@ -13,7 +13,7 @@ config_t cfg;
const char rcChannelLetters[] = "AERT1234";
static uint32_t enabledSensors = 0;
uint8_t checkNewConf = 14;
uint8_t checkNewConf = 15;
void parseRcChannels(const char *input)
{
@ -127,6 +127,7 @@ void checkFirstTime(bool reset)
cfg.accZero[0] = 0;
cfg.accZero[1] = 0;
cfg.accZero[2] = 0;
cfg.magDeclination = 0; // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero.
cfg.acc_lpf_factor = 100;
cfg.gyro_lpf = 42;
cfg.gyro_smoothing_factor = 0x00141403; // default factors of 20, 20, 3 for R/P/Y

45
src/drv_hmc5883l.c
View File

@ -4,6 +4,30 @@
#define MAG_ADDRESS 0x1E
#define MAG_DATA_REGISTER 0x03
#define ConfigRegA 0x00
#define ConfigRegB 0x01
#define magGain 0x20
#define PositiveBiasConfig 0x11
#define NegativeBiasConfig 0x12
#define NormalOperation 0x10
#define ModeRegister 0x02
#define ContinuousConversion 0x00
#define SingleConversion 0x01
// ConfigRegA valid sample averaging for 5883L
#define SampleAveraging_1 0x00
#define SampleAveraging_2 0x01
#define SampleAveraging_4 0x02
#define SampleAveraging_8 0x03
// ConfigRegA valid data output rates for 5883L
#define DataOutputRate_0_75HZ 0x00
#define DataOutputRate_1_5HZ 0x01
#define DataOutputRate_3HZ 0x02
#define DataOutputRate_7_5HZ 0x03
#define DataOutputRate_15HZ 0x04
#define DataOutputRate_30HZ 0x05
#define DataOutputRate_75HZ 0x06
bool hmc5883lDetect(void)
{
@ -20,21 +44,26 @@ bool hmc5883lDetect(void)
void hmc5883lInit(void)
{
delay(100);
// force positiveBias
i2cWrite(MAG_ADDRESS, 0x00, 0x71); // Configuration Register A -- 0 11 100 01 num samples: 8 ; output rate: 15Hz ; positive bias
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
delay(50);
}
void hmc5883lCal(uint8_t calibration_gain)
{
// force positiveBias (compass should return 715 for all channels)
i2cWrite(MAG_ADDRESS, ConfigRegA, PositiveBiasConfig);
delay(50);
// set gains for calibration
i2cWrite(MAG_ADDRESS, 0x01, 0x60); // Configuration Register B -- 011 00000 configuration gain 2.5Ga
i2cWrite(MAG_ADDRESS, 0x02, 0x01); // Mode register -- 000000 01 single Conversion Mode
// this enters test mode
i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);
i2cWrite(MAG_ADDRESS, ModeRegister, SingleConversion);
}
void hmc5883lFinishCal(void)
{
// leave test mode
i2cWrite(MAG_ADDRESS, 0x00, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, 0x01, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, 0x02, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
i2cWrite(MAG_ADDRESS, ConfigRegB, magGain);
i2cWrite(MAG_ADDRESS, ModeRegister, ContinuousConversion);
}
void hmc5883lRead(int16_t *magData)

1
src/drv_hmc5883l.h
View File

@ -2,5 +2,6 @@
bool hmc5883lDetect(void);
void hmc5883lInit(void);
void hmc5883lCal(uint8_t calibration_gain);
void hmc5883lFinishCal(void);
void hmc5883lRead(int16_t *magData);

9
src/imu.c
View File

@ -9,6 +9,7 @@ int32_t EstAlt; // in cm
int16_t BaroPID = 0;
int32_t AltHold;
int16_t errorAltitudeI = 0;
float magneticDeclination = 0.0f; // calculated at startup from config
// **************
// gyro+acc IMU
@ -207,9 +208,8 @@ static void getEstimatedAttitude(void)
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle);
if (sensors(SENSOR_MAG)) {
if (sensors(SENSOR_MAG))
rotateV(&EstM.V, deltaGyroAngle);
}
if (abs(accSmooth[ROLL]) < acc_25deg && abs(accSmooth[PITCH]) < acc_25deg && accSmooth[YAW] > 0)
smallAngle25 = 1;
@ -237,6 +237,11 @@ static void getEstimatedAttitude(void)
if (sensors(SENSOR_MAG)) {
// Attitude of the cross product vector GxM
heading = _atan2f(EstG.V.X * EstM.V.Z - EstG.V.Z * EstM.V.X, EstG.V.Z * EstM.V.Y - EstG.V.Y * EstM.V.Z) / 10;
heading = heading + magneticDeclination;
if (heading > 180)
heading = heading - 360;
else if (heading < -180)
heading = heading + 360;
}
#endif
}

2
src/mw.c
View File

@ -88,7 +88,7 @@ void blinkLED(uint8_t num, uint8_t wait, uint8_t repeat)
// 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 buzzerTime, calibratedAccTime;
static uint32_t calibratedAccTime;
uint16_t tmp,tmp2;
static uint8_t vbatTimer = 0;
static uint8_t buzzerFreq; //delay between buzzer ring

4
src/mw.h
View File

@ -126,6 +126,7 @@ typedef struct config_t {
uint8_t dynThrPID;
int16_t accZero[3];
int16_t magZero[3];
int16_t magDeclination; // Get your magnetic decliniation from here : http://magnetic-declination.com/
int16_t accTrim[2];
// sensor-related stuff
@ -290,6 +291,9 @@ uint32_t featureMask(void);
void spektrumInit(void);
bool spektrumFrameComplete(void);
// buzzer
void buzzer(uint8_t warn_vbat);
// cli
void cliProcess(void);

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);
}
}