diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 68bcc8cea..851048fd1 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -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,7 +222,8 @@ int main() PIOS_SPI_Init(); lfsm_init(); - + reset_values(); + ahrs_state = AHRS_IDLE; /* Use simple averaging filter for now */ @@ -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) @@ -305,7 +274,7 @@ int main() timer_start(); - /******************* Main EKF loop ****************************/ + /******************* Main EKF loop ****************************/ while (1) { // Alive signal @@ -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,39 +311,24 @@ 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]; - + /******************** INS ALGORITHM **************************/ if (ahrs_algorithm == INSGPS_Algo) { - /******************** INS ALGORITHM **************************/ // format data for INS algo gyro[0] = gyro_data.filtered.x; gyro[1] = gyro_data.filtered.y; gyro[2] = gyro_data.filtered.z; accel[0] = accel_data.filtered.x, - accel[1] = accel_data.filtered.y, - 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]); + accel[1] = accel_data.filtered.y, + 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.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,52 +642,46 @@ 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; +} + /** * @addtogroup AHRS_SPI SPI Messaging * @{ @@ -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 = diff --git a/flight/AHRS/ahrs_adc.c b/flight/AHRS/ahrs_adc.c index be7546825..4606fedbe 100644 --- a/flight/AHRS/ahrs_adc.c +++ b/flight/AHRS/ahrs_adc.c @@ -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 * @{ * diff --git a/flight/AHRS/ahrs_fsm.c b/flight/AHRS/ahrs_fsm.c index 1d0858b60..6dc813453 100644 --- a/flight/AHRS/ahrs_fsm.c +++ b/flight/AHRS/ahrs_fsm.c @@ -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 /* uint*_t */ #include /* NULL */ @@ -695,3 +728,8 @@ void lfsm_irq_callback(uint8_t crc_ok, uint8_t crc_val) lfsm_inject_event(LFSM_EVENT_RX_UNKNOWN); } } + +/** + * @} + * @} + */ \ No newline at end of file diff --git a/flight/AHRS/ahrs_timer.c b/flight/AHRS/ahrs_timer.c index ec1dd1d6c..d30a6c9f6 100644 --- a/flight/AHRS/ahrs_timer.c +++ b/flight/AHRS/ahrs_timer.c @@ -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 * @{ * diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index e9e4386c3..f5a6fcdb7 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c @@ -159,7 +159,12 @@ static void ahrscommsTask(void* parameters) portTickType lastSysTime; GPSGoodUpdates = 0; - + + AhrsStatusData data; + AhrsStatusGet(&data); + data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE; + AhrsStatusSet(&data); + // Main task loop while (1) { struct opahrs_msg_v1 rsp; @@ -170,8 +175,8 @@ static void ahrscommsTask(void* parameters) /* Whenever resyncing, assume AHRS doesn't reset and doesn't know home */ AhrsStatusGet(&data); data.HomeSet = AHRSSTATUS_HOMESET_FALSE; - //data.CalibrationSet = AHRSSTATUS_CALIBRATIONSET_FALSE; - data.AlgorithmSet = AHRSSTATUS_CALIBRATIONSET_FALSE; +// data.CalibrationSet = 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]; diff --git a/flight/OpenPilot/UAVObjects/ahrscalibration.c b/flight/OpenPilot/UAVObjects/ahrscalibration.c index 15de84ead..6cd7b47d0 100644 --- a/flight/OpenPilot/UAVObjects/ahrscalibration.c +++ b/flight/OpenPilot/UAVObjects/ahrscalibration.c @@ -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; diff --git a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h index 5503f3394..1e8d1ba95 100644 --- a/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h +++ b/flight/OpenPilot/UAVObjects/inc/ahrscalibration.h @@ -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; diff --git a/flight/PiOS/Boards/STM32103CB_AHRS.h b/flight/PiOS/Boards/STM32103CB_AHRS.h index 5cb8c2299..4c8479f93 100644 --- a/flight/PiOS/Boards/STM32103CB_AHRS.h +++ b/flight/PiOS/Boards/STM32103CB_AHRS.h @@ -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�s */ /* (1 / (ADCCLK / CYCLES)) = Sample Time (�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 diff --git a/flight/PiOS/inc/pios_opahrs_proto.h b/flight/PiOS/inc/pios_opahrs_proto.h index e4110681e..6a8793906 100644 --- a/flight/PiOS/inc/pios_opahrs_proto.h +++ b/flight/PiOS/inc/pios_opahrs_proto.h @@ -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__)); diff --git a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj index 5a4c36cda..caa4d3495 100644 --- a/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj +++ b/flight/Project/OpenPilotOSX/OpenPilotOSX.xcodeproj/project.pbxproj @@ -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 = ""; }; 655268BC121FBD2900410C6E /* ahrscalibration.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = ahrscalibration.xml; sourceTree = ""; }; + 655E4A05124B1CFC00687939 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; + 655E4A06124B1CFC00687939 /* STM32103CB_AHRS.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM32103CB_AHRS.h; sourceTree = ""; }; + 655E4A07124B1CFC00687939 /* STM3210E_OP.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = STM3210E_OP.h; sourceTree = ""; }; + 65632C62124DC5F700469B77 /* ahrscalibration.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrscalibration.h; sourceTree = ""; }; + 65632C63124DC5F700469B77 /* ahrssettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrssettings.h; sourceTree = ""; }; + 65632C64124DC5F700469B77 /* attituderaw.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attituderaw.h; sourceTree = ""; }; + 65632C65124DC5F700469B77 /* attitudesettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = attitudesettings.h; sourceTree = ""; }; + 65632C66124DC5F700469B77 /* baroaltitude.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = baroaltitude.h; sourceTree = ""; }; + 65632C67124DC5F700469B77 /* gpsposition.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpsposition.h; sourceTree = ""; }; + 65632C68124DC5F700469B77 /* gpssatellites.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpssatellites.h; sourceTree = ""; }; + 65632C69124DC5F700469B77 /* gpstime.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = gpstime.h; sourceTree = ""; }; + 65632C6A124DC5F700469B77 /* homelocation.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = homelocation.h; sourceTree = ""; }; + 65632C6B124DC5F700469B77 /* mixersettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = mixersettings.h; sourceTree = ""; }; + 65632C6C124DC5F700469B77 /* mixerstatus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = mixerstatus.h; sourceTree = ""; }; + 65632C6D124DC5F700469B77 /* vtolsettings.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = vtolsettings.h; sourceTree = ""; }; + 65632C6E124DC5F700469B77 /* vtolstatus.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = vtolstatus.h; sourceTree = ""; }; 657CEEAD121DB6C8007A1FBE /* homelocation.xml */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = text.xml; path = homelocation.xml; sourceTree = ""; }; 657CEEB7121DBC63007A1FBE /* CoordinateConversions.c */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.c; path = CoordinateConversions.c; sourceTree = ""; }; 657CEEB9121DBC63007A1FBE /* CoordinateConversions.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = CoordinateConversions.h; sourceTree = ""; }; @@ -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 = ""; }; 65B7E6B1120DF1E2000C1123 /* ahrs_fsm.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = ahrs_fsm.h; sourceTree = ""; }; - 65B7E6B3120DF1E2000C1123 /* pios_board.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_board.h; sourceTree = ""; }; 65B7E6B4120DF1E2000C1123 /* pios_config.h */ = {isa = PBXFileReference; fileEncoding = 4; lastKnownFileType = sourcecode.c.h; path = pios_config.h; sourceTree = ""; }; 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 = ""; }; + 655E4A04124B1CFC00687939 /* Boards */ = { + isa = PBXGroup; + children = ( + 655E4A05124B1CFC00687939 /* pios_board.h */, + 655E4A06124B1CFC00687939 /* STM32103CB_AHRS.h */, + 655E4A07124B1CFC00687939 /* STM3210E_OP.h */, + ); + path = Boards; + sourceTree = ""; + }; 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 */, diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index 8fd6549ef..b266cd46d 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -36,7 +36,7 @@ #include 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")); - 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()); - 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()); + + 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(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 diff --git a/ground/src/plugins/config/configahrswidget.h b/ground/src/plugins/config/configahrswidget.h index 3b3588628..6a403b5bf 100644 --- a/ground/src/plugins/config/configahrswidget.h +++ b/ground/src/plugins/config/configahrswidget.h @@ -71,6 +71,11 @@ private: const static double maxVarValue; const static int calibrationDelay; + bool collectingData; + + QList gyro_accum_x; + QList gyro_accum_y; + QList gyro_accum_z; QList accel_accum_x; QList accel_accum_y; QList accel_accum_z; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.cpp b/ground/src/plugins/uavobjects/ahrscalibration.cpp index 8112b4a84..55619f68b 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.cpp +++ b/ground/src/plugins/uavobjects/ahrscalibration.cpp @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.h b/ground/src/plugins/uavobjects/ahrscalibration.h index 7004e111c..fc9dd7493 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.h +++ b/ground/src/plugins/uavobjects/ahrscalibration.h @@ -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; diff --git a/ground/src/plugins/uavobjects/ahrscalibration.py b/ground/src/plugins/uavobjects/ahrscalibration.py index 856aa40f0..173c8195b 100644 --- a/ground/src/plugins/uavobjects/ahrscalibration.py +++ b/ground/src/plugins/uavobjects/ahrscalibration.py @@ -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 diff --git a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml index 5936fab47..12ff99469 100644 --- a/ground/src/shared/uavobjectdefinition/ahrscalibration.xml +++ b/ground/src/shared/uavobjectdefinition/ahrscalibration.xml @@ -2,13 +2,14 @@ Contains the calibration settings for the @ref AHRSCommsModule - + - - + + - + +