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
This commit is contained in:
timecop@gmail.com
parent
491b3627f6
commit
98d0581ac2
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
hmc5883lRead(magADC);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
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);
|
||||
// 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);
|
||||
// set gains for calibration
|
||||
i2cWrite(MAG_ADDRESS, ConfigRegB, calibration_gain);
|
||||
i2cWrite(MAG_ADDRESS, ModeRegister, SingleConversion);
|
||||
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;
|
||||
}
|
||||
|
||||
void hmc5883lFinishCal(void)
|
||||
{
|
||||
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]);
|
||||
|
||||
// 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)
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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,23 +150,23 @@ 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;
|
||||
|
|
@ -175,6 +174,10 @@ static int32_t ms5611_calculate(void)
|
|||
}
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
96
src/imu.c
96
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,71 +353,86 @@ 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;
|
||||
|
||||
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);
|
||||
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;
|
||||
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;
|
||||
}
|
||||
|
|
|
|||
8
src/mw.h
8
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;
|
||||
|
|
|
|||
128
src/sensors.c
128
src/sensors.c
|
|
@ -262,43 +262,48 @@ 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:
|
||||
if (state) {
|
||||
baro.get_up();
|
||||
pressure = baro.calculate();
|
||||
BaroAlt = (1.0f - pow(pressure / 101325.0f, 0.190295f)) * 4433000.0f; // centimeter
|
||||
baro.start_ut();
|
||||
baroDeadline += baro.ut_delay;
|
||||
baro.calculate(&baroPressure, &baroTemperature);
|
||||
state = 0;
|
||||
baroDeadline += baro.repeat_delay;
|
||||
break;
|
||||
}
|
||||
|
||||
return 2;
|
||||
} else {
|
||||
baro.get_ut();
|
||||
baro.start_up();
|
||||
Baro_Common();
|
||||
state = 1;
|
||||
baroDeadline += baro.up_delay;
|
||||
return 1;
|
||||
}
|
||||
}
|
||||
#endif /* BARO */
|
||||
|
||||
typedef struct stdev_t
|
||||
|
|
@ -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;
|
||||
}
|
||||
|
||||
|
|
|
|||
Loading…
Reference in New Issue