englebert
/
inav-simplefc
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

part 2 of ?? of mw2.2 merge (still not flight-tested, so no binaries) 

defaulted to looptime of 3500 (yea, yea)
rewrote baro stuff to match mwc2.2 - both supported sensors now return temperature and pressure, which is used in altitude calculation code
rewrote hmc5883 driver to include calibration inside the driver file instead of calling parts of calibration from userspace. it will now blink LED1 while calibrating
some parts remaining to do.

git-svn-id: https://afrodevices.googlecode.com/svn/trunk/baseflight@298 7c89a4a9-59b9-e629-4cfe-3a2d53b20e61
master
timecop@gmail.com 12 years ago
parent
491b3627f6
commit
98d0581ac2
9 changed files with 230 additions and 239 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 8
      src/board.h
  2. 2
      src/config.c
  3. 43
      src/drv_bmp085.c
  4. 123
      src/drv_hmc5883l.c
  5. 4
      src/drv_hmc5883l.h
  6. 45
      src/drv_ms5611.c
  7. 102
      src/imu.c
  8. 8
      src/mw.h
  9. 134
      src/sensors.c

8
src/board.h
View File

@ -21,6 +21,11 @@
#define RADX10 (M_PI / 1800.0f) // 0.001745329252f
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
// Chip Unique ID on F103
#define U_ID_0 (*(uint32_t*)0x1FFFF7E8)
#define U_ID_1 (*(uint32_t*)0x1FFFF7EC)
@ -67,7 +72,7 @@ typedef enum {
typedef void (* sensorInitFuncPtr)(void); // sensor init prototype
typedef void (* sensorReadFuncPtr)(int16_t *data); // sensor read and align prototype
typedef int32_t (* baroCalculateFuncPtr)(void); // baro calculation (returns altitude in cm based on static data collected)
typedef void (* baroCalculateFuncPtr)(int32_t *pressure, int32_t *temperature); // baro calculation (filled params are pressure and temperature)
typedef void (* uartReceiveCallbackPtr)(uint16_t data); // used by uart2 driver to return frames to app
typedef uint16_t (* rcReadRawDataPtr)(uint8_t chan); // used by receiver driver to return channel data
@ -84,7 +89,6 @@ typedef struct baro_t
{
uint16_t ut_delay;
uint16_t up_delay;
uint16_t repeat_delay;
sensorInitFuncPtr start_ut;
sensorInitFuncPtr get_ut;
sensorInitFuncPtr start_up;

2
src/config.c
View File

@ -199,8 +199,8 @@ static void resetConf(void)
mcfg.gps_baudrate = 115200;
// serial (USART1) baudrate
mcfg.serial_baudrate = 115200;
mcfg.looptime = 3500;
// cfg.looptime = 0;
cfg.P8[ROLL] = 40;
cfg.I8[ROLL] = 30;
cfg.D8[ROLL] = 23;

43
src/drv_bmp085.c
View File

@ -84,9 +84,9 @@ static void bmp085_start_ut(void);
static void bmp085_get_ut(void);
static void bmp085_start_up(void);
static void bmp085_get_up(void);
static int16_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_temperature(uint32_t ut);
static int32_t bmp085_get_pressure(uint32_t up);
static int32_t bmp085_calculate(void);
static void bmp085_calculate(int32_t *pressure, int32_t *temperature);
bool bmp085Detect(baro_t *baro)
{
@ -135,9 +135,8 @@ bool bmp085Detect(baro_t *baro)
bmp085.al_version = BMP085_GET_BITSLICE(data, BMP085_AL_VERSION); /* get AL Version */
bmp085_get_cal_param(); /* readout bmp085 calibparam structure */
bmp085InitDone = true;
baro->ut_delay = 4600;
baro->up_delay = 26000;
baro->repeat_delay = 5000;
baro->ut_delay = 6000;
baro->up_delay = 27000;
baro->start_ut = bmp085_start_ut;
baro->get_ut = bmp085_get_ut;
baro->start_up = bmp085_start_up;
@ -149,28 +148,17 @@ bool bmp085Detect(baro_t *baro)
return false;
}
// #define BMP_TEMP_OSS 4
static int16_t bmp085_get_temperature(uint32_t ut)
static int32_t bmp085_get_temperature(uint32_t ut)
{
int16_t temperature;
int32_t temperature;
int32_t x1, x2;
#ifdef BMP_TEMP_OSS
static uint32_t temp;
#endif
x1 = (((int32_t) ut - (int32_t) bmp085.cal_param.ac6) * (int32_t) bmp085.cal_param.ac5) >> 15;
x2 = ((int32_t) bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md);
x1 = (((int32_t)ut - (int32_t)bmp085.cal_param.ac6) * (int32_t)bmp085.cal_param.ac5) >> 15;
x2 = ((int32_t)bmp085.cal_param.mc << 11) / (x1 + bmp085.cal_param.md);
bmp085.param_b5 = x1 + x2;
temperature = ((bmp085.param_b5 + 8) >> 4); // temperature in 0.1°C
#ifdef BMP_TEMP_OSS
temp *= (1 << BMP_TEMP_OSS) - 1; // multiply the temperature variable by 3 - we have tau == 1/4
temp += ((uint32_t)temperature) << 8; // add on the buffer
temp >>= BMP_TEMP_OSS; // divide by 4
return (int16_t)temp;
#else
temperature = ((bmp085.param_b5 * 10 + 8) >> 4); // temperature in 0.01°C (make same as MS5611)
return temperature;
#endif
}
static int32_t bmp085_get_pressure(uint32_t up)
@ -266,10 +254,15 @@ static void bmp085_get_up(void)
bmp085_up = (((uint32_t) data[0] << 16) | ((uint32_t) data[1] << 8) | (uint32_t) data[2]) >> (8 - bmp085.oversampling_setting);
}
static int32_t bmp085_calculate(void)
static void bmp085_calculate(int32_t *pressure, int32_t *temperature)
{
bmp085_get_temperature(bmp085_ut);
return bmp085_get_pressure(bmp085_up);
int32_t temp, press;
temp = bmp085_get_temperature(bmp085_ut);
press = bmp085_get_pressure(bmp085_up);
if (pressure)
*pressure = press;
if (temperature)
*temperature = temp;
}
static void bmp085_get_cal_param(void)

123
src/drv_hmc5883l.c
View File

@ -5,30 +5,17 @@
#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
#define HMC58X3_R_CONFA 0
#define HMC58X3_R_CONFB 1
#define HMC58X3_R_MODE 2
#define HMC58X3_X_SELF_TEST_GAUSS (+1.16) // X axis level when bias current is applied.
#define HMC58X3_Y_SELF_TEST_GAUSS (+1.16) // Y axis level when bias current is applied.
#define HMC58X3_Z_SELF_TEST_GAUSS (+1.08) // Y axis level when bias current is applied.
#define SELF_TEST_LOW_LIMIT (243.0 / 390.0) // Low limit when gain is 5.
#define SELF_TEST_HIGH_LIMIT (575.0 / 390.0) // High limit when gain is 5.
#define HMC_POS_BIAS 1
#define HMC_NEG_BIAS 2
bool hmc5883lDetect(void)
{
@ -42,9 +29,14 @@ bool hmc5883lDetect(void)
return true;
}
void hmc5883lInit(void)
void hmc5883lInit(float *calibrationGain)
{
GPIO_InitTypeDef GPIO_InitStructure;
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.
bool bret = true; // Error indicator
// PB12 - MAG_DRDY output on rev4 hardware
GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12;
@ -52,27 +44,74 @@ void hmc5883lInit(void)
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
GPIO_Init(GPIOB, &GPIO_InitStructure);
delay(100);
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
delay(50);
}
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_POS_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to pos bias
// Note that the very first measurement after a gain change maintains the same gain as the previous setting.
// The new gain setting is effective from the second measurement and on.
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 2 << 5); // Set the Gain
delay(100);
hmc5883lRead(magADC);
void hmc5883lCal(uint8_t calibration_gain)
{
// force positiveBias (compass should return 715 for all channels)
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | PositiveBiasConfig);
delay(50);
// set gains for calibration
i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);
i2cWrite(MAG_ADDRESS, ModeRegister, SingleConversion);
}
for (i = 0; i < 10; i++) { // Collect 10 samples
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
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];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
// Apply the negative bias. (Same gain)
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x010 + HMC_NEG_BIAS); // Reg A DOR = 0x010 + MS1, MS0 set to negative bias.
for (i = 0; i < 10; i++) {
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 1);
delay(50);
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];
// Detect saturation.
if (-4096 >= min(magADC[0], min(magADC[1], magADC[2]))) {
bret = false;
break; // Breaks out of the for loop. No sense in continuing if we saturated.
}
LED1_TOGGLE;
}
magGain[0] = fabs(820.0 * HMC58X3_X_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[0]);
magGain[1] = fabs(820.0 * HMC58X3_Y_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[1]);
magGain[2] = fabs(820.0 * HMC58X3_Z_SELF_TEST_GAUSS * 2.0 * 10.0 / xyz_total[2]);
void hmc5883lFinishCal(void)
{
// leave test mode
i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation);
i2cWrite(MAG_ADDRESS, ConfigRegB, magGain);
i2cWrite(MAG_ADDRESS, ModeRegister, ContinuousConversion);
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFA, 0x70); // Configuration Register A -- 0 11 100 00 num samples: 8 ; output rate: 15Hz ; normal measurement mode
i2cWrite(MAG_ADDRESS, HMC58X3_R_CONFB, 0x20); // Configuration Register B -- 001 00000 configuration gain 1.3Ga
i2cWrite(MAG_ADDRESS, HMC58X3_R_MODE, 0x00); // Mode register -- 000000 00 continuous Conversion Mode
delay(100);
if (!bret) { // Something went wrong so get a best guess
magGain[0] = 1.0;
magGain[1] = 1.0;
magGain[2] = 1.0;
}
// if parameter was passed, give calibration values back
if (calibrationGain) {
calibrationGain[0] = magGain[0];
calibrationGain[1] = magGain[1];
calibrationGain[2] = magGain[2];
}
}
void hmc5883lRead(int16_t *magData)

