added FY90Q buyild target to Makefile
fixed mag calibration finally I think... heading + mag declination calculation done using better precision. increased gyro bias calculation to 1000. git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@163 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
This commit is contained in:
parent
572d5827cc
commit
8fb580d3f5
4
Makefile
4
Makefile
|
@ -10,6 +10,8 @@ TCHAIN=
|
||||||
CC = $(TCHAIN)arm-none-eabi-gcc
|
CC = $(TCHAIN)arm-none-eabi-gcc
|
||||||
OPT = -Os
|
OPT = -Os
|
||||||
OBJCOPY = $(TCHAIN)arm-none-eabi-objcopy
|
OBJCOPY = $(TCHAIN)arm-none-eabi-objcopy
|
||||||
|
# enable for FYxxQ builds
|
||||||
|
EXTRA= #-DFY90Q
|
||||||
|
|
||||||
#Atollic TrueStudio
|
#Atollic TrueStudio
|
||||||
#CC = "C:\Program Files (x86)\Atollic\TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0\ARMTools\bin\arm-atollic-eabi-gcc"
|
#CC = "C:\Program Files (x86)\Atollic\TrueSTUDIO for STMicroelectronics STM32 Lite 2.3.0\ARMTools\bin\arm-atollic-eabi-gcc"
|
||||||
|
@ -32,7 +34,7 @@ LINK_SCRIPT=stm32_flash.ld
|
||||||
# Assembler, Compiler and Linker flags and linker script settings
|
# Assembler, Compiler and Linker flags and linker script settings
|
||||||
LINKER_FLAGS=-lm -mthumb -mcpu=cortex-m3 -Wl,--gc-sections -T$(LINK_SCRIPT) -static -Wl,-cref "-Wl,-Map=$(BIN_DIR)/baseflight.map" -Wl,--defsym=malloc_getpagesize_P=0x1000
|
LINKER_FLAGS=-lm -mthumb -mcpu=cortex-m3 -Wl,--gc-sections -T$(LINK_SCRIPT) -static -Wl,-cref "-Wl,-Map=$(BIN_DIR)/baseflight.map" -Wl,--defsym=malloc_getpagesize_P=0x1000
|
||||||
ASSEMBLER_FLAGS=-c $(OPT) -mcpu=cortex-m3 -mthumb -x assembler-with-cpp -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
|
ASSEMBLER_FLAGS=-c $(OPT) -mcpu=cortex-m3 -mthumb -x assembler-with-cpp -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
|
||||||
COMPILER_FLAGS=-c -mcpu=cortex-m3 $(OPT) -Wall -ffunction-sections -fdata-sections -mthumb -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
|
COMPILER_FLAGS=-c -mcpu=cortex-m3 $(OPT) -Wall -ffunction-sections -fdata-sections -mthumb $(EXTRA) -DSTM32F10X_MD -DUSE_STDPERIPH_DRIVER -Isrc -Ilib/STM32F10x_StdPeriph_Driver/inc -Ilib/CMSIS/CM3/CoreSupport -Ilib/CMSIS/CM3/DeviceSupport/ST/STM32F10x
|
||||||
|
|
||||||
# Define sources and objects
|
# Define sources and objects
|
||||||
#
|
#
|
||||||
|
|
1188
obj/baseflight.hex
1188
obj/baseflight.hex
File diff suppressed because it is too large
Load Diff
File diff suppressed because it is too large
Load Diff
|
@ -51,7 +51,7 @@ void hmc5883lInit(void)
|
||||||
void hmc5883lCal(uint8_t calibration_gain)
|
void hmc5883lCal(uint8_t calibration_gain)
|
||||||
{
|
{
|
||||||
// force positiveBias (compass should return 715 for all channels)
|
// force positiveBias (compass should return 715 for all channels)
|
||||||
i2cWrite(MAG_ADDRESS, ConfigRegA, PositiveBiasConfig);
|
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | PositiveBiasConfig);
|
||||||
delay(50);
|
delay(50);
|
||||||
// set gains for calibration
|
// set gains for calibration
|
||||||
i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);
|
i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);
|
||||||
|
|
|
@ -246,8 +246,10 @@ static void getEstimatedAttitude(void)
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
if (sensors(SENSOR_MAG)) {
|
if (sensors(SENSOR_MAG)) {
|
||||||
// Attitude of the cross product vector GxM
|
// 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 = _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);
|
||||||
heading = heading + magneticDeclination;
|
heading = heading + magneticDeclination;
|
||||||
|
heading = heading / 10;
|
||||||
|
|
||||||
if (heading > 180)
|
if (heading > 180)
|
||||||
heading = heading - 360;
|
heading = heading - 360;
|
||||||
else if (heading < -180)
|
else if (heading < -180)
|
||||||
|
|
|
@ -120,7 +120,7 @@ int main(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
previousTime = micros();
|
previousTime = micros();
|
||||||
calibratingG = 400;
|
calibratingG = 1000;
|
||||||
|
|
||||||
// loopy
|
// loopy
|
||||||
while (1) {
|
while (1) {
|
||||||
|
|
2
src/mw.c
2
src/mw.c
|
@ -305,7 +305,7 @@ void loop(void)
|
||||||
rcDelayCommand++;
|
rcDelayCommand++;
|
||||||
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && armed == 0) {
|
if (rcData[YAW] < cfg.mincheck && rcData[PITCH] < cfg.mincheck && armed == 0) {
|
||||||
if (rcDelayCommand == 20) {
|
if (rcDelayCommand == 20) {
|
||||||
calibratingG = 400;
|
calibratingG = 1000;
|
||||||
if (feature(FEATURE_GPS))
|
if (feature(FEATURE_GPS))
|
||||||
GPS_reset_home_position();
|
GPS_reset_home_position();
|
||||||
}
|
}
|
||||||
|
|
|
@ -104,7 +104,7 @@ retry:
|
||||||
// calculate magnetic declination
|
// calculate magnetic declination
|
||||||
deg = cfg.mag_declination / 100;
|
deg = cfg.mag_declination / 100;
|
||||||
min = cfg.mag_declination % 100;
|
min = cfg.mag_declination % 100;
|
||||||
magneticDeclination = deg + ((float)min * (1.0f / 60.0f));
|
magneticDeclination = (deg + ((float)min * (1.0f / 60.0f))) * 10; // heading is in 0.1deg units
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -278,15 +278,16 @@ static void GYRO_Common(void)
|
||||||
if (calibratingG > 0) {
|
if (calibratingG > 0) {
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
// Reset g[axis] at start of calibration
|
// Reset g[axis] at start of calibration
|
||||||
if (calibratingG == 400)
|
if (calibratingG == 1000)
|
||||||
g[axis] = 0;
|
g[axis] = 0;
|
||||||
// Sum up 400 readings
|
// Sum up 1000 readings
|
||||||
g[axis] += gyroADC[axis];
|
g[axis] += gyroADC[axis];
|
||||||
|
// g[axis] += (1000 - calibratingG) >> 1;
|
||||||
// Clear global variables for next reading
|
// Clear global variables for next reading
|
||||||
gyroADC[axis] = 0;
|
gyroADC[axis] = 0;
|
||||||
gyroZero[axis] = 0;
|
gyroZero[axis] = 0;
|
||||||
if (calibratingG == 1) {
|
if (calibratingG == 1) {
|
||||||
gyroZero[axis] = g[axis] / 400;
|
gyroZero[axis] = g[axis] / 1000;
|
||||||
blinkLED(10, 15, 1);
|
blinkLED(10, 15, 1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -320,9 +321,10 @@ static void Mag_getRawADC(void)
|
||||||
|
|
||||||
void Mag_init(void)
|
void Mag_init(void)
|
||||||
{
|
{
|
||||||
|
uint8_t calibration_gain = 0x60; // HMC5883
|
||||||
|
#if 1
|
||||||
uint8_t numAttempts = 0, good_count = 0;
|
uint8_t numAttempts = 0, good_count = 0;
|
||||||
bool success = false;
|
bool success = false;
|
||||||
uint8_t calibration_gain = 0x60; // HMC5883
|
|
||||||
uint16_t expected_x = 766; // default values for HMC5883
|
uint16_t expected_x = 766; // default values for HMC5883
|
||||||
uint16_t expected_yz = 713;
|
uint16_t expected_yz = 713;
|
||||||
float gain_multiple = 660.0f / 1090.0f; // adjustment for runtime vs calibration gain
|
float gain_multiple = 660.0f / 1090.0f; // adjustment for runtime vs calibration gain
|
||||||
|
@ -368,13 +370,28 @@ void Mag_init(void)
|
||||||
magCal[2] = 1.0f;
|
magCal[2] = 1.0f;
|
||||||
}
|
}
|
||||||
|
|
||||||
#if 0
|
hmc5883lFinishCal();
|
||||||
|
#else
|
||||||
|
// initial calibration
|
||||||
|
hmc5883lInit();
|
||||||
|
|
||||||
|
magCal[0] = 0;
|
||||||
|
magCal[1] = 0;
|
||||||
|
magCal[2] = 0;
|
||||||
|
|
||||||
|
// enter calibration mode
|
||||||
|
hmc5883lCal(calibration_gain);
|
||||||
|
delay(100);
|
||||||
|
Mag_getRawADC();
|
||||||
|
delay(10);
|
||||||
|
|
||||||
magCal[ROLL] = 1160.0f / abs(magADC[ROLL]);
|
magCal[ROLL] = 1160.0f / abs(magADC[ROLL]);
|
||||||
magCal[PITCH] = 1160.0f / abs(magADC[PITCH]);
|
magCal[PITCH] = 1160.0f / abs(magADC[PITCH]);
|
||||||
magCal[YAW] = 1080.0f / abs(magADC[YAW]);
|
magCal[YAW] = 1080.0f / abs(magADC[YAW]);
|
||||||
#endif
|
|
||||||
|
|
||||||
hmc5883lFinishCal();
|
hmc5883lFinishCal();
|
||||||
|
#endif
|
||||||
|
|
||||||
magInit = 1;
|
magInit = 1;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -424,7 +441,7 @@ void Mag_getADC(void)
|
||||||
} else {
|
} else {
|
||||||
tCal = 0;
|
tCal = 0;
|
||||||
for (axis = 0; axis < 3; axis++)
|
for (axis = 0; axis < 3; axis++)
|
||||||
cfg.magZero[axis] = ((magZeroTempMax[axis] - magZeroTempMin[axis]) / 2 - magZeroTempMax[axis]); // Calculate offsets
|
cfg.magZero[axis] = (magZeroTempMin[axis] + magZeroTempMax[axis]) / 2; // Calculate offsets
|
||||||
writeParams(1);
|
writeParams(1);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue