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 - -#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) 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(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) 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 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; } 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; - - // **** 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 */ 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; }