aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Consolidated Barometer pressure sensing to a single filter based on Randy's new filter class.

Browse Source
This commit is contained in:
Jason Short 2012-03-08 23:13:04 -08:00
parent df812dd78a
commit 13e9608fb8
3 changed files with 46 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
ArduCopter/ArduCopter.pde
View File

@ -502,10 +502,14 @@ static int32_t initial_simple_bearing;
// Used to control Axis lock
int32_t roll_axis;
int32_t pitch_axis;
// Filters
AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration
AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration
// Barometer filter
AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration
////////////////////////////////////////////////////////////////////////////////
// Circle Mode / Loiter control
////////////////////////////////////////////////////////////////////////////////
@ -1070,8 +1074,8 @@ static void medium_loop()
// -----------------------
arm_motors();
// Do an extra baro read
// ---------------------
// Do an extra baro read for Temp sensing
// ---------------------------------------
#if HIL_MODE != HIL_MODE_ATTITUDE
barometer.read();
#endif
@ -1884,7 +1888,8 @@ static void update_altitude()
// This is real life
// read in Actual Baro Altitude
baro_alt = (baro_alt + read_barometer()) >> 1;
baro_alt = read_barometer();
//Serial.printf("baro_alt: %d \n", baro_alt);
// calc the vertical accel rate
int temp = (baro_alt - old_baro_alt) * 10;

22
ArduCopter/sensors.pde
View File

@ -35,7 +35,7 @@ static void init_barometer(void)
//Serial.printf("init %ld, %d, -, %ld, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press);
}
for(i = 0; i < 20; i++){
for(i = 0; i < 40; i++){
delay(20);
#if HIL_MODE == HIL_MODE_SENSORS
@ -44,15 +44,25 @@ static void init_barometer(void)
// Get initial data from absolute pressure sensor
barometer.read();
ground_pressure = barometer.get_pressure();
ground_temperature = (ground_temperature * 7 + barometer.get_temperature()) / 8;
//Serial.printf("init %ld, %d, -, %ld, %ld, -, %d, %ld\n", barometer.RawTemp, barometer.Temp, barometer.RawPress, barometer.Press, ground_temperature, ground_pressure);
ground_pressure = baro_filter.apply(barometer.get_pressure());
//Serial.printf("t: %ld, p: %d\n", ground_pressure, ground_temperature);
/*Serial.printf("init %d, %d, -, %d, %d, -, %d, %d\n",
barometer.RawTemp,
barometer.Temp,
barometer.RawPress,
barometer.Press,
ground_temperature,
ground_pressure);*/
}
// save our ground temp
ground_temperature = barometer.get_temperature();
}
static void reset_baro(void)
{
ground_pressure = barometer.get_pressure();
ground_pressure = baro_filter.apply(barometer.get_pressure());
ground_temperature = barometer.get_temperature();
}
@ -61,7 +71,7 @@ static int32_t read_barometer(void)
float x, scaling, temp;
barometer.read();
float abs_pressure = barometer.get_pressure();
float abs_pressure = baro_filter.apply(barometer.get_pressure());
//Serial.printf("%ld, %ld, %ld, %ld\n", barometer.RawTemp, barometer.RawPress, barometer.Press, abs_pressure);

40
libraries/AP_Baro/AP_Baro_BMP085.cpp
View File

@ -94,6 +94,9 @@ bool AP_Baro_BMP085::init( AP_PeriodicProcess * scheduler )
Command_ReadTemp();
BMP085_State = 1;
// init raw temo
RawTemp = 0;
healthy = true;
return true;
}
@ -170,29 +173,29 @@ void AP_Baro_BMP085::ReadPress()
RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
if (_offset_press == 0){
_offset_press = RawPress;
RawPress = 0;
} else{
RawPress -= _offset_press;
}
//if (_offset_press == 0){
// _offset_press = RawPress;
// RawPress = 0;
//} else{
// RawPress -= _offset_press;
//}
// filter
_press_filter[_press_index++] = RawPress;
//_press_filter[_press_index++] = RawPress;
if(_press_index >= PRESS_FILTER_SIZE)
_press_index = 0;
//if(_press_index >= PRESS_FILTER_SIZE)
// _press_index = 0;
RawPress = 0;
//RawPress = 0;
// sum our filter
for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i];
}
//for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
// RawPress += _press_filter[i];
//}
// grab result
RawPress /= PRESS_FILTER_SIZE;
RawPress += _offset_press;
//RawPress /= PRESS_FILTER_SIZE;
//RawPress += _offset_press;
}
// Send Command to Read Temperature
@ -222,10 +225,11 @@ void AP_Baro_BMP085::ReadTemp()
_temp_sensor = buf[0];
_temp_sensor = (_temp_sensor << 8) | buf[1];
if (RawTemp == 0)
if (RawTemp == 0){
RawTemp = _temp_sensor;
RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
}else{
RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
}
}
// Calculate Temperature and Pressure in real units.