mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-167 OP-157 AHRS/Calibration: Added mag scale and really cleaned up calibration.
Calibration should take less time now too (using second moments to estimate variance in one pass). Now need to change to multiple messages to get the calibration in to keep the request message size minimal. Also currently running sensor calibrate doesn't store the gyro bias so if you want to use this you'll have to tweak it manually. I'll fix that step tomorrow. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1741 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124
peabody124
parent
fd370f571d
commit
9b28f2d72c
@ -1,6 +1,6 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
@ -40,8 +40,6 @@
|
||||
#include "insgps.h"
|
||||
#include "CoordinateConversions.h"
|
||||
|
||||
volatile enum algorithms ahrs_algorithm;
|
||||
|
||||
// For debugging the raw sensors
|
||||
//#define DUMP_RAW
|
||||
//#define DUMP_FRIENDLY
|
||||
@ -58,43 +56,31 @@ extern float Q[NUMW], R[NUMV]; // input noise and measurement noise variances
|
||||
extern float K[NUMX][NUMV]; // feedback gain matrix
|
||||
#endif
|
||||
|
||||
volatile enum algorithms ahrs_algorithm;
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Definitions
|
||||
* @addtogroup AHRS_Structures Local Structres
|
||||
* @{
|
||||
*/
|
||||
// Currently analog acquistion hard coded at 480 Hz
|
||||
#define ADC_RATE (4*480)
|
||||
#define EKF_RATE (ADC_RATE / adc_oversampling)
|
||||
#define VDD 3.3 /* supply voltage for ADC */
|
||||
#define FULL_RANGE 4096 /* 12 bit ADC */
|
||||
#define ACCEL_RANGE 2 /* adjustable by FS input */
|
||||
#define ACCEL_GRAVITY 9.81 /* m s^-1 */
|
||||
#define ACCEL_SENSITIVITY ( VDD / 5 )
|
||||
#define ACCEL_SCALE ( (VDD / FULL_RANGE) / ACCEL_SENSITIVITY * 2 / ACCEL_RANGE * ACCEL_GRAVITY )
|
||||
#define ACCEL_OFFSET -2048
|
||||
|
||||
#define GYRO_SENSITIVITY ( 2.0 / 1000 ) /* 2 mV / (deg s^-1) */
|
||||
#define RAD_PER_DEGREE ( M_PI / 180 )
|
||||
#define GYRO_SCALE ( (VDD / FULL_RANGE) / GYRO_SENSITIVITY * RAD_PER_DEGREE )
|
||||
#define GYRO_OFFSET -1675 /* From data sheet, zero accel output is 1.35 v */
|
||||
|
||||
#define MAX_IDLE_COUNT 65e3
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
|
||||
/**
|
||||
* @addtogroup AHRS_Local Local Variables
|
||||
* @{
|
||||
*/
|
||||
//! Contains the data from the mag sensor chip
|
||||
struct mag_sensor {
|
||||
uint8_t id[4];
|
||||
uint8_t updated;
|
||||
struct {
|
||||
int16_t axis[3];
|
||||
} raw;
|
||||
};
|
||||
struct {
|
||||
float axis[3];
|
||||
} scaled;
|
||||
struct {
|
||||
float bias[3];
|
||||
float scale[3];
|
||||
float variance[3];
|
||||
} calibration;
|
||||
} mag_data;
|
||||
|
||||
//! Contains the data from the accelerometer
|
||||
struct accel_sensor {
|
||||
struct {
|
||||
uint16_t x;
|
||||
@ -106,8 +92,14 @@ struct accel_sensor {
|
||||
float y;
|
||||
float z;
|
||||
} filtered;
|
||||
};
|
||||
struct {
|
||||
float bias[3];
|
||||
float scale[3];
|
||||
float variance[3];
|
||||
} calibration;
|
||||
} accel_data;
|
||||
|
||||
//! Contains the data from the gyro
|
||||
struct gyro_sensor {
|
||||
struct {
|
||||
uint16_t x;
|
||||
@ -119,12 +111,18 @@ struct gyro_sensor {
|
||||
float y;
|
||||
float z;
|
||||
} filtered;
|
||||
struct {
|
||||
float bias[3];
|
||||
float scale[3];
|
||||
float variance[3];
|
||||
} calibration;
|
||||
struct {
|
||||
uint16_t xy;
|
||||
uint16_t z;
|
||||
} temp;
|
||||
};
|
||||
} gyro_data;
|
||||
|
||||
//! Conains the current estimate of the attitude
|
||||
struct attitude_solution {
|
||||
struct {
|
||||
float q1;
|
||||
@ -132,27 +130,22 @@ struct attitude_solution {
|
||||
float q3;
|
||||
float q4;
|
||||
} quaternion;
|
||||
};
|
||||
} attitude_data;
|
||||
|
||||
//! Contains data from the altitude sensor
|
||||
struct altitude_sensor {
|
||||
float altitude;
|
||||
bool updated;
|
||||
};
|
||||
} altitude_data;
|
||||
|
||||
//! Contains data from the GPS (via the SPI link)
|
||||
struct gps_sensor {
|
||||
float NED[3];
|
||||
float heading;
|
||||
float groundspeed;
|
||||
float quality;
|
||||
bool updated;
|
||||
};
|
||||
|
||||
struct mag_sensor mag_data;
|
||||
volatile struct accel_sensor accel_data;
|
||||
volatile struct gyro_sensor gyro_data;
|
||||
volatile struct altitude_sensor altitude_data;
|
||||
struct gps_sensor gps_data;
|
||||
volatile struct attitude_solution attitude_data;
|
||||
} gps_data;
|
||||
|
||||
/**
|
||||
* @}
|
||||
@ -163,6 +156,7 @@ void process_spi_request(void);
|
||||
void downsample_data(void);
|
||||
void calibrate_sensors(void);
|
||||
void converge_insgps();
|
||||
void reset_values();
|
||||
|
||||
volatile uint32_t last_counter_idle_start = 0;
|
||||
volatile uint32_t last_counter_idle_end = 0;
|
||||
@ -175,41 +169,15 @@ uint32_t counter_val;
|
||||
* @{
|
||||
* Public data. Used by both EKF and the sender
|
||||
*/
|
||||
//! Accelerometer variance after filter from OP or calibrate_sensors
|
||||
float accel_var[3] = { 1, 1, 1 };
|
||||
|
||||
//! Gyro variance after filter from OP or calibrate sensors
|
||||
float gyro_var[3] = { 1, 1, 1 };
|
||||
|
||||
//! Accelerometer scale after calibration
|
||||
float accel_scale[3] = { ACCEL_SCALE, ACCEL_SCALE, ACCEL_SCALE };
|
||||
|
||||
//! Gyro scale after calibration
|
||||
float gyro_scale[3] = { GYRO_SCALE, GYRO_SCALE, GYRO_SCALE };
|
||||
|
||||
//! Magnetometer variance from OP or calibrate sensors
|
||||
float mag_var[3] = { 1, 1, 1 };
|
||||
|
||||
//! Accelerometer bias from OP or calibrate sensors
|
||||
int16_t accel_bias[3] = { ACCEL_OFFSET, ACCEL_OFFSET, ACCEL_OFFSET };
|
||||
|
||||
//! Gyroscope bias term from OP or calibrate sensors
|
||||
int16_t gyro_bias[3] = { 0, 0, 0 };
|
||||
|
||||
//! Magnetometer bias (direction) from OP or calibrate sensors
|
||||
int16_t mag_bias[3] = { 0, 0, 0 };
|
||||
|
||||
//! Filter coefficients used in decimation. Limited order so filter can't run between samples
|
||||
int16_t fir_coeffs[50];
|
||||
//! Home location in ECEF coordinates
|
||||
double BaseECEF[3] = { 0, 0, 0 };
|
||||
|
||||
//! Rotation matrix from LLA to Rne
|
||||
float Rne[3][3];
|
||||
//! Indicates the communications are requesting a calibration
|
||||
uint8_t calibration_pending = FALSE;
|
||||
|
||||
//! The oversampling rate, ekf is 2k / this
|
||||
static uint8_t adc_oversampling = 25;
|
||||
|
||||
/**
|
||||
* @}
|
||||
*/
|
||||
@ -254,6 +222,7 @@ int main()
|
||||
PIOS_SPI_Init();
|
||||
|
||||
lfsm_init();
|
||||
reset_values();
|
||||
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
@ -292,7 +261,7 @@ int main()
|
||||
result +=
|
||||
PIOS_COM_SendBuffer(PIOS_COM_AUX,
|
||||
(uint8_t *) & valid_data_buffer[0],
|
||||
ADC_OVERSAMPLE *
|
||||
adc_oversampling *
|
||||
ADC_CONTINUOUS_CHANNELS *
|
||||
sizeof(valid_data_buffer[0]));
|
||||
if (result == 0)
|
||||
@ -320,6 +289,9 @@ int main()
|
||||
// Get magnetic readings
|
||||
if (PIOS_HMC5843_NewDataAvailable()) {
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
mag_data.updated = 1;
|
||||
}
|
||||
#endif
|
||||
@ -339,22 +311,8 @@ int main()
|
||||
|
||||
downsample_data();
|
||||
|
||||
/***************** SEND BACK SOME RAW DATA ************************/
|
||||
// Hacky - grab one sample from buffer to populate this. Need to send back
|
||||
// all raw data if it's happening
|
||||
accel_data.raw.x = valid_data_buffer[0];
|
||||
accel_data.raw.y = valid_data_buffer[2];
|
||||
accel_data.raw.z = valid_data_buffer[4];
|
||||
|
||||
gyro_data.raw.x = valid_data_buffer[1];
|
||||
gyro_data.raw.y = valid_data_buffer[3];
|
||||
gyro_data.raw.z = valid_data_buffer[5];
|
||||
|
||||
gyro_data.temp.xy = valid_data_buffer[6];
|
||||
gyro_data.temp.z = valid_data_buffer[7];
|
||||
|
||||
if (ahrs_algorithm == INSGPS_Algo) {
|
||||
/******************** INS ALGORITHM **************************/
|
||||
if (ahrs_algorithm == INSGPS_Algo) {
|
||||
|
||||
// format data for INS algo
|
||||
gyro[0] = gyro_data.filtered.x;
|
||||
@ -365,13 +323,12 @@ int main()
|
||||
accel[2] = accel_data.filtered.z,
|
||||
// Note: The magnetometer driver returns registers X,Y,Z from the chip which are
|
||||
// (left, backward, up). Remapping to (forward, right, down).
|
||||
mag[0] = -(mag_data.raw.axis[1] - mag_bias[1]);
|
||||
mag[1] = -(mag_data.raw.axis[0] - mag_bias[0]);
|
||||
mag[2] = -(mag_data.raw.axis[2] - mag_bias[2]);
|
||||
mag[0] = -mag_data.scaled.axis[1];
|
||||
mag[1] = -mag_data.scaled.axis[0];
|
||||
mag[2] = -mag_data.scaled.axis[2];
|
||||
|
||||
INSStatePrediction(gyro, accel,
|
||||
1 / (float)EKF_RATE);
|
||||
process_spi_request();
|
||||
INSStatePrediction(gyro, accel, 1 / (float)EKF_RATE);
|
||||
process_spi_request(); // get message out quickly
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
|
||||
if (gps_data.updated && gps_data.quality == 1) {
|
||||
@ -383,9 +340,6 @@ int main()
|
||||
gps_data.groundspeed *
|
||||
sin(gps_data.heading * M_PI / 180);
|
||||
|
||||
// Completely unprincipled way to make the position variance
|
||||
// increase as data quality decreases but keep it bounded
|
||||
// Variance becomes 40 m^2 and 40 (m/s)^2 when no gps
|
||||
INSSetPosVelVar(0.004);
|
||||
if (gps_data.updated) {
|
||||
//TOOD: add check for altitude updates
|
||||
@ -537,68 +491,49 @@ int main()
|
||||
*/
|
||||
void downsample_data()
|
||||
{
|
||||
int32_t accel_raw[3], gyro_raw[3];
|
||||
uint16_t i;
|
||||
|
||||
// Get the Y data. Third byte in. Convert to m/s
|
||||
accel_raw[0] = 0;
|
||||
accel_data.filtered.y = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_raw[0] +=
|
||||
(valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] +
|
||||
accel_bias[1]) * fir_coeffs[i];
|
||||
accel_data.filtered.y =
|
||||
(float)accel_raw[0] / (float)fir_coeffs[adc_oversampling] *
|
||||
accel_scale[1];
|
||||
accel_data.filtered.y += valid_data_buffer[0 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.y /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.y = (accel_data.filtered.y * accel_data.calibration.scale[1]) + accel_data.calibration.bias[1];
|
||||
|
||||
// Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
|
||||
accel_raw[1] = 0;
|
||||
accel_data.filtered.x = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_raw[1] +=
|
||||
(valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] +
|
||||
accel_bias[0]) * fir_coeffs[i];
|
||||
accel_data.filtered.x =
|
||||
(float)accel_raw[1] / (float)fir_coeffs[adc_oversampling] *
|
||||
accel_scale[0];
|
||||
accel_data.filtered.x += valid_data_buffer[2 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.x /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.x = (accel_data.filtered.x * accel_data.calibration.scale[0]) + accel_data.calibration.bias[0];
|
||||
|
||||
// Get the Z data. Third byte in. Convert to m/s
|
||||
accel_raw[2] = 0;
|
||||
accel_data.filtered.z = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
accel_raw[2] +=
|
||||
(valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] +
|
||||
accel_bias[2]) * fir_coeffs[i];
|
||||
accel_data.filtered.z =
|
||||
-(float)accel_raw[2] / (float)fir_coeffs[adc_oversampling] *
|
||||
accel_scale[2];
|
||||
accel_data.filtered.z += valid_data_buffer[4 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
accel_data.filtered.z /= (float) fir_coeffs[adc_oversampling];
|
||||
accel_data.filtered.z = (accel_data.filtered.z * accel_data.calibration.scale[2]) + accel_data.calibration.bias[2];
|
||||
|
||||
// Get the X gyro data. Seventh byte in. Convert to deg/s.
|
||||
gyro_raw[0] = 0;
|
||||
gyro_data.filtered.x = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_raw[0] +=
|
||||
(valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] +
|
||||
gyro_bias[0]) * fir_coeffs[i];
|
||||
gyro_data.filtered.x =
|
||||
(float)gyro_raw[0] / (float)fir_coeffs[adc_oversampling] *
|
||||
gyro_scale[0];
|
||||
gyro_data.filtered.x += valid_data_buffer[1 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.x /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.x = (gyro_data.filtered.x * gyro_data.calibration.scale[0]) + gyro_data.calibration.bias[0];
|
||||
|
||||
// Get the Y gyro data. Second byte in. Convert to deg/s.
|
||||
gyro_raw[1] = 0;
|
||||
gyro_data.filtered.y = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_raw[1] +=
|
||||
(valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] +
|
||||
gyro_bias[1]) * fir_coeffs[i];
|
||||
gyro_data.filtered.y =
|
||||
(float)gyro_raw[1] / (float)fir_coeffs[adc_oversampling] *
|
||||
gyro_scale[1];
|
||||
gyro_data.filtered.y += valid_data_buffer[3 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.y /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.y = (gyro_data.filtered.y * gyro_data.calibration.scale[1]) + gyro_data.calibration.bias[1];
|
||||
|
||||
// Get the Z gyro data. Fifth byte in. Convert to deg/s.
|
||||
gyro_raw[2] = 0;
|
||||
gyro_data.filtered.z = 0;
|
||||
for (i = 0; i < adc_oversampling; i++)
|
||||
gyro_raw[2] +=
|
||||
(valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] +
|
||||
gyro_bias[2]) * fir_coeffs[i];
|
||||
gyro_data.filtered.z =
|
||||
(float)gyro_raw[2] / (float)fir_coeffs[adc_oversampling] *
|
||||
gyro_scale[2];
|
||||
gyro_data.filtered.z += valid_data_buffer[5 + i * PIOS_ADC_NUM_PINS] * fir_coeffs[i];
|
||||
gyro_data.filtered.z /= fir_coeffs[adc_oversampling];
|
||||
gyro_data.filtered.z = (gyro_data.filtered.z * gyro_data.calibration.scale[2]) + gyro_data.calibration.bias[2];
|
||||
}
|
||||
|
||||
/**
|
||||
@ -607,19 +542,40 @@ void downsample_data()
|
||||
*
|
||||
* All data is stored in global structures. This function should be called from OP when
|
||||
* aircraft is in stable state and then the data stored to SD card.
|
||||
*
|
||||
* After this function the bias for each sensor will be the mean value. This doesn't make
|
||||
* sense for the z accel so make sure 6 point calibration is also run and those values set
|
||||
* after these read.
|
||||
*/
|
||||
void calibrate_sensors()
|
||||
{
|
||||
int i;
|
||||
int16_t mag_raw[3] = { 0, 0, 0 };
|
||||
// local biases for noise analysis
|
||||
float accel_bias[3], gyro_bias[3], mag_bias[3];
|
||||
int i,j;
|
||||
float accel_bias[3] = {0, 0, 0};
|
||||
float gyro_bias[3] = {0, 0, 0};
|
||||
float mag_bias[3] = {0, 0, 0};
|
||||
|
||||
// run few loops to get mean
|
||||
gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
|
||||
accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
|
||||
mag_bias[0] = mag_bias[1] = mag_bias[2] = 0;
|
||||
for (i = 0; i < 50; i++) {
|
||||
gyro_data.calibration.bias[0] = 0;
|
||||
gyro_data.calibration.bias[1] = 0;
|
||||
gyro_data.calibration.bias[2] = 0;
|
||||
accel_data.calibration.bias[0] = 0;
|
||||
accel_data.calibration.bias[1] = 0;
|
||||
accel_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
|
||||
gyro_data.calibration.variance[0] = 0;
|
||||
gyro_data.calibration.variance[1] = 0;
|
||||
gyro_data.calibration.variance[2] = 0;
|
||||
mag_data.calibration.variance[0] = 0;
|
||||
mag_data.calibration.variance[1] = 0;
|
||||
mag_data.calibration.variance[2] = 0;
|
||||
accel_data.calibration.variance[0] = 0;
|
||||
accel_data.calibration.variance[1] = 0;
|
||||
accel_data.calibration.variance[2] = 0;
|
||||
|
||||
for (i = 0, j = 0; i < 250; i++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
downsample_data();
|
||||
@ -629,89 +585,53 @@ void calibrate_sensors()
|
||||
accel_bias[0] += accel_data.filtered.x;
|
||||
accel_bias[1] += accel_data.filtered.y;
|
||||
accel_bias[2] += accel_data.filtered.z;
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
#endif
|
||||
mag_bias[0] += mag_raw[0];
|
||||
mag_bias[1] += mag_raw[1];
|
||||
mag_bias[2] += mag_raw[2];
|
||||
|
||||
gyro_data.calibration.variance[0] += pow(gyro_data.filtered.x,2);
|
||||
gyro_data.calibration.variance[1] += pow(gyro_data.filtered.y,2);
|
||||
gyro_data.calibration.variance[2] += pow(gyro_data.filtered.z,2);
|
||||
accel_data.calibration.variance[0] += pow(accel_data.filtered.x,2);
|
||||
accel_data.calibration.variance[1] += pow(accel_data.filtered.y,2);
|
||||
accel_data.calibration.variance[2] += pow(accel_data.filtered.z,2);
|
||||
ahrs_state = AHRS_IDLE;
|
||||
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
if(PIOS_HMC5843_NewDataAvailable()) {
|
||||
j ++;
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
mag_data.scaled.axis[0] = (mag_data.raw.axis[0] * mag_data.calibration.scale[0]) + mag_data.calibration.bias[0];
|
||||
mag_data.scaled.axis[1] = (mag_data.raw.axis[1] * mag_data.calibration.scale[1]) + mag_data.calibration.bias[1];
|
||||
mag_data.scaled.axis[2] = (mag_data.raw.axis[2] * mag_data.calibration.scale[2]) + mag_data.calibration.bias[2];
|
||||
|
||||
mag_bias[0] += mag_data.scaled.axis[0];
|
||||
mag_bias[1] += mag_data.scaled.axis[1];
|
||||
mag_bias[2] += mag_data.scaled.axis[2];
|
||||
mag_data.calibration.variance[0] += pow(mag_data.scaled.axis[0],2);
|
||||
mag_data.calibration.variance[1] += pow(mag_data.scaled.axis[1],2);
|
||||
mag_data.calibration.variance[2] += pow(mag_data.scaled.axis[2],2);
|
||||
}
|
||||
#endif
|
||||
|
||||
process_spi_request();
|
||||
}
|
||||
gyro_bias[0] /= i;
|
||||
gyro_bias[1] /= i;
|
||||
gyro_bias[2] /= i;
|
||||
accel_bias[0] /= i;
|
||||
accel_bias[1] /= i;
|
||||
accel_bias[2] /= i;
|
||||
mag_bias[0] /= i;
|
||||
mag_bias[1] /= i;
|
||||
mag_bias[2] /= i;
|
||||
gyro_data.calibration.bias[0] = gyro_bias[0] / i;
|
||||
gyro_data.calibration.bias[1] = gyro_bias[1] / i;
|
||||
gyro_data.calibration.bias[2] = gyro_bias[2] / i;
|
||||
accel_data.calibration.bias[0] = accel_bias[0] / i;
|
||||
accel_data.calibration.bias[1] = accel_bias[1] / i;
|
||||
accel_data.calibration.bias[2] = accel_bias[2] / i;
|
||||
mag_data.calibration.bias[0] = mag_bias[0] / j;
|
||||
mag_data.calibration.bias[1] = mag_bias[1] / j;
|
||||
mag_data.calibration.bias[2] = mag_bias[2] / j;
|
||||
|
||||
// more iterations for variance
|
||||
accel_var[0] = accel_var[1] = accel_var[2] = 0;
|
||||
gyro_var[0] = gyro_var[1] = gyro_var[2] = 0;
|
||||
mag_var[0] = mag_var[1] = mag_var[2] = 0;
|
||||
for (i = 0; i < 500; i++) {
|
||||
while (ahrs_state != AHRS_DATA_READY) ;
|
||||
ahrs_state = AHRS_PROCESSING;
|
||||
downsample_data();
|
||||
gyro_var[0] +=
|
||||
(gyro_data.filtered.x -
|
||||
gyro_bias[0]) * (gyro_data.filtered.x - gyro_bias[0]);
|
||||
gyro_var[1] +=
|
||||
(gyro_data.filtered.y -
|
||||
gyro_bias[1]) * (gyro_data.filtered.y - gyro_bias[1]);
|
||||
gyro_var[2] +=
|
||||
(gyro_data.filtered.z -
|
||||
gyro_bias[2]) * (gyro_data.filtered.z - gyro_bias[2]);
|
||||
accel_var[0] +=
|
||||
(accel_data.filtered.x -
|
||||
accel_bias[0]) * (accel_data.filtered.x -
|
||||
accel_bias[0]);
|
||||
accel_var[1] +=
|
||||
(accel_data.filtered.y -
|
||||
accel_bias[1]) * (accel_data.filtered.y -
|
||||
accel_bias[1]);
|
||||
accel_var[2] +=
|
||||
(accel_data.filtered.z -
|
||||
accel_bias[2]) * (accel_data.filtered.z -
|
||||
accel_bias[2]);
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_raw);
|
||||
#endif
|
||||
mag_var[0] +=
|
||||
(mag_raw[0] - mag_bias[0]) * (mag_raw[0] -
|
||||
mag_bias[0]);
|
||||
mag_var[1] +=
|
||||
(mag_raw[1] - mag_bias[1]) * (mag_raw[1] -
|
||||
mag_bias[1]);
|
||||
mag_var[2] +=
|
||||
(mag_raw[2] - mag_bias[2]) * (mag_raw[2] -
|
||||
mag_bias[2]);
|
||||
ahrs_state = AHRS_IDLE;
|
||||
process_spi_request();
|
||||
}
|
||||
gyro_var[0] /= i;
|
||||
gyro_var[1] /= i;
|
||||
gyro_var[2] /= i;
|
||||
accel_var[0] /= i;
|
||||
accel_var[1] /= i;
|
||||
accel_var[2] /= i;
|
||||
mag_var[0] /= i;
|
||||
mag_var[1] /= i;
|
||||
mag_var[2] /= i;
|
||||
|
||||
float mag_length2 =
|
||||
mag_bias[0] * mag_bias[0] + mag_bias[1] * mag_bias[1] +
|
||||
mag_bias[2] * mag_bias[2];
|
||||
mag_var[0] = mag_var[0] / mag_length2;
|
||||
mag_var[1] = mag_var[1] / mag_length2;
|
||||
mag_var[2] = mag_var[2] / mag_length2;
|
||||
|
||||
if (ahrs_algorithm == INSGPS_Algo)
|
||||
converge_insgps();
|
||||
gyro_data.calibration.variance[0] = gyro_data.calibration.variance[0] / i - pow(gyro_data.calibration.bias[0],2);
|
||||
gyro_data.calibration.variance[1] = gyro_data.calibration.variance[1] / i - pow(gyro_data.calibration.bias[1],2);
|
||||
gyro_data.calibration.variance[2] = gyro_data.calibration.variance[2] / i - pow(gyro_data.calibration.bias[2],2);
|
||||
accel_data.calibration.variance[0] = accel_data.calibration.variance[0] / i - pow(accel_data.calibration.bias[0],2);
|
||||
accel_data.calibration.variance[1] = accel_data.calibration.variance[1] / i - pow(accel_data.calibration.bias[1],2);
|
||||
accel_data.calibration.variance[2] = accel_data.calibration.variance[2] / i - pow(accel_data.calibration.bias[2],2);
|
||||
mag_data.calibration.variance[0] = mag_data.calibration.variance[0] / j - pow(mag_data.calibration.bias[0],2);
|
||||
mag_data.calibration.variance[1] = mag_data.calibration.variance[1] / j - pow(mag_data.calibration.bias[1],2);
|
||||
mag_data.calibration.variance[2] = mag_data.calibration.variance[2] / j - pow(mag_data.calibration.bias[2],2);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -722,50 +642,44 @@ void calibrate_sensors()
|
||||
*/
|
||||
void converge_insgps()
|
||||
{
|
||||
float pos[3] = { 0, 0, 0 }, vel[3] = {
|
||||
0, 0, 0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {
|
||||
0, 0, 0};
|
||||
float mag_var[3] = {mag_data.calibration.variance[1], mag_data.calibration.variance[0], mag_data.calibration.variance[2]}; // order swap
|
||||
INSGPSInit();
|
||||
INSSetAccelVar(accel_var);
|
||||
INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data
|
||||
INSSetGyroVar(gyro_var);
|
||||
INSSetAccelVar(accel_data.calibration.variance);
|
||||
INSSetGyroVar(gyro_data.calibration.variance);
|
||||
INSSetMagVar(mag_var);
|
||||
|
||||
float temp_var[3] = { 10, 10, 10 };
|
||||
INSSetGyroVar(temp_var); // ignore gyro's
|
||||
|
||||
accel[0] = accel_data.filtered.x;
|
||||
accel[1] = accel_data.filtered.y;
|
||||
accel[2] = accel_data.filtered.z;
|
||||
|
||||
// Iteratively constrain pitch and roll while updating yaw to align magnetic axis.
|
||||
for (int i = 0; i < 50; i++) {
|
||||
// This should be done directly but I'm too dumb.
|
||||
float rpy[3];
|
||||
Quaternion2RPY(Nav.q, rpy);
|
||||
rpy[1] =
|
||||
-atan2(accel_data.filtered.x,
|
||||
accel_data.filtered.z) * 180 / M_PI;
|
||||
rpy[0] =
|
||||
-atan2(accel_data.filtered.y,
|
||||
accel_data.filtered.z) * 180 / M_PI;
|
||||
// Get magnetic readings
|
||||
#if defined(PIOS_INCLUDE_HMC5843) && defined(PIOS_INCLUDE_I2C)
|
||||
PIOS_HMC5843_ReadMag(mag_data.raw.axis);
|
||||
#endif
|
||||
mag[0] = -mag_data.raw.axis[1];
|
||||
mag[1] = -mag_data.raw.axis[0];
|
||||
mag[2] = -mag_data.raw.axis[2];
|
||||
|
||||
RPY2Quaternion(rpy, Nav.q);
|
||||
INSStatePrediction(temp_gyro, accel, 1 / (float)EKF_RATE);
|
||||
INSCovariancePrediction(1 / (float)EKF_RATE);
|
||||
FullCorrection(mag, pos, vel, BaroAlt);
|
||||
process_spi_request(); // again we must keep this hear to prevent SPI connection dropping
|
||||
}
|
||||
|
||||
INSSetGyroVar(gyro_var);
|
||||
|
||||
/**
|
||||
* @brief Populate fields with initial values
|
||||
*/
|
||||
void reset_values() {
|
||||
accel_data.calibration.scale[0] = 0.012;
|
||||
accel_data.calibration.scale[1] = 0.012;
|
||||
accel_data.calibration.scale[2] = -0.012;
|
||||
accel_data.calibration.bias[0] = 24;
|
||||
accel_data.calibration.bias[1] = 24;
|
||||
accel_data.calibration.bias[2] = -24;
|
||||
accel_data.calibration.variance[0] = 1e-4;
|
||||
accel_data.calibration.variance[1] = 1e-4;
|
||||
accel_data.calibration.variance[2] = 1e-4;
|
||||
gyro_data.calibration.scale[0] = -0.014;
|
||||
gyro_data.calibration.scale[1] = 0.014;
|
||||
gyro_data.calibration.scale[2] = -0.014;
|
||||
gyro_data.calibration.bias[0] = -24;
|
||||
gyro_data.calibration.bias[1] = -24;
|
||||
gyro_data.calibration.bias[2] = -24;
|
||||
gyro_data.calibration.variance[0] = 1;
|
||||
gyro_data.calibration.variance[1] = 1;
|
||||
gyro_data.calibration.variance[2] = 1;
|
||||
mag_data.calibration.scale[0] = 1;
|
||||
mag_data.calibration.scale[1] = 1;
|
||||
mag_data.calibration.scale[2] = 1;
|
||||
mag_data.calibration.bias[0] = 0;
|
||||
mag_data.calibration.bias[1] = 0;
|
||||
mag_data.calibration.bias[2] = 0;
|
||||
mag_data.calibration.variance[0] = 1;
|
||||
mag_data.calibration.variance[1] = 1;
|
||||
mag_data.calibration.variance[2] = 1;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -816,23 +730,18 @@ void process_spi_request(void)
|
||||
PIOS_SYS_Reset();
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_SERIAL:
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,
|
||||
OPAHRS_MSG_V1_RSP_SERIAL);
|
||||
PIOS_SYS_SerialNumberGet((char *)
|
||||
&(user_tx_v1.payload.user.v.rsp.
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_SERIAL);
|
||||
PIOS_SYS_SerialNumberGet((char *) &(user_tx_v1.payload.user.v.rsp.
|
||||
serial.serial_bcd));
|
||||
lfsm_user_set_tx_v1(&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_ALGORITHM:
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,
|
||||
OPAHRS_MSG_V1_RSP_ALGORITHM);
|
||||
ahrs_algorithm =
|
||||
user_rx_v1.payload.user.v.req.algorithm.algorithm;
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_ALGORITHM);
|
||||
ahrs_algorithm = user_rx_v1.payload.user.v.req.algorithm.algorithm;
|
||||
lfsm_user_set_tx_v1(&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_NORTH:
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,
|
||||
OPAHRS_MSG_V1_RSP_NORTH);
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,OPAHRS_MSG_V1_RSP_NORTH);
|
||||
INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be);
|
||||
lfsm_user_set_tx_v1(&user_tx_v1);
|
||||
break;
|
||||
@ -840,123 +749,79 @@ void process_spi_request(void)
|
||||
if (user_rx_v1.payload.user.v.req.calibration.
|
||||
measure_var == AHRS_MEASURE) {
|
||||
calibration_pending = TRUE;
|
||||
} else if (user_rx_v1.payload.user.v.req.calibration.
|
||||
measure_var == AHRS_SET) {
|
||||
accel_var[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_var[0];
|
||||
accel_var[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_var[1];
|
||||
accel_var[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_var[2];
|
||||
gyro_bias[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_bias[0];
|
||||
gyro_bias[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_bias[1];
|
||||
gyro_bias[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_bias[2];
|
||||
gyro_var[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_var[0];
|
||||
gyro_var[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_var[1];
|
||||
gyro_var[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_var[2];
|
||||
mag_var[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_var[0];
|
||||
mag_var[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_var[1];
|
||||
mag_var[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_var[2];
|
||||
INSSetAccelVar(accel_var);
|
||||
float gyro_bias_ins[3] = { 0, 0, 0 };
|
||||
INSSetGyroBias(gyro_bias_ins); //gyro bias corrects in preprocessing
|
||||
INSSetGyroVar(gyro_var);
|
||||
INSSetMagVar(mag_var);
|
||||
}
|
||||
if (user_rx_v1.payload.user.v.req.calibration.
|
||||
measure_var != AHRS_ECHO) {
|
||||
/* if echoing don't set anything */
|
||||
accel_bias[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_bias[0];
|
||||
accel_bias[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_bias[1];
|
||||
accel_bias[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_bias[2];
|
||||
accel_scale[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_scale[0];
|
||||
accel_scale[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_scale[1];
|
||||
accel_scale[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
accel_scale[2];
|
||||
gyro_scale[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_scale[0];
|
||||
gyro_scale[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_scale[1];
|
||||
gyro_scale[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
gyro_scale[2];
|
||||
mag_bias[0] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_bias[0];
|
||||
mag_bias[1] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_bias[1];
|
||||
mag_bias[2] =
|
||||
user_rx_v1.payload.user.v.req.calibration.
|
||||
mag_bias[2];
|
||||
} else if (user_rx_v1.payload.user.v.req.calibration.measure_var == AHRS_SET) {
|
||||
|
||||
// Set the accel calibration
|
||||
accel_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.accel_var[0];
|
||||
accel_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.accel_var[1];
|
||||
accel_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.accel_var[2];
|
||||
accel_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.accel_scale[0];
|
||||
accel_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.accel_scale[1];
|
||||
accel_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.accel_scale[2];
|
||||
accel_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.accel_bias[0];
|
||||
accel_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.accel_bias[1];
|
||||
accel_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.accel_bias[2];
|
||||
|
||||
// Set the gyro calibration
|
||||
gyro_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.gyro_var[0];
|
||||
gyro_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.gyro_var[1];
|
||||
gyro_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.gyro_var[2];
|
||||
gyro_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[0];
|
||||
gyro_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[1];
|
||||
gyro_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.gyro_scale[2];
|
||||
gyro_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[0];
|
||||
gyro_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[1];
|
||||
gyro_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.gyro_bias[2];
|
||||
|
||||
// Set the mag calibration
|
||||
mag_data.calibration.variance[0] = user_rx_v1.payload.user.v.req.calibration.mag_var[0];
|
||||
mag_data.calibration.variance[1] = user_rx_v1.payload.user.v.req.calibration.mag_var[1];
|
||||
mag_data.calibration.variance[2] = user_rx_v1.payload.user.v.req.calibration.mag_var[2];
|
||||
mag_data.calibration.scale[0] = user_rx_v1.payload.user.v.req.calibration.mag_scale[0];
|
||||
mag_data.calibration.scale[1] = user_rx_v1.payload.user.v.req.calibration.mag_scale[1];
|
||||
mag_data.calibration.scale[2] = user_rx_v1.payload.user.v.req.calibration.mag_scale[2];
|
||||
mag_data.calibration.bias[0] = user_rx_v1.payload.user.v.req.calibration.mag_bias[0];
|
||||
mag_data.calibration.bias[1] = user_rx_v1.payload.user.v.req.calibration.mag_bias[1];
|
||||
mag_data.calibration.bias[2] = user_rx_v1.payload.user.v.req.calibration.mag_bias[2];
|
||||
|
||||
float zeros[3] = { 0, 0, 0 };
|
||||
INSSetGyroBias(zeros); //gyro bias corrects in preprocessing
|
||||
INSSetAccelVar(accel_data.calibration.variance);
|
||||
INSSetGyroVar(gyro_data.calibration.variance);
|
||||
INSSetMagVar(mag_data.calibration.variance);
|
||||
}
|
||||
|
||||
// echo back the values used
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,
|
||||
OPAHRS_MSG_V1_RSP_CALIBRATION);
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] =
|
||||
accel_var[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] =
|
||||
accel_var[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] =
|
||||
accel_var[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[0] =
|
||||
gyro_bias[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[1] =
|
||||
gyro_bias[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_bias[2] =
|
||||
gyro_bias[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] =
|
||||
gyro_var[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] =
|
||||
gyro_var[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] =
|
||||
gyro_var[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] =
|
||||
mag_var[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] =
|
||||
mag_var[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] =
|
||||
mag_var[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[0] = accel_data.calibration.variance[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[1] = accel_data.calibration.variance[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.accel_var[2] = accel_data.calibration.variance[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[0] = gyro_data.calibration.variance[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[1] = gyro_data.calibration.variance[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.gyro_var[2] = gyro_data.calibration.variance[2];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[0] = mag_data.calibration.variance[0];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[1] = mag_data.calibration.variance[1];
|
||||
user_tx_v1.payload.user.v.rsp.calibration.mag_var[2] = mag_data.calibration.variance[2];
|
||||
|
||||
lfsm_user_set_tx_v1(&user_tx_v1);
|
||||
break;
|
||||
case OPAHRS_MSG_V1_REQ_ATTITUDERAW:
|
||||
opahrs_msg_v1_init_user_tx(&user_tx_v1,
|
||||
OPAHRS_MSG_V1_RSP_ATTITUDERAW);
|
||||
|
||||
// Grab one sample from buffer to populate this
|
||||
accel_data.raw.x = valid_data_buffer[0];
|
||||
accel_data.raw.y = valid_data_buffer[2];
|
||||
accel_data.raw.z = valid_data_buffer[4];
|
||||
|
||||
gyro_data.raw.x = valid_data_buffer[1];
|
||||
gyro_data.raw.y = valid_data_buffer[3];
|
||||
gyro_data.raw.z = valid_data_buffer[5];
|
||||
|
||||
gyro_data.temp.xy = valid_data_buffer[6];
|
||||
gyro_data.temp.z = valid_data_buffer[7];
|
||||
|
||||
user_tx_v1.payload.user.v.rsp.attituderaw.mags.x =
|
||||
mag_data.raw.axis[0];
|
||||
user_tx_v1.payload.user.v.rsp.attituderaw.mags.y =
|
||||
@ -1013,24 +878,12 @@ void process_spi_request(void)
|
||||
}
|
||||
if (user_rx_v1.payload.user.v.req.update.gps.updated) {
|
||||
gps_data.updated = true;
|
||||
gps_data.NED[0] =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
NED[0];
|
||||
gps_data.NED[1] =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
NED[1];
|
||||
gps_data.NED[2] =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
NED[2];
|
||||
gps_data.heading =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
heading;
|
||||
gps_data.groundspeed =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
groundspeed;
|
||||
gps_data.quality =
|
||||
user_rx_v1.payload.user.v.req.update.gps.
|
||||
quality;
|
||||
gps_data.NED[0] = user_rx_v1.payload.user.v.req.update.gps.NED[0];
|
||||
gps_data.NED[1] = user_rx_v1.payload.user.v.req.update.gps.NED[1];
|
||||
gps_data.NED[2] = user_rx_v1.payload.user.v.req.update.gps.NED[2];
|
||||
gps_data.heading = user_rx_v1.payload.user.v.req.update.gps.heading;
|
||||
gps_data.groundspeed = user_rx_v1.payload.user.v.req.update.gps.groundspeed;
|
||||
gps_data.quality = user_rx_v1.payload.user.v.req.update.gps.quality;
|
||||
}
|
||||
// send out attitude/position estimate
|
||||
user_tx_v1.payload.user.v.rsp.update.quaternion.q1 =
|
||||
|
||||
@ -1,10 +1,10 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_ADC
|
||||
* @addtogroup AHRS_ADC AHRS ADC
|
||||
* @brief Specialized ADC code for double buffered DMA for AHRS
|
||||
* @{
|
||||
*
|
||||
|
||||
@ -1,3 +1,36 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_ADC AHRS ADC
|
||||
* @brief Specialized ADC code for double buffered DMA for AHRS
|
||||
* @{
|
||||
*
|
||||
*
|
||||
* @file ahrs.c
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief INSGPS Test Program
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <stdint.h> /* uint*_t */
|
||||
#include <stddef.h> /* NULL */
|
||||
|
||||
@ -695,3 +728,8 @@ void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val)
|
||||
lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -1,10 +1,10 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup AHRS AHRS Control
|
||||
* @addtogroup AHRS AHRS
|
||||
* @brief The AHRS Modules perform
|
||||
*
|
||||
* @{
|
||||
* @addtogroup AHRS_TIMER
|
||||
* @addtogroup AHRS_TIMER AHRS Timer
|
||||
* @brief Sets up a simple timer that can be polled to estimate idle time
|
||||
* @{
|
||||
*
|
||||
|
||||
@ -160,6 +160,11 @@ static void ahrscommsTask(void* parameters)
|
||||
|
||||
GPSGoodUpdates = 0;
|
||||
|
||||
AhrsStatusData data;
|
||||
AhrsStatusGet(&data);
|
||||
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
|
||||
AhrsStatusSet(&data);
|
||||
|
||||
// Main task loop
|
||||
while (1) {
|
||||
struct opahrs_msg_v1 rsp;
|
||||
@ -171,7 +176,7 @@ static void ahrscommsTask(void* parameters)
|
||||
AhrsStatusGet(&data);
|
||||
data.HomeSet = AHRSSTATUS_HOMESET_FALSE;
|
||||
// data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
|
||||
data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE;
|
||||
data.AlgorithmSet = AHRSSTATUS_ALGORITHMSET_FALSE;
|
||||
AhrsStatusSet(&data);
|
||||
|
||||
/* Spin here until we're in sync */
|
||||
@ -228,7 +233,7 @@ static void ahrscommsTask(void* parameters)
|
||||
if(( result = PIOS_OPAHRS_SetGetCalibration(&req,&rsp) ) == OPAHRS_RESULT_OK ) {
|
||||
update_calibration(&(rsp.payload.user.v.rsp.calibration));
|
||||
AHRSCalibrationIsUpdatedFlag = false;
|
||||
if(rsp.payload.user.v.rsp.calibration.measure_var != AHRS_ECHO)
|
||||
if(rsp.payload.user.v.req.calibration.measure_var != AHRS_ECHO)
|
||||
data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_TRUE;
|
||||
AhrsStatusSet(&data);
|
||||
|
||||
@ -347,6 +352,9 @@ static void load_calibration(struct opahrs_msg_v1_req_calibration * calibration)
|
||||
calibration->mag_bias[0] = data.mag_bias[0];
|
||||
calibration->mag_bias[1] = data.mag_bias[1];
|
||||
calibration->mag_bias[2] = data.mag_bias[2];
|
||||
calibration->mag_scale[0] = data.mag_scale[0];
|
||||
calibration->mag_scale[1] = data.mag_scale[1];
|
||||
calibration->mag_scale[2] = data.mag_scale[2];
|
||||
calibration->mag_var[0] = data.mag_var[0];
|
||||
calibration->mag_var[1] = data.mag_var[1];
|
||||
calibration->mag_var[2] = data.mag_var[2];
|
||||
|
||||
@ -81,27 +81,30 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
|
||||
UAVObjGetInstanceData(obj, instId, &data);
|
||||
memset(&data, 0, sizeof(AHRSCalibrationData));
|
||||
data.measure_var = 0;
|
||||
data.accel_bias[0] = -2048;
|
||||
data.accel_bias[1] = -2048;
|
||||
data.accel_bias[2] = -2048;
|
||||
data.accel_bias[0] = 24;
|
||||
data.accel_bias[1] = 24;
|
||||
data.accel_bias[2] = 24;
|
||||
data.accel_scale[0] = 0.012;
|
||||
data.accel_scale[1] = 0.012;
|
||||
data.accel_scale[2] = 0.012;
|
||||
data.accel_var[0] = 5e-05;
|
||||
data.accel_var[1] = 5e-05;
|
||||
data.accel_var[2] = 5e-05;
|
||||
data.gyro_bias[0] = -1675;
|
||||
data.gyro_bias[1] = -1675;
|
||||
data.gyro_bias[2] = -1675;
|
||||
data.gyro_scale[0] = -0.007;
|
||||
data.gyro_scale[1] = -0.007;
|
||||
data.gyro_scale[2] = -0.007;
|
||||
data.gyro_bias[0] = 23;
|
||||
data.gyro_bias[1] = 23;
|
||||
data.gyro_bias[2] = 23;
|
||||
data.gyro_scale[0] = -0.014;
|
||||
data.gyro_scale[1] = -0.014;
|
||||
data.gyro_scale[2] = -0.014;
|
||||
data.gyro_var[0] = 0.0001;
|
||||
data.gyro_var[1] = 0.0001;
|
||||
data.gyro_var[2] = 0.0001;
|
||||
data.mag_bias[0] = 0;
|
||||
data.mag_bias[1] = 0;
|
||||
data.mag_bias[2] = 0;
|
||||
data.mag_scale[0] = 1;
|
||||
data.mag_scale[1] = 1;
|
||||
data.mag_scale[2] = 1;
|
||||
data.mag_var[0] = 5e-05;
|
||||
data.mag_var[1] = 5e-05;
|
||||
data.mag_var[2] = 5e-05;
|
||||
|
||||
@ -41,7 +41,7 @@
|
||||
#define AHRSCALIBRATION_H
|
||||
|
||||
// Object constants
|
||||
#define AHRSCALIBRATION_OBJID 2082766848U
|
||||
#define AHRSCALIBRATION_OBJID 1408636690U
|
||||
#define AHRSCALIBRATION_NAME "AHRSCalibration"
|
||||
#define AHRSCALIBRATION_METANAME "AHRSCalibrationMeta"
|
||||
#define AHRSCALIBRATION_ISSINGLEINST 1
|
||||
@ -72,13 +72,14 @@
|
||||
// Object data
|
||||
typedef struct {
|
||||
uint8_t measure_var;
|
||||
int16_t accel_bias[3];
|
||||
float accel_bias[3];
|
||||
float accel_scale[3];
|
||||
float accel_var[3];
|
||||
int16_t gyro_bias[3];
|
||||
float gyro_bias[3];
|
||||
float gyro_scale[3];
|
||||
float gyro_var[3];
|
||||
int16_t mag_bias[3];
|
||||
float mag_bias[3];
|
||||
float mag_scale[3];
|
||||
float mag_var[3];
|
||||
|
||||
} __attribute__((packed)) AHRSCalibrationData;
|
||||
@ -122,6 +123,11 @@ typedef enum { AHRSCALIBRATION_GYRO_VAR_X=0, AHRSCALIBRATION_GYRO_VAR_Y=1, AHRSC
|
||||
typedef enum { AHRSCALIBRATION_MAG_BIAS_X=0, AHRSCALIBRATION_MAG_BIAS_Y=1, AHRSCALIBRATION_MAG_BIAS_Z=2 } AHRSCalibrationmag_biasElem;
|
||||
/* Number of elements for field mag_bias */
|
||||
#define AHRSCALIBRATION_MAG_BIAS_NUMELEM 3
|
||||
// Field mag_scale information
|
||||
/* Array element names for field mag_scale */
|
||||
typedef enum { AHRSCALIBRATION_MAG_SCALE_X=0, AHRSCALIBRATION_MAG_SCALE_Y=1, AHRSCALIBRATION_MAG_SCALE_Z=2 } AHRSCalibrationmag_scaleElem;
|
||||
/* Number of elements for field mag_scale */
|
||||
#define AHRSCALIBRATION_MAG_SCALE_NUMELEM 3
|
||||
// Field mag_var information
|
||||
/* Array element names for field mag_var */
|
||||
typedef enum { AHRSCALIBRATION_MAG_VAR_X=0, AHRSCALIBRATION_MAG_VAR_Y=1, AHRSCALIBRATION_MAG_VAR_Z=2 } AHRSCalibrationmag_varElem;
|
||||
|
||||
@ -220,11 +220,16 @@ TIM8 | | | |
|
||||
/* RCC_PCLK2_Div8: ADC clock = PCLK2/8 */
|
||||
#define PIOS_ADC_SAMPLE_TIME ADC_SampleTime_239Cycles5
|
||||
/* Sample time: */
|
||||
/* With an ADCCLK = 14 MHz and a sampling time of 293.5 cycles: */
|
||||
/* With an ADCCLK = 14 MHz and a sampling time of 239.5 cycles: */
|
||||
/* Tconv = 239.5 + 12.5 = 252 cycles = 18<31>s */
|
||||
/* (1 / (ADCCLK / CYCLES)) = Sample Time (<28>S) */
|
||||
#define PIOS_ADC_IRQ_PRIO PIOS_IRQ_PRIO_HIGH
|
||||
|
||||
// Currently analog acquistion hard coded at 480 Hz
|
||||
// PCKL2 = HCLK / 16
|
||||
// ADCCLK = PCLK2 / 2
|
||||
#define PIOS_ADC_RATE (72.0e6 / 16 / 2 / 252 / (PIOS_ADC_NUM_PINS / 2))
|
||||
#define EKF_RATE (PIOS_ADC_RATE / adc_oversampling)
|
||||
|
||||
//-------------------------
|
||||
// GPIO
|
||||
|
||||
@ -232,13 +232,14 @@ enum measure_mode {AHRS_SET, AHRS_MEASURE, AHRS_ECHO};
|
||||
|
||||
struct opahrs_msg_v1_req_calibration {
|
||||
enum measure_mode measure_var;
|
||||
uint16_t accel_bias[3];
|
||||
float accel_bias[3];
|
||||
float accel_scale[3];
|
||||
float accel_var[3];
|
||||
uint16_t gyro_bias[3];
|
||||
float gyro_bias[3];
|
||||
float gyro_scale[3];
|
||||
float gyro_var[3];
|
||||
uint16_t mag_bias[3];
|
||||
float mag_bias[3];
|
||||
float mag_scale[3];
|
||||
float mag_var[3];
|
||||
} __attribute__((__packed__));
|
||||
|
||||
@ -316,9 +317,7 @@ struct opahrs_msg_v1_rsp_update {
|
||||
} __attribute__((__packed__));
|
||||
|
||||
struct opahrs_msg_v1_rsp_calibration {
|
||||
uint8_t measure_var;
|
||||
float accel_var[3];
|
||||
uint16_t gyro_bias[3];
|
||||
float gyro_var[3];
|
||||
float mag_var[3];
|
||||
} __attribute__((__packed__));
|
||||
|
||||
@ -32,6 +32,22 @@
|
||||
654330231218E9780063F913 /* insgps.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = insgps.c; path = ../../AHRS/insgps.c; sourceTree = SOURCE_ROOT; };
|
||||
6543304F121980300063F913 /* insgps.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = insgps.h; sourceTree = "<group>"; };
|
||||
655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = "<group>"; };
|
||||
655E4A05124B1CFC00687939 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
|
||||
655E4A06124B1CFC00687939 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = "<group>"; };
|
||||
655E4A07124B1CFC00687939 /* STM3210E_OP.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM3210E_OP.h; sourceTree = "<group>"; };
|
||||
65632C62124DC5F700469B77 /* ahrscalibration.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrscalibration.h; sourceTree = "<group>"; };
|
||||
65632C63124DC5F700469B77 /* ahrssettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrssettings.h; sourceTree = "<group>"; };
|
||||
65632C64124DC5F700469B77 /* attituderaw.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attituderaw.h; sourceTree = "<group>"; };
|
||||
65632C65124DC5F700469B77 /* attitudesettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitudesettings.h; sourceTree = "<group>"; };
|
||||
65632C66124DC5F700469B77 /* baroaltitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = baroaltitude.h; sourceTree = "<group>"; };
|
||||
65632C67124DC5F700469B77 /* gpsposition.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpsposition.h; sourceTree = "<group>"; };
|
||||
65632C68124DC5F700469B77 /* gpssatellites.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpssatellites.h; sourceTree = "<group>"; };
|
||||
65632C69124DC5F700469B77 /* gpstime.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpstime.h; sourceTree = "<group>"; };
|
||||
65632C6A124DC5F700469B77 /* homelocation.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = homelocation.h; sourceTree = "<group>"; };
|
||||
65632C6B124DC5F700469B77 /* mixersettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = mixersettings.h; sourceTree = "<group>"; };
|
||||
65632C6C124DC5F700469B77 /* mixerstatus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = mixerstatus.h; sourceTree = "<group>"; };
|
||||
65632C6D124DC5F700469B77 /* vtolsettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = vtolsettings.h; sourceTree = "<group>"; };
|
||||
65632C6E124DC5F700469B77 /* vtolstatus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = vtolstatus.h; sourceTree = "<group>"; };
|
||||
657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = "<group>"; };
|
||||
657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = "<group>"; };
|
||||
657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = "<group>"; };
|
||||
@ -2583,7 +2599,6 @@
|
||||
65B7E6AE120DF1E2000C1123 /* ahrs.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = ahrs.c; path = ../../AHRS/ahrs.c; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs.h; sourceTree = "<group>"; };
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = "<group>"; };
|
||||
65B7E6B3120DF1E2000C1123 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = "<group>"; };
|
||||
65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = "<group>"; };
|
||||
65B7E6B6120DF1E2000C1123 /* Makefile */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.make; name = Makefile; path = ../../AHRS/Makefile; sourceTree = SOURCE_ROOT; };
|
||||
65B7E6B7120DF1E2000C1123 /* pios_board.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; name = pios_board.c; path = ../../AHRS/pios_board.c; sourceTree = SOURCE_ROOT; };
|
||||
@ -2927,6 +2942,16 @@
|
||||
path = inc;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
655E4A04124B1CFC00687939 /* Boards */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
655E4A05124B1CFC00687939 /* pios_board.h */,
|
||||
655E4A06124B1CFC00687939 /* STM32103CB_AHRS.h */,
|
||||
655E4A07124B1CFC00687939 /* STM3210E_OP.h */,
|
||||
);
|
||||
path = Boards;
|
||||
sourceTree = "<group>";
|
||||
};
|
||||
657CEEB5121DBC49007A1FBE /* flight */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
@ -6770,7 +6795,6 @@
|
||||
6543304F121980300063F913 /* insgps.h */,
|
||||
65B7E6B0120DF1E2000C1123 /* ahrs.h */,
|
||||
65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */,
|
||||
65B7E6B3120DF1E2000C1123 /* pios_board.h */,
|
||||
65B7E6B4120DF1E2000C1123 /* pios_config.h */,
|
||||
);
|
||||
name = inc;
|
||||
@ -7157,6 +7181,19 @@
|
||||
65E8EF8011EEA61E00BBF654 /* inc */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
65632C62124DC5F700469B77 /* ahrscalibration.h */,
|
||||
65632C63124DC5F700469B77 /* ahrssettings.h */,
|
||||
65632C64124DC5F700469B77 /* attituderaw.h */,
|
||||
65632C65124DC5F700469B77 /* attitudesettings.h */,
|
||||
65632C66124DC5F700469B77 /* baroaltitude.h */,
|
||||
65632C67124DC5F700469B77 /* gpsposition.h */,
|
||||
65632C68124DC5F700469B77 /* gpssatellites.h */,
|
||||
65632C69124DC5F700469B77 /* gpstime.h */,
|
||||
65632C6A124DC5F700469B77 /* homelocation.h */,
|
||||
65632C6B124DC5F700469B77 /* mixersettings.h */,
|
||||
65632C6C124DC5F700469B77 /* mixerstatus.h */,
|
||||
65632C6D124DC5F700469B77 /* vtolsettings.h */,
|
||||
65632C6E124DC5F700469B77 /* vtolstatus.h */,
|
||||
65E8EF8111EEA61E00BBF654 /* actuatorcommand.h */,
|
||||
65E8EF8211EEA61E00BBF654 /* actuatordesired.h */,
|
||||
65E8EF8311EEA61E00BBF654 /* actuatorsettings.h */,
|
||||
@ -7210,6 +7247,7 @@
|
||||
65E8F02F11EFF25C00BBF654 /* PiOS */ = {
|
||||
isa = PBXGroup;
|
||||
children = (
|
||||
655E4A04124B1CFC00687939 /* Boards */,
|
||||
65E8F03011EFF25C00BBF654 /* Common */,
|
||||
65E8F03811EFF25C00BBF654 /* inc */,
|
||||
65E8F05711EFF25C00BBF654 /* pios.h */,
|
||||
|
||||
@ -36,7 +36,7 @@
|
||||
#include <QtGui/QPushButton>
|
||||
|
||||
const double ConfigAHRSWidget::maxVarValue = 0.1;
|
||||
const int ConfigAHRSWidget::calibrationDelay = 15; // Time to wait for the AHRS to do its calibration
|
||||
const int ConfigAHRSWidget::calibrationDelay = 5; // Time to wait for the AHRS to do its calibration
|
||||
|
||||
ConfigAHRSWidget::ConfigAHRSWidget(QWidget *parent) : ConfigTaskWidget(parent)
|
||||
{
|
||||
@ -311,15 +311,28 @@ void ConfigAHRSWidget::attitudeRawUpdated(UAVObject * obj)
|
||||
{
|
||||
QMutexLocker lock(&attitudeRawUpdateLock);
|
||||
UAVObjectField *accel_field = obj->getField(QString("accels_filtered"));
|
||||
UAVObjectField *gyro_field = obj->getField(QString("gyros_filtered"));
|
||||
UAVObjectField *mag_field = obj->getField(QString("magnetometers"));
|
||||
|
||||
Q_ASSERT(gyro_field != 0 && accel_field != 0 & mag_field != 0);
|
||||
|
||||
// This is necessary to prevent a race condition on disconnect signal and another update
|
||||
if (collectingData == true) {
|
||||
accel_accum_x.append(accel_field->getValue(0).toDouble());
|
||||
accel_accum_y.append(accel_field->getValue(1).toDouble());
|
||||
accel_accum_z.append(accel_field->getValue(2).toDouble());
|
||||
// Note gyros actually (-y,-x,-z) but since we consistent here no prob
|
||||
mag_accum_x.append(mag_field->getValue(0).toDouble());
|
||||
mag_accum_y.append(mag_field->getValue(1).toDouble());
|
||||
mag_accum_z.append(mag_field->getValue(2).toDouble());
|
||||
gyro_accum_x.append(gyro_field->getValue(0).toDouble());
|
||||
gyro_accum_y.append(gyro_field->getValue(1).toDouble());
|
||||
gyro_accum_z.append(gyro_field->getValue(2).toDouble());
|
||||
}
|
||||
|
||||
qDebug() << "Size: " << accel_accum_x.size();
|
||||
if(accel_accum_x.size() >= 20) {
|
||||
if(accel_accum_x.size() >= 20 && collectingData == true) {
|
||||
collectingData = false;
|
||||
disconnect(obj,SIGNAL(objectUpdated(UAVObject*)),this,SLOT(attitudeRawUpdated(UAVObject*)));
|
||||
m_ahrs->sixPointsSave->setEnabled(true);
|
||||
|
||||
@ -391,7 +404,11 @@ void ConfigAHRSWidget::savePositionData()
|
||||
mag_accum_x.clear();
|
||||
mag_accum_y.clear();
|
||||
mag_accum_z.clear();
|
||||
gyro_accum_x.clear();
|
||||
gyro_accum_y.clear();
|
||||
gyro_accum_z.clear();
|
||||
|
||||
collectingData = true;
|
||||
UAVObject *obj = dynamic_cast<UAVDataObject*>(getObjectManager()->getObject(QString("AttitudeRaw")));
|
||||
connect(obj, SIGNAL(objectUpdated(UAVObject*)), this, SLOT(attitudeRawUpdated(UAVObject*)));
|
||||
|
||||
@ -512,17 +529,27 @@ void ConfigAHRSWidget::computeScaleBias()
|
||||
field = obj->getField(QString("accel_scale"));
|
||||
field->setDouble(S[0],0);
|
||||
field->setDouble(S[1],1);
|
||||
field->setDouble(S[2],2); // Flip the Z-axis scale to be negative
|
||||
field->setDouble(S[2],2);
|
||||
field = obj->getField(QString("accel_bias"));
|
||||
field->setDouble((int) b[0] / S[0],0);
|
||||
field->setDouble((int) b[1] / S[1],1);
|
||||
field->setDouble((int) -b[2] / S[2],2);
|
||||
field->setDouble(b[0],0);
|
||||
field->setDouble(b[1],1);
|
||||
field->setDouble(b[2],2);
|
||||
|
||||
SixPointInConstFieldCal( 1, mag_data_x, mag_data_y, mag_data_z, S, b);
|
||||
field = obj->getField(QString("mag_scale"));
|
||||
field->setDouble(S[0],0);
|
||||
field->setDouble(S[1],1);
|
||||
field->setDouble(S[2],2);
|
||||
field = obj->getField(QString("mag_bias"));
|
||||
field->setDouble(b[0] / S[0], 0);
|
||||
field->setDouble(b[1] / S[1], 1);
|
||||
field->setDouble(b[2] / S[2], 2);
|
||||
field->setDouble(b[0], 0);
|
||||
field->setDouble(b[1], 1);
|
||||
field->setDouble(b[2], 2);
|
||||
|
||||
field = obj->getField(QString("gyro_bias"));
|
||||
field->setDouble(listMean(gyro_accum_x),0);
|
||||
field->setDouble(listMean(gyro_accum_y),1);
|
||||
field->setDouble(listMean(gyro_accum_z),2);
|
||||
|
||||
obj->updated();
|
||||
|
||||
position = -1; //set to run again
|
||||
|
||||
@ -71,6 +71,11 @@ private:
|
||||
const static double maxVarValue;
|
||||
const static int calibrationDelay;
|
||||
|
||||
bool collectingData;
|
||||
|
||||
QList<double> gyro_accum_x;
|
||||
QList<double> gyro_accum_y;
|
||||
QList<double> gyro_accum_z;
|
||||
QList<double> accel_accum_x;
|
||||
QList<double> accel_accum_y;
|
||||
QList<double> accel_accum_z;
|
||||
|
||||
@ -53,7 +53,7 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
|
||||
accel_biasElemNames.append("X");
|
||||
accel_biasElemNames.append("Y");
|
||||
accel_biasElemNames.append("Z");
|
||||
fields.append( new UAVObjectField(QString("accel_bias"), QString("raw"), UAVObjectField::INT16, accel_biasElemNames, QStringList()) );
|
||||
fields.append( new UAVObjectField(QString("accel_bias"), QString("raw"), UAVObjectField::FLOAT32, accel_biasElemNames, QStringList()) );
|
||||
QStringList accel_scaleElemNames;
|
||||
accel_scaleElemNames.append("X");
|
||||
accel_scaleElemNames.append("Y");
|
||||
@ -68,7 +68,7 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
|
||||
gyro_biasElemNames.append("X");
|
||||
gyro_biasElemNames.append("Y");
|
||||
gyro_biasElemNames.append("Z");
|
||||
fields.append( new UAVObjectField(QString("gyro_bias"), QString("raw"), UAVObjectField::INT16, gyro_biasElemNames, QStringList()) );
|
||||
fields.append( new UAVObjectField(QString("gyro_bias"), QString("raw"), UAVObjectField::FLOAT32, gyro_biasElemNames, QStringList()) );
|
||||
QStringList gyro_scaleElemNames;
|
||||
gyro_scaleElemNames.append("X");
|
||||
gyro_scaleElemNames.append("Y");
|
||||
@ -83,7 +83,12 @@ AHRSCalibration::AHRSCalibration(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTING
|
||||
mag_biasElemNames.append("X");
|
||||
mag_biasElemNames.append("Y");
|
||||
mag_biasElemNames.append("Z");
|
||||
fields.append( new UAVObjectField(QString("mag_bias"), QString("mGau"), UAVObjectField::INT16, mag_biasElemNames, QStringList()) );
|
||||
fields.append( new UAVObjectField(QString("mag_bias"), QString("mGau"), UAVObjectField::FLOAT32, mag_biasElemNames, QStringList()) );
|
||||
QStringList mag_scaleElemNames;
|
||||
mag_scaleElemNames.append("X");
|
||||
mag_scaleElemNames.append("Y");
|
||||
mag_scaleElemNames.append("Z");
|
||||
fields.append( new UAVObjectField(QString("mag_scale"), QString("mGau"), UAVObjectField::FLOAT32, mag_scaleElemNames, QStringList()) );
|
||||
QStringList mag_varElemNames;
|
||||
mag_varElemNames.append("X");
|
||||
mag_varElemNames.append("Y");
|
||||
@ -123,27 +128,30 @@ UAVObject::Metadata AHRSCalibration::getDefaultMetadata()
|
||||
void AHRSCalibration::setDefaultFieldValues()
|
||||
{
|
||||
data.measure_var = 0;
|
||||
data.accel_bias[0] = -2048;
|
||||
data.accel_bias[1] = -2048;
|
||||
data.accel_bias[2] = -2048;
|
||||
data.accel_bias[0] = 24;
|
||||
data.accel_bias[1] = 24;
|
||||
data.accel_bias[2] = 24;
|
||||
data.accel_scale[0] = 0.012;
|
||||
data.accel_scale[1] = 0.012;
|
||||
data.accel_scale[2] = 0.012;
|
||||
data.accel_var[0] = 5e-05;
|
||||
data.accel_var[1] = 5e-05;
|
||||
data.accel_var[2] = 5e-05;
|
||||
data.gyro_bias[0] = -1675;
|
||||
data.gyro_bias[1] = -1675;
|
||||
data.gyro_bias[2] = -1675;
|
||||
data.gyro_scale[0] = -0.007;
|
||||
data.gyro_scale[1] = -0.007;
|
||||
data.gyro_scale[2] = -0.007;
|
||||
data.gyro_bias[0] = 23;
|
||||
data.gyro_bias[1] = 23;
|
||||
data.gyro_bias[2] = 23;
|
||||
data.gyro_scale[0] = -0.014;
|
||||
data.gyro_scale[1] = -0.014;
|
||||
data.gyro_scale[2] = -0.014;
|
||||
data.gyro_var[0] = 0.0001;
|
||||
data.gyro_var[1] = 0.0001;
|
||||
data.gyro_var[2] = 0.0001;
|
||||
data.mag_bias[0] = 0;
|
||||
data.mag_bias[1] = 0;
|
||||
data.mag_bias[2] = 0;
|
||||
data.mag_scale[0] = 1;
|
||||
data.mag_scale[1] = 1;
|
||||
data.mag_scale[2] = 1;
|
||||
data.mag_var[0] = 5e-05;
|
||||
data.mag_var[1] = 5e-05;
|
||||
data.mag_var[2] = 5e-05;
|
||||
|
||||
@ -44,13 +44,14 @@ public:
|
||||
// Field structure
|
||||
typedef struct {
|
||||
quint8 measure_var;
|
||||
qint16 accel_bias[3];
|
||||
float accel_bias[3];
|
||||
float accel_scale[3];
|
||||
float accel_var[3];
|
||||
qint16 gyro_bias[3];
|
||||
float gyro_bias[3];
|
||||
float gyro_scale[3];
|
||||
float gyro_var[3];
|
||||
qint16 mag_bias[3];
|
||||
float mag_bias[3];
|
||||
float mag_scale[3];
|
||||
float mag_var[3];
|
||||
|
||||
} __attribute__((packed)) DataFields;
|
||||
@ -94,6 +95,11 @@ public:
|
||||
typedef enum { MAG_BIAS_X=0, MAG_BIAS_Y=1, MAG_BIAS_Z=2 } mag_biasElem;
|
||||
/* Number of elements for field mag_bias */
|
||||
static const quint32 MAG_BIAS_NUMELEM = 3;
|
||||
// Field mag_scale information
|
||||
/* Array element names for field mag_scale */
|
||||
typedef enum { MAG_SCALE_X=0, MAG_SCALE_Y=1, MAG_SCALE_Z=2 } mag_scaleElem;
|
||||
/* Number of elements for field mag_scale */
|
||||
static const quint32 MAG_SCALE_NUMELEM = 3;
|
||||
// Field mag_var information
|
||||
/* Array element names for field mag_var */
|
||||
typedef enum { MAG_VAR_X=0, MAG_VAR_Y=1, MAG_VAR_Z=2 } mag_varElem;
|
||||
@ -102,7 +108,7 @@ public:
|
||||
|
||||
|
||||
// Constants
|
||||
static const quint32 OBJID = 2082766848U;
|
||||
static const quint32 OBJID = 1408636690U;
|
||||
static const QString NAME;
|
||||
static const bool ISSINGLEINST = 1;
|
||||
static const bool ISSETTINGS = 1;
|
||||
|
||||
@ -52,7 +52,7 @@ _fields = [ \
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'accel_bias',
|
||||
'h',
|
||||
'f',
|
||||
3,
|
||||
[
|
||||
'X',
|
||||
@ -88,7 +88,7 @@ _fields = [ \
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'gyro_bias',
|
||||
'h',
|
||||
'f',
|
||||
3,
|
||||
[
|
||||
'X',
|
||||
@ -124,7 +124,19 @@ _fields = [ \
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'mag_bias',
|
||||
'h',
|
||||
'f',
|
||||
3,
|
||||
[
|
||||
'X',
|
||||
'Y',
|
||||
'Z',
|
||||
],
|
||||
{
|
||||
}
|
||||
),
|
||||
uavobject.UAVObjectField(
|
||||
'mag_scale',
|
||||
'f',
|
||||
3,
|
||||
[
|
||||
'X',
|
||||
@ -151,7 +163,7 @@ _fields = [ \
|
||||
|
||||
class AHRSCalibration(uavobject.UAVObject):
|
||||
## Object constants
|
||||
OBJID = 2082766848
|
||||
OBJID = 1408636690
|
||||
NAME = "AHRSCalibration"
|
||||
METANAME = "AHRSCalibrationMeta"
|
||||
ISSINGLEINST = 1
|
||||
|
||||
@ -2,13 +2,14 @@
|
||||
<object name="AHRSCalibration" singleinstance="true" settings="true">
|
||||
<description>Contains the calibration settings for the @ref AHRSCommsModule</description>
|
||||
<field name="measure_var" units="" type="enum" elements="1" options="SET,MEASURE,ECHO" defaultvalue="SET"/>
|
||||
<field name="accel_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-2048"/>
|
||||
<field name="accel_bias" units="raw" type="float" elementnames="X,Y,Z" defaultvalue="24"/>
|
||||
<field name="accel_scale" units="m/s" type="float" elementnames="X,Y,Z" defaultvalue="0.012"/>
|
||||
<field name="accel_var" units="m^2/s^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
|
||||
<field name="gyro_bias" units="raw" type="int16" elementnames="X,Y,Z" defaultvalue="-1675"/>
|
||||
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="-0.0070"/>
|
||||
<field name="gyro_bias" units="raw" type="float" elementnames="X,Y,Z" defaultvalue="23"/>
|
||||
<field name="gyro_scale" units="deg/s" type="float" elementnames="X,Y,Z" defaultvalue="-0.0140"/>
|
||||
<field name="gyro_var" units="deg^s/s^2" type="float" elementnames="X,Y,Z" defaultvalue="1e-4"/>
|
||||
<field name="mag_bias" units="mGau" type="int16" elementnames="X,Y,Z" defaultvalue="0"/>
|
||||
<field name="mag_bias" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="0"/>
|
||||
<field name="mag_scale" units="mGau" type="float" elementnames="X,Y,Z" defaultvalue="1"/>
|
||||
<field name="mag_var" units="mGau^s" type="float" elementnames="X,Y,Z" defaultvalue="5e-5"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user