Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-07 08:23:56 -04:00
Code Issues 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 c8e9d57567
commit 486c56ce41
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 // Used to control Axis lock
int32_t roll_axis; int32_t roll_axis;
int32_t pitch_axis; int32_t pitch_axis;
// Filters // Filters
AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration AverageFilterInt32_Size3 roll_rate_d_filter; // filtered acceleration
AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration AverageFilterInt32_Size3 pitch_rate_d_filter; // filtered pitch acceleration
// Barometer filter
AverageFilterInt32_Size5 baro_filter; // filtered pitch acceleration
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
// Circle Mode / Loiter control // Circle Mode / Loiter control
//////////////////////////////////////////////////////////////////////////////// ////////////////////////////////////////////////////////////////////////////////
@ -1070,8 +1074,8 @@ static void medium_loop()
// ----------------------- // -----------------------
arm_motors(); arm_motors();
// Do an extra baro read // Do an extra baro read for Temp sensing
// --------------------- // ---------------------------------------
#if HIL_MODE != HIL_MODE_ATTITUDE #if HIL_MODE != HIL_MODE_ATTITUDE
barometer.read(); barometer.read();
#endif #endif
@ -1884,7 +1888,8 @@ static void update_altitude()
// This is real life // This is real life
// read in Actual Baro Altitude // 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 // calc the vertical accel rate
int temp = (baro_alt - old_baro_alt) * 10; 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); //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); delay(20);
#if HIL_MODE == HIL_MODE_SENSORS #if HIL_MODE == HIL_MODE_SENSORS
@ -44,15 +44,25 @@ static void init_barometer(void)
// Get initial data from absolute pressure sensor // Get initial data from absolute pressure sensor
barometer.read(); barometer.read();
ground_pressure = barometer.get_pressure(); ground_pressure = baro_filter.apply(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); //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) static void reset_baro(void)
{ {
ground_pressure = barometer.get_pressure(); ground_pressure = baro_filter.apply(barometer.get_pressure());
ground_temperature = barometer.get_temperature(); ground_temperature = barometer.get_temperature();
} }
@ -61,7 +71,7 @@ static int32_t read_barometer(void)
float x, scaling, temp; float x, scaling, temp;
barometer.read(); 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); //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(); Command_ReadTemp();
BMP085_State = 1; BMP085_State = 1;
// init raw temo
RawTemp = 0;
healthy = true; healthy = true;
return 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); RawPress = (((uint32_t)buf[0] << 16) | ((uint32_t)buf[1] << 8) | ((uint32_t)buf[2])) >> (8 - OVERSAMPLING);
if (_offset_press == 0){ //if (_offset_press == 0){
_offset_press = RawPress; // _offset_press = RawPress;
RawPress = 0; // RawPress = 0;
} else{ //} else{
RawPress -= _offset_press; // RawPress -= _offset_press;
} //}
// filter // filter
_press_filter[_press_index++] = RawPress; //_press_filter[_press_index++] = RawPress;
if(_press_index >= PRESS_FILTER_SIZE) //if(_press_index >= PRESS_FILTER_SIZE)
_press_index = 0; // _press_index = 0;
RawPress = 0; //RawPress = 0;
// sum our filter // sum our filter
for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){ //for (uint8_t i = 0; i < PRESS_FILTER_SIZE; i++){
RawPress += _press_filter[i]; // RawPress += _press_filter[i];
} //}
// grab result // grab result
RawPress /= PRESS_FILTER_SIZE; //RawPress /= PRESS_FILTER_SIZE;
RawPress += _offset_press; //RawPress += _offset_press;
} }
// Send Command to Read Temperature // Send Command to Read Temperature
@ -222,10 +225,11 @@ void AP_Baro_BMP085::ReadTemp()
_temp_sensor = buf[0]; _temp_sensor = buf[0];
_temp_sensor = (_temp_sensor << 8) | buf[1]; _temp_sensor = (_temp_sensor << 8) | buf[1];
if (RawTemp == 0) if (RawTemp == 0){
RawTemp = _temp_sensor; RawTemp = _temp_sensor;
}else{
RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99; RawTemp = (float)_temp_sensor * .01 + (float)RawTemp * .99;
}
} }
// Calculate Temperature and Pressure in real units. // Calculate Temperature and Pressure in real units.