4
src/drv_hmc5883l.h
View File

@ -1,7 +1,5 @@
#pragma once
bool hmc5883lDetect(void);
void hmc5883lInit(void);
void hmc5883lCal(uint8_t calibration_gain);
void hmc5883lFinishCal(void);
void hmc5883lInit(float *calibrationGain);
void hmc5883lRead(int16_t *magData);

45
src/drv_ms5611.c
View File

@ -27,7 +27,7 @@ static void ms5611_start_ut(void);
static void ms5611_get_ut(void);
static void ms5611_start_up(void);
static void ms5611_get_up(void);
static int32_t ms5611_calculate(void);
static void ms5611_calculate(int32_t *pressure, int32_t *temperature);
static uint32_t ms5611_ut; // static result of temperature measurement
static uint32_t ms5611_up; // static result of pressure measurement
@ -67,7 +67,6 @@ bool ms5611Detect(baro_t *baro)
// TODO prom + CRC
baro->ut_delay = 10000;
baro->up_delay = 10000;
baro->repeat_delay = 4000;
baro->start_ut = ms5611_start_ut;
baro->get_ut = ms5611_get_ut;
baro->start_up = ms5611_start_up;
@ -151,30 +150,34 @@ static void ms5611_get_up(void)
ms5611_up = ms5611_read_adc();
}
static int32_t ms5611_calculate(void)
static void ms5611_calculate(int32_t *pressure, int32_t *temperature)
{
int32_t temperature, off2 = 0, sens2 = 0, delt;
int32_t pressure;
int32_t dT = ms5611_ut - ((uint32_t)ms5611_c[5] << 8);
int64_t off = ((uint32_t)ms5611_c[2] << 16) + (((int64_t)dT * ms5611_c[4]) >> 7);
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + (((int64_t)dT * ms5611_c[3]) >> 8);
temperature = 2000 + (((int64_t)dT * ms5611_c[6]) >> 23);
if (temperature < 2000) { // temperature lower than 20degC
delt = temperature - 2000;
delt = delt * delt;
off2 = (5 * delt) >> 1;
sens2 = (5 * delt) >> 2;
if (temperature < -1500) { // temperature lower than -15degC
delt = temperature + 1500;
int32_t temp, off2 = 0, sens2 = 0, delt;
int32_t press;
int64_t dT = ms5611_ut - ((int32_t)ms5611_c[5] << 8);
int64_t off = ((uint32_t)ms5611_c[2] << 16) + ((dT * ms5611_c[4]) >> 7);
int64_t sens = ((uint32_t)ms5611_c[1] << 15) + ((dT * ms5611_c[3]) >> 8);
temp = 2000 + ((dT * ms5611_c[6]) >> 23);
if (temp < 2000) { // temperature lower than 20degC
delt = temp - 2000;
delt = 5 * delt * delt;
off2 = delt >> 1;
sens2 = delt >> 2;
if (temp < -1500) { // temperature lower than -15degC
delt = temp + 1500;
delt = delt * delt;
off2 += 7 * delt;
sens2 += (11 * delt) >> 1;
}
}
off -= off2;
off -= off2;
sens -= sens2;
pressure = (((ms5611_up * sens ) >> 21) - off) >> 15;
return pressure;
press = (((ms5611_up * sens ) >> 21) - off) >> 15;
if (pressure)
*pressure = press;
if (temperature)
*temperature = temp;
}

102
src/imu.c
View File

@ -4,7 +4,10 @@
int16_t gyroADC[3], accADC[3], accSmooth[3], magADC[3];
float accLPFVel[3];
int16_t acc_25deg = 0;
int32_t BaroAlt;
int32_t baroPressure = 0;
int32_t baroTemperature = 0;
int32_t baroPressureSum = 0;
int32_t BaroAlt = 0;
int16_t sonarAlt; // to think about the unit
int32_t EstAlt; // in cm
int16_t BaroPID = 0;
@ -350,72 +353,87 @@ int32_t isq(int32_t x)
return x * x;
}
#define applyDeadband(value, deadband) \
if(abs(value) < deadband) { \
value = 0; \
} else if(value > 0){ \
value -= deadband; \
} else if(value < 0){ \
value += deadband; \
}
int getEstimatedAltitude(void)
{
static uint32_t deadLine = INIT_DELAY;
static int16_t baroHistTab[BARO_TAB_SIZE_MAX];
static int8_t baroHistIdx;
static int32_t baroHigh;
uint32_t dTime;
int16_t error;
static int32_t baroGroundPressure;
static uint16_t previousT;
uint16_t currentT = micros();
uint16_t dTime;
float invG;
int16_t error16;
int16_t baroVel;
int16_t accZ;
static int16_t accZoffset = 0; // = acc_1G*6; //58 bytes saved and convergence is fast enough to omit init
static float vel = 0.0f;
static int32_t lastBaroAlt;
float baroVel;
int16_t vel_tmp;
if ((int32_t)(currentTime - deadLine) < UPDATE_INTERVAL)
dTime = currentT - previousT;
if (dTime < UPDATE_INTERVAL)
return 0;
dTime = currentTime - deadLine;
deadLine = currentTime;
// **** Alt. Set Point stabilization PID ****
baroHistTab[baroHistIdx] = BaroAlt / 10;
baroHigh += baroHistTab[baroHistIdx];
baroHigh -= baroHistTab[(baroHistIdx + 1) % cfg.baro_tab_size];
previousT = currentT;
baroHistIdx++;
if (baroHistIdx == cfg.baro_tab_size)
baroHistIdx = 0;
if (calibratingB > 0) {
baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1);
calibratingB--;
}
EstAlt = EstAlt * cfg.baro_noise_lpf + (baroHigh * 10.0f / (cfg.baro_tab_size - 1)) * (1.0f - cfg.baro_noise_lpf); // additional LPF to reduce baro noise
// pressure relative to ground pressure with temperature compensation (fast!)
// baroGroundPressure is not supposed to be 0 here
// see: https://code.google.com/p/ardupilot-mega/source/browse/libraries/AP_Baro/AP_Baro.cpp
BaroAlt = log(baroGroundPressure * (cfg.baro_tab_size - 1) / (float)baroPressureSum) * (baroTemperature + 27315) * 29.271267f; // in cemtimeter
EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise
// P
error = constrain(AltHold - EstAlt, -300, 300);
error = applyDeadband16(error, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error / 100), -150, +150);
//P
error16 = constrain(AltHold - EstAlt, -300, 300);
applyDeadband(error16, 10); // remove small P parametr to reduce noise near zero position
BaroPID = constrain((cfg.P8[PIDALT] * error16 >> 7), -150, +150);
// I
errorAltitudeI += error * cfg.I8[PIDALT] / 50;
//I
errorAltitudeI += cfg.I8[PIDALT] * error16 >> 6;
errorAltitudeI = constrain(errorAltitudeI, -30000, 30000);
BaroPID += (errorAltitudeI / 500); // I in range +/-60
BaroPID += errorAltitudeI >> 9; // I in range +/-60
// projection of ACC vector to global Z, with 1G subtructed
// Math: accZ = A * G / |G| - 1G
invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
accZ = applyDeadband16(accZ, acc_1G / cfg.accz_deadband);
debug[0] = accZ;
accZ = (accSmooth[ROLL] * EstG.V.X + accSmooth[PITCH] * EstG.V.Y + accSmooth[YAW] * EstG.V.Z) * invG;
if (!f.ARMED) {
accZoffset -= accZoffset >> 3;
accZoffset += accZ;
}
accZ -= accZoffset >> 3;
applyDeadband(accZ, cfg.accz_deadband);
// Integrator - velocity, cm/sec
vel += accZ * accVelScale * dTime;
baroVel = (EstAlt - lastBaroAlt) / (dTime / 1000000.0f);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero
baroVel = (EstAlt - lastBaroAlt) * 1000000.0f / dTime;
lastBaroAlt = EstAlt;
debug[1] = baroVel;
// apply Complimentary Filter to keep near zero caluculated velocity based on baro velocity
vel = vel * cfg.baro_cf + baroVel * (1.0f - cfg.baro_cf);
// vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s
debug[2] = vel;
// debug[3] = applyDeadbandFloat(vel, 5);
baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
applyDeadband(baroVel, 10); // to reduce noise near zero
// apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity).
// By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
vel = vel * 0.985f + baroVel * 0.015f;
// D
BaroPID -= constrain(cfg.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
debug[3] = BaroPID;
vel_tmp = vel;
applyDeadband(vel_tmp, 5);
vario = vel_tmp;
BaroPID -= constrain(cfg.D8[PIDALT] * vel_tmp >> 4, -150, 150);
return 1;
}
#endif /* BARO */

