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:
parent
04072accd4
commit
8c16f614f2
121
flight/INS/ins.c
121
flight/INS/ins.c
@ -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
|
||||
|
@ -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];
|
||||
|
Loading…
x
Reference in New Issue
Block a user