From 98d0581ac233fa28828538846d688af3269a5756 Mon Sep 17 00:00:00 2001 From: "timecop@gmail.com" Date: Sat, 23 Mar 2013 15:58:18 +0000 Subject: [PATCH] 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 --- src/board.h | 8 ++- src/config.c | 2 +- src/drv_bmp085.c | 41 ++++++-------- src/drv_hmc5883l.c | 121 ++++++++++++++++++++++++++-------------- src/drv_hmc5883l.h | 4 +- src/drv_ms5611.c | 41 +++++++------- src/imu.c | 102 ++++++++++++++++++++-------------- src/mw.h | 8 +-- src/sensors.c | 134 ++++++++++++--------------------------------- 9 files changed, 226 insertions(+), 235 deletions(-) diff --git a/src/board.h b/src/board.h index 30bd3a7f3..798cea628 100755 --- a/src/board.h +++ b/src/board.h @@ -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; diff --git a/src/config.c b/src/config.c index fdb7b10a3..2e09c8975 100755 --- a/src/config.c +++ b/src/config.c @@ -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; diff --git a/src/drv_bmp085.c b/src/drv_bmp085.c index 31571b2c5..2c7979901 100755 --- a/src/drv_bmp085.c +++ b/src/drv_bmp085.c @@ -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 + temperature = ((bmp085.param_b5 * 10 + 8) >> 4); // temperature in 0.01°C (make same as MS5611) -#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 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) diff --git a/src/drv_hmc5883l.c b/src/drv_hmc5883l.c index 31bfd8a12..91b3bae72 100755 --- a/src/drv_hmc5883l.c +++ b/src/drv_hmc5883l.c @@ -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(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); - i2cWrite(MAG_ADDRESS, ConfigRegA, SampleAveraging_8 << 5 | DataOutputRate_75HZ << 2 | NormalOperation); - delay(50); -} + 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) diff --git a/src/drv_hmc5883l.h b/src/drv_hmc5883l.h index ac87f1dae..92096fa1a 100755 --- a/src/drv_hmc5883l.h +++ b/src/drv_hmc5883l.h @@ -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); diff --git a/src/drv_ms5611.c b/src/drv_ms5611.c index b53e34a75..612f4329d 100644 --- a/src/drv_ms5611.c +++ b/src/drv_ms5611.c @@ -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 temp, off2 = 0, sens2 = 0, delt; + int32_t press; - 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); + 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 (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; + 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; } diff --git a/src/imu.c b/src/imu.c index 64da1462f..175f43912 100755 --- a/src/imu.c +++ b/src/imu.c @@ -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; + previousT = currentT; - // **** Alt. Set Point stabilization PID **** - baroHistTab[baroHistIdx] = BaroAlt / 10; - baroHigh += baroHistTab[baroHistIdx]; - baroHigh -= baroHistTab[(baroHistIdx + 1) % cfg.baro_tab_size]; + if (calibratingB > 0) { + baroGroundPressure = baroPressureSum / (cfg.baro_tab_size - 1); + calibratingB--; + } - baroHistIdx++; - if (baroHistIdx == cfg.baro_tab_size) - baroHistIdx = 0; + // 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 - 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 + //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); - // 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); - - // 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 */ diff --git a/src/mw.h b/src/mw.h index b91dc8561..3aeb0aa5b 100755 --- a/src/mw.h +++ b/src/mw.h @@ -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; diff --git a/src/sensors.c b/src/sensors.c index 82eb2cf80..95fd779ec 100755 --- a/src/sensors.c +++ b/src/sensors.c @@ -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; }