8
src/mw.h
View File

@ -113,11 +113,6 @@ enum {
#define THR_CE (3 << (2 * THROTTLE))
#define THR_HI (2 << (2 * THROTTLE))
#define min(a, b) ((a) < (b) ? (a) : (b))
#define max(a, b) ((a) > (b) ? (a) : (b))
#define abs(x) ((x) > 0 ? (x) : -(x))
#define constrain(amt, low, high) ((amt) < (low) ? (low) : ((amt) > (high) ? (high) : (amt)))
#define SERVO_NORMAL (1)
#define SERVO_REVERSE (-1)
@ -322,6 +317,9 @@ extern uint16_t calibratingB;
extern uint16_t calibratingG;
extern int16_t heading;
extern int16_t annex650_overrun_count;
extern int32_t baroPressure;
extern int32_t baroTemperature;
extern int32_t baroPressureSum;
extern int32_t BaroAlt;
extern int16_t sonarAlt;
extern int32_t EstAlt;

134
src/sensors.c
View File

@ -262,42 +262,47 @@ void ACC_getADC(void)
}
#ifdef BARO
void Baro_Common(void)
{
static int32_t baroHistTab[BARO_TAB_SIZE_MAX];
static int baroHistIdx;
int indexplus1;
indexplus1 = (baroHistIdx + 1);
if (indexplus1 == cfg.baro_tab_size)
indexplus1 = 0;
baroHistTab[baroHistIdx] = baroPressure;
baroPressureSum += baroHistTab[baroHistIdx];
baroPressureSum -= baroHistTab[indexplus1];
baroHistIdx = indexplus1;
}
int Baro_update(void)
{
static uint32_t baroDeadline = 0;
static uint8_t state = 0;
int32_t pressure;
static int state = 0;
if ((int32_t)(currentTime - baroDeadline) < 0)
return 0;
baroDeadline = currentTime;
switch (state) {
case 0:
baro.start_ut();
state++;
baroDeadline += baro.ut_delay;
break;
case 1:
baro.get_ut();
state++;
break;
case 2:
baro.start_up();
state++;
baroDeadline += baro.up_delay;
break;
case 3:
baro.get_up();
pressure = baro.calculate();
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
state = 0;
baroDeadline += baro.repeat_delay;
break;
}
return 1;
if (state) {
baro.get_up();
baro.start_ut();
baroDeadline += baro.ut_delay;
baro.calculate(&baroPressure, &baroTemperature);
state = 0;
return 2;
} else {
baro.get_ut();
baro.start_up();
Baro_Common();
state = 1;
baroDeadline += baro.up_delay;
return 1;
}
}
#endif /* BARO */
@ -412,77 +417,10 @@ static void Mag_getRawADC(void)
void Mag_init(void)
{
uint8_t calibration_gain = 0x60; // HMC5883
#if 1
uint32_t numAttempts = 0, good_count = 0;
bool success = false;
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();
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;
}
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[PITCH] = 1160.0f / abs(magADC[PITCH]);
magCal[YAW] = 1080.0f / abs(magADC[YAW]);
hmc5883lFinishCal();
#endif
// initialize and calibration. turn on led during mag calibration (calibration routine blinks it)
LED1_ON;
hmc5883lInit(magCal);
LED1_OFF;
magInit = 1;
}

Write Preview
Loading…
Cancel
Save