1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-20 10:54:14 +01:00

INS: Use the data from the BMP085 and feed into EKF

This commit is contained in:
James Cotton 2011-08-22 02:32:40 -05:00
parent 04072accd4
commit 8c16f614f2
2 changed files with 63 additions and 68 deletions

View File

@ -66,7 +66,10 @@ volatile int8_t ahrs_algorithm;
/* Data accessors */
void adc_callback(float *);
void process_mag_data();
void get_mag_data();
void get_baro_data();
void get_accel_gyro_data();
void reset_values();
void calibrate_sensors(void);
@ -84,7 +87,6 @@ void calibration(float result[3], float scale[3][4], float arg[3]);
extern void PIOS_Board_Init(void);
static void panic(uint32_t blinks);
void simple_update();
bool get_accel_gyro_data();
/* Bootloader related functions and var*/
void firmwareiapobj_callback(AhrsObjHandle obj);
@ -129,7 +131,7 @@ typedef enum { INS_IDLE, INS_DATA_READY, INS_PROCESSING } states;
uint32_t total_conversion_blocks;
float altitude;
float pressure, altitude;
int32_t dr;
@ -166,6 +168,9 @@ int main()
PIOS_LED_On(LED1);
PIOS_LED_Off(LED2);
// Kickstart BMP085 measurements until driver improved
PIOS_BMP085_StartADC(TemperatureConv);
// Flash warning light while trying to connect
uint32_t time_val1 = PIOS_DELAY_GetRaw();
@ -206,11 +211,9 @@ int main()
loop_time = PIOS_DELAY_DiffuS(time_val1);
time_val1 = PIOS_DELAY_GetRaw();
// This function blocks till data avilable
get_accel_gyro_data();
// Get any mag data available
process_mag_data();
get_accel_gyro_data(); // This function blocks till data avilable
get_mag_data();
get_baro_data();
status.IdleTimePerCycle = PIOS_DELAY_DiffuS(time_val1);
@ -340,7 +343,7 @@ int32_t accel_accum[3] = {0, 0, 0};
int32_t gyro_accum[3] = {0,0,0};
float scaling;
bool get_accel_gyro_data()
void get_accel_gyro_data()
{
int32_t read_good;
int32_t count;
@ -397,66 +400,24 @@ bool get_accel_gyro_data()
raw.gyros[0] = gyro_data.filtered.x * RAD_TO_DEG;
raw.gyros[1] = gyro_data.filtered.y * RAD_TO_DEG;
raw.gyros[2] = gyro_data.filtered.z * RAD_TO_DEG;
AHRSSettingsData settings;
AHRSSettingsGet(&settings);
if (settings.BiasCorrectedRaw == AHRSSETTINGS_BIASCORRECTEDRAW_TRUE)
{
raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG;
raw.gyros[1] -= Nav.gyro_bias[1] * RAD_TO_DEG;
raw.gyros[2] -= Nav.gyro_bias[2] * RAD_TO_DEG;
raw.accels[0] -= Nav.accel_bias[0];
raw.accels[1] -= Nav.accel_bias[1];
raw.accels[2] -= Nav.accel_bias[2];
}
raw.magnetometers[0] = mag_data.scaled.axis[0];
raw.magnetometers[1] = mag_data.scaled.axis[1];
raw.magnetometers[2] = mag_data.scaled.axis[2];
AttitudeRawSet(&raw);
return true;
}
/**
* @brief Perform calibration of a 3-axis field sensor using an affine transformation
* matrix.
*
* Computes result = scale * arg.
*
* @param result[out] The three-axis resultant field.
* @param scale[in] The 4x4 affine transformation matrix. The fourth row is implicitly
* [0 0 0 1]
* @param arg[in] The 3-axis input field. The 'w' component is implicitly 1.
*/
void calibration(float result[3], float scale[3][4], float arg[3])
{
for (int row = 0; row < 3; ++row) {
result[row] = 0.0f;
int col;
for (col = 0; col < 3; ++col) {
result[row] += scale[row][col] * arg[col];
}
// fourth column: arg has an implicit w value of 1.0f.
result[row] += scale[row][col];
}
}
/**
* @brief Scale an affine transformation matrix by a rotation, defined by a
* rotation vector. scale <- rotation * scale
*
* @param scale[in,out] The affine transformation matrix to be rotated
* @param rotation[in] The rotation vector defining the rotation
*/
void affine_rotate(float scale[3][4], float rotation[3])
{
// Rotate the scale factor matrix in-place
float rmatrix[3][3];
Rv2Rot(rotation, rmatrix);
float ret[3][4];
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
ret[row][col] = 0.0f;
for (int i = 0; i < 3; ++i) {
ret[row][col] += rmatrix[row][i] * scale[i][col];
}
}
}
// copy output to argument
for (int row = 0; row < 3; ++row) {
for (int col = 0; col < 4; ++col) {
scale[row][col] = ret[row][col];
}
}
}
/**
@ -468,7 +429,7 @@ void affine_rotate(float scale[3][4], float rotation[3])
* cannot be interpreted. In addition the mag data is not used for the first
* five seconds to allow the filter to start to converge
*/
void process_mag_data()
void get_mag_data()
{
// Get magnetic readings
// For now don't use mags until the magnetic field is set AND until 5 seconds
@ -493,6 +454,34 @@ void process_mag_data()
}
}
/**
* @brief Get the barometer data
* @return none
*/
uint32_t baro_conversions = 0;
void get_baro_data()
{
int32_t retval = PIOS_BMP085_ReadADC();
if (retval == 0) { // Conversion completed
pressure = PIOS_BMP085_GetPressure();
altitude = 44330.0 * (1.0 - powf(pressure / BMP085_P0, (1.0 / 5.255)));
BaroAltitudeData data;
BaroAltitudeGet(&data);
data.Altitude = altitude;
data.Pressure = pressure / 1000.0f;
data.Temperature = PIOS_BMP085_GetTemperature() / 10.0f; // Convert to deg C
BaroAltitudeSet(&data);
if((baro_conversions++) % 2)
PIOS_BMP085_StartADC(PressureConv);
else
PIOS_BMP085_StartADC(TemperatureConv);
altitude_data.altitude = data.Altitude;
altitude_data.updated = true;
}
}
/**
* @brief Assumes board is not moving computes biases and variances of sensors

View File

@ -91,8 +91,14 @@ void PIOS_BMP085_Init(const struct pios_bmp085_cfg * cfg)
/* Read all 22 bytes of calibration data in one transfer, this is a very optimized way of doing things */
uint8_t Data[BMP085_CALIB_LEN];
while (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) != 0)
continue;
bool good_cal = false;
while(!good_cal) {
good_cal = (PIOS_BMP085_Read(BMP085_CALIB_ADDR, Data, BMP085_CALIB_LEN) == 0);
// Check none of the calibration is zero to ensure calibration is good
for(uint8_t i = 0; i < BMP085_CALIB_LEN; i += 2)
good_cal &= (Data[i] != 0) || (Data[i+1] != 0);
}
/* Parameters AC1-AC6 */
CalibData.AC1 = (Data[0] << 8) | Data[1];