From 12657440c372e562e4a5983a85de285ddd871f58 Mon Sep 17 00:00:00 2001 From: peabody124 Date: Tue, 31 Aug 2010 02:11:09 +0000 Subject: [PATCH] OP-132 AHRS: Added an indoor flight mode and also add an ad-hoc quality heuristic to the GPS signal to increase the measurement variance from GPS when fix quality is poor or when in indoor flight mode. In addition corrected the indentation to conform to the tab/space convention forum discussion. git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1474 ebee16cc-31ac-478f-84a7-5cbb03baadba --- flight/AHRS/ahrs.c | 1182 +++++++++-------- flight/AHRS/inc/insgps.h | 43 +- flight/AHRS/insgps.c | 12 +- .../OpenPilot/Modules/AHRSComms/ahrs_comms.c | 11 +- flight/OpenPilot/UAVObjects/homelocation.c | 1 + .../OpenPilot/UAVObjects/inc/homelocation.h | 6 +- .../src/plugins/config/configahrswidget.cpp | 3 + .../src/plugins/uavobjects/homelocation.cpp | 7 + ground/src/plugins/uavobjects/homelocation.h | 6 +- ground/src/plugins/uavobjects/homelocation.py | 14 +- .../uavobjectdefinition/homelocation.xml | 1 + 11 files changed, 667 insertions(+), 619 deletions(-) diff --git a/flight/AHRS/ahrs.c b/flight/AHRS/ahrs.c index 3831c8e9c..8a6a7d70f 100644 --- a/flight/AHRS/ahrs.c +++ b/flight/AHRS/ahrs.c @@ -97,62 +97,62 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler"))) * @{ */ struct mag_sensor { - uint8_t id[4]; - struct { - int16_t axis[3]; - } raw; + uint8_t id[4]; + struct { + int16_t axis[3]; + } raw; }; struct accel_sensor { - struct { - uint16_t x; - uint16_t y; - uint16_t z; - } raw; - struct { - float x; - float y; - float z; - } filtered; + struct { + uint16_t x; + uint16_t y; + uint16_t z; + } raw; + struct { + float x; + float y; + float z; + } filtered; }; struct gyro_sensor { - struct { - uint16_t x; - uint16_t y; - uint16_t z; - } raw; - struct { - float x; - float y; - float z; - } filtered; - struct { - uint16_t xy; - uint16_t z; - } temp; + struct { + uint16_t x; + uint16_t y; + uint16_t z; + } raw; + struct { + float x; + float y; + float z; + } filtered; + struct { + uint16_t xy; + uint16_t z; + } temp; }; struct attitude_solution { - struct { - float q1; - float q2; - float q3; - float q4; - } quaternion; + struct { + float q1; + float q2; + float q3; + float q4; + } quaternion; }; struct altitude_sensor { - float altitude; - bool updated; + float altitude; + bool updated; }; struct gps_sensor { - float NED[3]; - float heading; - float groundspeed; - float quality; - bool updated; + float NED[3]; + float heading; + float groundspeed; + float quality; + bool updated; }; static struct mag_sensor mag_data; @@ -219,64 +219,64 @@ static uint32_t idle_counter = 0; /** -* @brief AHRS Main function -*/ + * @brief AHRS Main function + */ int main() { - float gyro[3], accel[3], mag[3]; - float vel[3] = {0,0,0}; - - ahrs_algorithm = INSGPS_Algo; - - /* Brings up System using CMSIS functions, enables the LEDs. */ - PIOS_SYS_Init(); - - /* Delay system */ - PIOS_DELAY_Init(); - - /* Communication system */ - PIOS_COM_Init(); - - /* ADC system */ - AHRS_ADC_Config(EKF_RATE, ADC_OVERSAMPLE); - - /* Magnetic sensor system */ - PIOS_I2C_Init(); - PIOS_HMC5843_Init(); - - /* Setup the Accelerometer FS (Full-Scale) GPIO */ - PIOS_GPIO_Enable(0); - SET_ACCEL_2G; - - /* Configure the HMC5843 Sensor */ - PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; - HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; - HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; - HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; - HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; - PIOS_HMC5843_Config(&HMC5843_InitStructure); - - // Get 3 ID bytes - strcpy ((char *)mag_data.id, "ZZZ"); - PIOS_HMC5843_ReadID(mag_data.id); - - /* SPI link to master */ - PIOS_SPI_Init(); - - lfsm_init(); + float gyro[3], accel[3], mag[3]; + float vel[3] = {0,0,0}; + + ahrs_algorithm = INSGPS_Algo; + + /* Brings up System using CMSIS functions, enables the LEDs. */ + PIOS_SYS_Init(); + + /* Delay system */ + PIOS_DELAY_Init(); + + /* Communication system */ + PIOS_COM_Init(); + + /* ADC system */ + AHRS_ADC_Config(EKF_RATE, ADC_OVERSAMPLE); + + /* Magnetic sensor system */ + PIOS_I2C_Init(); + PIOS_HMC5843_Init(); + + /* Setup the Accelerometer FS (Full-Scale) GPIO */ + PIOS_GPIO_Enable(0); + SET_ACCEL_2G; + + /* Configure the HMC5843 Sensor */ + PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; + HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; + HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; + HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; + HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; + PIOS_HMC5843_Config(&HMC5843_InitStructure); + + // Get 3 ID bytes + strcpy ((char *)mag_data.id, "ZZZ"); + PIOS_HMC5843_ReadID(mag_data.id); + + /* SPI link to master */ + PIOS_SPI_Init(); + + lfsm_init(); + + ahrs_state = AHRS_IDLE;; + /* Use simple averaging filter for now */ + for (int i = 0; i < ADC_OVERSAMPLE; i++) + fir_coeffs[i] = 1; + fir_coeffs[ADC_OVERSAMPLE] = ADC_OVERSAMPLE; + + if(ahrs_algorithm == INSGPS_Algo) { + // compute a data point and initialize INS + downsample_data(); + converge_insgps(); + } - ahrs_state = AHRS_IDLE;; - /* Use simple averaging filter for now */ - for (int i = 0; i < ADC_OVERSAMPLE; i++) - fir_coeffs[i] = 1; - fir_coeffs[ADC_OVERSAMPLE] = ADC_OVERSAMPLE; - - if(ahrs_algorithm == INSGPS_Algo) { - // compute a data point and initialize INS - downsample_data(); - converge_insgps(); - } - #ifdef DUMP_RAW while(1) { int result; @@ -286,7 +286,7 @@ int main() downsample_data(); ahrs_state = AHRS_IDLE;; - + // Dump raw buffer result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &sync[0], 4); // dump block number result += PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number @@ -298,113 +298,117 @@ int main() } } #endif - - /******************* Main EKF loop ****************************/ - while (1) { - // Alive signal - PIOS_LED_Toggle(LED1); + + /******************* Main EKF loop ****************************/ + while (1) { + // Alive signal + PIOS_LED_Toggle(LED1); - if(calibration_pending) - { - calibrate_sensors(); - calibration_pending = FALSE; - } - - // Get magnetic readings - PIOS_HMC5843_ReadMag(mag_data.raw.axis); - - // Delay for valid data - idle_counter = 0; - while( ahrs_state != AHRS_DATA_READY ) - idle_counter ++; - - ahrs_state = AHRS_PROCESSING; - - 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 **************************/ - - // 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]); + if(calibration_pending) + { + calibrate_sensors(); + calibration_pending = FALSE; + } + + // Get magnetic readings + PIOS_HMC5843_ReadMag(mag_data.raw.axis); + + // Delay for valid data + idle_counter = 0; + while( ahrs_state != AHRS_DATA_READY ) + idle_counter ++; + + ahrs_state = AHRS_PROCESSING; + + 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 **************************/ - INSPrediction(gyro, accel, 1 / (float) EKF_RATE); - - if ( gps_data.updated ) - { - // Compute velocity from Heading and groundspeed - vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180); - vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180); - vel[2] = 0; + // 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]); + + INSPrediction(gyro, accel, 1 / (float) EKF_RATE); + + if ( gps_data.updated ) + { + // Compute velocity from Heading and groundspeed + vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180); + vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180); + vel[2] = 0; + + // 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 / (.00001 + gps_data.quality)); + FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude); + gps_data.updated = false; + } + else + MagCorrection(mag); + + attitude_data.quaternion.q1 = Nav.q[0]; + attitude_data.quaternion.q2 = Nav.q[1]; + attitude_data.quaternion.q3 = Nav.q[2]; + attitude_data.quaternion.q4 = Nav.q[3]; + } + else if( ahrs_algorithm == SIMPLE_Algo ) + { + float q[4]; + float rpy[3]; + /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ + /* Very simple computation of the heading and attitude from accel. */ + rpy[2] = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI; + 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; + + RPY2Quaternion(rpy,q); + attitude_data.quaternion.q1 = q[0]; + attitude_data.quaternion.q2 = q[1]; + attitude_data.quaternion.q3 = q[2]; + attitude_data.quaternion.q4 = q[3]; + } + + ahrs_state = AHRS_IDLE; - FullCorrection(mag, gps_data.NED, vel, altitude_data.altitude); - gps_data.updated = false; - } - else - MagCorrection(mag); - - attitude_data.quaternion.q1 = Nav.q[0]; - attitude_data.quaternion.q2 = Nav.q[1]; - attitude_data.quaternion.q3 = Nav.q[2]; - attitude_data.quaternion.q4 = Nav.q[3]; - } - else if( ahrs_algorithm == SIMPLE_Algo ) - { - float q[4]; - float rpy[3]; - /***************** SIMPLE ATTITUDE FROM NORTH AND ACCEL ************/ - /* Very simple computation of the heading and attitude from accel. */ - rpy[2] = atan2((mag_data.raw.axis[0]), (-1 * mag_data.raw.axis[1])) * 180 / M_PI; - 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; - - RPY2Quaternion(rpy,q); - attitude_data.quaternion.q1 = q[0]; - attitude_data.quaternion.q2 = q[1]; - attitude_data.quaternion.q3 = q[2]; - attitude_data.quaternion.q4 = q[3]; - } - - ahrs_state = AHRS_IDLE; - #ifdef DUMP_FRIENDLY - PIOS_COM_SendFormattedString(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 100), (int16_t)(accel_data.filtered.y * 100), (int16_t)(accel_data.filtered.z * 100)); - PIOS_COM_SendFormattedString(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 100), (int16_t)(gyro_data.filtered.y * 100), (int16_t)(gyro_data.filtered.z * 100)); - PIOS_COM_SendFormattedString(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]); - PIOS_COM_SendFormattedString(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 100), (int16_t)(Nav.q[1] * 100), (int16_t)(Nav.q[2] * 100), (int16_t)(Nav.q[3] * 100)); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "a: %d %d %d\r\n", (int16_t)(accel_data.filtered.x * 100), (int16_t)(accel_data.filtered.y * 100), (int16_t)(accel_data.filtered.z * 100)); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "g: %d %d %d\r\n", (int16_t)(gyro_data.filtered.x * 100), (int16_t)(gyro_data.filtered.y * 100), (int16_t)(gyro_data.filtered.z * 100)); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "m: %d %d %d\r\n", mag_data.raw.axis[0], mag_data.raw.axis[1], mag_data.raw.axis[2]); + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "q: %d %d %d %d\r\n", (int16_t)(Nav.q[0] * 100), (int16_t)(Nav.q[1] * 100), (int16_t)(Nav.q[2] * 100), (int16_t)(Nav.q[3] * 100)); #endif - - process_spi_request(); + + process_spi_request(); + + } - } - - return 0; + return 0; } /** @@ -421,48 +425,48 @@ int main() */ void downsample_data() { - int16_t temp; - 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; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - { - temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[1] ) * fir_coeffs[i]; - accel_raw[0] += temp; - } - accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[1]; - - // Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s - accel_raw[1] = 0; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - accel_raw[1] += ( valid_data_buffer[2 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[0] ) * fir_coeffs[i]; - accel_data.filtered.x = (float) accel_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[0]; - - // Get the Z data. Third byte in. Convert to m/s - accel_raw[2] = 0; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - accel_raw[2] += ( valid_data_buffer[4 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[2] ) * fir_coeffs[i]; - accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[2]; - - // Get the X gyro data. Seventh byte in. Convert to deg/s. - gyro_raw[0] = 0; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - gyro_raw[0] += ( valid_data_buffer[1 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[0] ) * fir_coeffs[i]; - gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[0]; - - // Get the Y gyro data. Second byte in. Convert to deg/s. - gyro_raw[1] = 0; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - gyro_raw[1] += ( valid_data_buffer[3 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[1] ) * fir_coeffs[i]; - gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[1]; - - // Get the Z gyro data. Fifth byte in. Convert to deg/s. - gyro_raw[2] = 0; - for( i = 0; i < ADC_OVERSAMPLE; i++ ) - gyro_raw[2] += ( valid_data_buffer[5 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[2] ) * fir_coeffs[i]; - gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[2]; + int16_t temp; + 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; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + { + temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[1] ) * fir_coeffs[i]; + accel_raw[0] += temp; + } + accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[1]; + + // Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s + accel_raw[1] = 0; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + accel_raw[1] += ( valid_data_buffer[2 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[0] ) * fir_coeffs[i]; + accel_data.filtered.x = (float) accel_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[0]; + + // Get the Z data. Third byte in. Convert to m/s + accel_raw[2] = 0; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + accel_raw[2] += ( valid_data_buffer[4 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[2] ) * fir_coeffs[i]; + accel_data.filtered.z = -(float) accel_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[2]; + + // Get the X gyro data. Seventh byte in. Convert to deg/s. + gyro_raw[0] = 0; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + gyro_raw[0] += ( valid_data_buffer[1 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[0] ) * fir_coeffs[i]; + gyro_data.filtered.x = (float) gyro_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[0]; + + // Get the Y gyro data. Second byte in. Convert to deg/s. + gyro_raw[1] = 0; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + gyro_raw[1] += ( valid_data_buffer[3 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[1] ) * fir_coeffs[i]; + gyro_data.filtered.y = (float) gyro_raw[1] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[1]; + + // Get the Z gyro data. Fifth byte in. Convert to deg/s. + gyro_raw[2] = 0; + for( i = 0; i < ADC_OVERSAMPLE; i++ ) + gyro_raw[2] += ( valid_data_buffer[5 + i * ADC_CONTINUOUS_CHANNELS] + gyro_bias[2] ) * fir_coeffs[i]; + gyro_data.filtered.z = (float) gyro_raw[2] / (float) fir_coeffs[ADC_OVERSAMPLE] * gyro_scale[2]; } /** @@ -473,82 +477,82 @@ void downsample_data() * aircraft is in stable state and then the data stored to SD card. */ void calibrate_sensors() { - int i; - int16_t mag_raw[3]; - // local biases for noise analysis - float accel_bias[3], gyro_bias[3], mag_bias[3]; - - // 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++) { - while( ahrs_state != AHRS_DATA_READY ); - ahrs_state = AHRS_PROCESSING; - downsample_data(); - gyro_bias[0] += gyro_data.filtered.x; - gyro_bias[1] += gyro_data.filtered.y; - gyro_bias[2] += gyro_data.filtered.z; - accel_bias[0] += accel_data.filtered.x; - accel_bias[1] += accel_data.filtered.y; - accel_bias[2] += accel_data.filtered.z; - PIOS_HMC5843_ReadMag(mag_raw); - mag_bias[0] += mag_raw[0]; - mag_bias[1] += mag_raw[1]; - mag_bias[2] += mag_raw[2]; + int i; + int16_t mag_raw[3]; + // local biases for noise analysis + float accel_bias[3], gyro_bias[3], mag_bias[3]; - ahrs_state = AHRS_IDLE; - 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; - - // 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]); - PIOS_HMC5843_ReadMag(mag_raw); - 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; + // 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++) { + while( ahrs_state != AHRS_DATA_READY ); + ahrs_state = AHRS_PROCESSING; + downsample_data(); + gyro_bias[0] += gyro_data.filtered.x; + gyro_bias[1] += gyro_data.filtered.y; + gyro_bias[2] += gyro_data.filtered.z; + accel_bias[0] += accel_data.filtered.x; + accel_bias[1] += accel_data.filtered.y; + accel_bias[2] += accel_data.filtered.z; + PIOS_HMC5843_ReadMag(mag_raw); + mag_bias[0] += mag_raw[0]; + mag_bias[1] += mag_raw[1]; + mag_bias[2] += mag_raw[2]; + + ahrs_state = AHRS_IDLE; + 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; - if(ahrs_algorithm == INSGPS_Algo) - converge_insgps(); + // 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]); + PIOS_HMC5843_ReadMag(mag_raw); + 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(); } /** @@ -559,42 +563,42 @@ 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}; - INSGPSInit(); - INSSetAccelVar(accel_var); - INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data - INSSetGyroVar(gyro_var); - INSSetMagVar(mag_var); + float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {0, 0, 0}; + INSGPSInit(); + INSSetAccelVar(accel_var); + INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data + INSSetGyroVar(gyro_var); + 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 + PIOS_HMC5843_ReadMag(mag_data.raw.axis); + 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); + INSPrediction( temp_gyro, accel, 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); - 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 - PIOS_HMC5843_ReadMag(mag_data.raw.axis); - 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); - INSPrediction( temp_gyro, accel, 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); - } /** @@ -622,204 +626,204 @@ static struct opahrs_msg_v1 user_tx_v1; void process_spi_request(void) { - bool msg_to_process = FALSE; - - PIOS_IRQ_Disable(); - /* Figure out if we're in an interesting stable state */ - switch (lfsm_get_state()) { - case LFSM_STATE_USER_BUSY: - msg_to_process = TRUE; - break; - case LFSM_STATE_INACTIVE: - /* Queue up a receive buffer */ + bool msg_to_process = FALSE; + + PIOS_IRQ_Disable(); + /* Figure out if we're in an interesting stable state */ + switch (lfsm_get_state()) { + case LFSM_STATE_USER_BUSY: + msg_to_process = TRUE; + break; + case LFSM_STATE_INACTIVE: + /* Queue up a receive buffer */ + lfsm_user_set_rx_v1 (&user_rx_v1); + lfsm_user_done (); + break; + case LFSM_STATE_STOPPED: + /* Get things going */ + lfsm_set_link_proto_v1 (&link_tx_v1, &link_rx_v1); + break; + default: + /* Not a stable state */ + break; + } + PIOS_IRQ_Enable(); + + if (!msg_to_process) { + /* Nothing to do */ + //PIOS_COM_SendFormattedString(PIOS_COM_AUX, "."); + return; + } + + if (user_rx_v1.tail.magic != OPAHRS_MSG_MAGIC_TAIL) { + PIOS_COM_SendFormattedString(PIOS_COM_AUX, "x"); + } + + /* We've got a message to process */ + //dump_spi_message(PIOS_COM_AUX, "+", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); + + switch (user_rx_v1.payload.user.t) { + case OPAHRS_MSG_V1_REQ_SYNC: + opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SYNC); + user_tx_v1.payload.user.v.rsp.sync.i_am_a_bootloader = FALSE; + user_tx_v1.payload.user.v.rsp.sync.hw_version = 1; + user_tx_v1.payload.user.v.rsp.sync.bl_version = 2; + user_tx_v1.payload.user.v.rsp.sync.fw_version = 3; + user_tx_v1.payload.user.v.rsp.sync.cookie = user_rx_v1.payload.user.v.req.sync.cookie; + dump_spi_message(PIOS_COM_AUX, "S", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); + lfsm_user_set_tx_v1 (&user_tx_v1); + break; + case OPAHRS_MSG_V1_REQ_RESET: + PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.reset_delay_in_ms); + 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.serial.serial_bcd)); + dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); + 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; + dump_spi_message(PIOS_COM_AUX, "A", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); + 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); + INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be); + dump_spi_message(PIOS_COM_AUX, "N", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); + lfsm_user_set_tx_v1 (&user_tx_v1); + break; + case OPAHRS_MSG_V1_REQ_CALIBRATION: + 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]; + } + // 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]; + + dump_spi_message(PIOS_COM_AUX, "C", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); + 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); + 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 = mag_data.raw.axis[1]; + user_tx_v1.payload.user.v.rsp.attituderaw.mags.z = mag_data.raw.axis[2]; + + user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x = gyro_data.raw.x; + user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y = gyro_data.raw.y; + user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z = gyro_data.raw.z; + + user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.x = gyro_data.filtered.x; + user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.y = gyro_data.filtered.y; + user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.z = gyro_data.filtered.z; + + user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp = gyro_data.temp.xy; + user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp = gyro_data.temp.z; + + user_tx_v1.payload.user.v.rsp.attituderaw.accels.x = accel_data.raw.x; + user_tx_v1.payload.user.v.rsp.attituderaw.accels.y = accel_data.raw.y; + user_tx_v1.payload.user.v.rsp.attituderaw.accels.z = accel_data.raw.z; + + user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.x = accel_data.filtered.x; + user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.y = accel_data.filtered.y; + user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.z = accel_data.filtered.z; + + dump_spi_message(PIOS_COM_AUX, "R", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); + lfsm_user_set_tx_v1 (&user_tx_v1); + break; + case OPAHRS_MSG_V1_REQ_UPDATE: + // process incoming data + opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_UPDATE); + if(user_rx_v1.payload.user.v.req.update.barometer.updated) + { + altitude_data.altitude = user_rx_v1.payload.user.v.req.update.barometer.altitude; + altitude_data.updated = user_rx_v1.payload.user.v.req.update.barometer.updated; + } + 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; + } + + // send out attitude/position estimate + user_tx_v1.payload.user.v.rsp.update.quaternion.q1 = attitude_data.quaternion.q1; + user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2; + user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3; + user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4; + + // TODO: separate this from INSGPS + user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0]; + user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1]; + user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2]; + user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0]; + user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1]; + user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2]; + + // compute the idle fraction + user_tx_v1.payload.user.v.rsp.update.load = (MAX_IDLE_COUNT - idle_counter) * 100 / MAX_IDLE_COUNT; + + dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); + lfsm_user_set_tx_v1 (&user_tx_v1); + break; + default: + break; + } + + /* Finished processing the received message, requeue it */ lfsm_user_set_rx_v1 (&user_rx_v1); lfsm_user_done (); - break; - case LFSM_STATE_STOPPED: - /* Get things going */ - lfsm_set_link_proto_v1 (&link_tx_v1, &link_rx_v1); - break; - default: - /* Not a stable state */ - break; - } - PIOS_IRQ_Enable(); - - if (!msg_to_process) { - /* Nothing to do */ - //PIOS_COM_SendFormattedString(PIOS_COM_AUX, "."); - return; - } - - if (user_rx_v1.tail.magic != OPAHRS_MSG_MAGIC_TAIL) { - PIOS_COM_SendFormattedString(PIOS_COM_AUX, "x"); - } - - /* We've got a message to process */ - //dump_spi_message(PIOS_COM_AUX, "+", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); - - switch (user_rx_v1.payload.user.t) { - case OPAHRS_MSG_V1_REQ_SYNC: - opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_SYNC); - user_tx_v1.payload.user.v.rsp.sync.i_am_a_bootloader = FALSE; - user_tx_v1.payload.user.v.rsp.sync.hw_version = 1; - user_tx_v1.payload.user.v.rsp.sync.bl_version = 2; - user_tx_v1.payload.user.v.rsp.sync.fw_version = 3; - user_tx_v1.payload.user.v.rsp.sync.cookie = user_rx_v1.payload.user.v.req.sync.cookie; - dump_spi_message(PIOS_COM_AUX, "S", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); - lfsm_user_set_tx_v1 (&user_tx_v1); - break; - case OPAHRS_MSG_V1_REQ_RESET: - PIOS_DELAY_WaitmS(user_rx_v1.payload.user.v.req.reset.reset_delay_in_ms); - 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.serial.serial_bcd)); - dump_spi_message(PIOS_COM_AUX, "I", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); - 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; - dump_spi_message(PIOS_COM_AUX, "A", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); - 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); - INSSetMagNorth(user_rx_v1.payload.user.v.req.north.Be); - dump_spi_message(PIOS_COM_AUX, "N", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); - lfsm_user_set_tx_v1 (&user_tx_v1); - break; - case OPAHRS_MSG_V1_REQ_CALIBRATION: - 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]; - } - // 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]; - - dump_spi_message(PIOS_COM_AUX, "C", (uint8_t *)&user_rx_v1, sizeof(user_rx_v1)); - 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); - 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 = mag_data.raw.axis[1]; - user_tx_v1.payload.user.v.rsp.attituderaw.mags.z = mag_data.raw.axis[2]; - - user_tx_v1.payload.user.v.rsp.attituderaw.gyros.x = gyro_data.raw.x; - user_tx_v1.payload.user.v.rsp.attituderaw.gyros.y = gyro_data.raw.y; - user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z = gyro_data.raw.z; - - user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.x = gyro_data.filtered.x; - user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.y = gyro_data.filtered.y; - user_tx_v1.payload.user.v.rsp.attituderaw.gyros_filtered.z = gyro_data.filtered.z; - - user_tx_v1.payload.user.v.rsp.attituderaw.gyros.xy_temp = gyro_data.temp.xy; - user_tx_v1.payload.user.v.rsp.attituderaw.gyros.z_temp = gyro_data.temp.z; - - user_tx_v1.payload.user.v.rsp.attituderaw.accels.x = accel_data.raw.x; - user_tx_v1.payload.user.v.rsp.attituderaw.accels.y = accel_data.raw.y; - user_tx_v1.payload.user.v.rsp.attituderaw.accels.z = accel_data.raw.z; - - user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.x = accel_data.filtered.x; - user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.y = accel_data.filtered.y; - user_tx_v1.payload.user.v.rsp.attituderaw.accels_filtered.z = accel_data.filtered.z; - - dump_spi_message(PIOS_COM_AUX, "R", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); - lfsm_user_set_tx_v1 (&user_tx_v1); - break; - case OPAHRS_MSG_V1_REQ_UPDATE: - // process incoming data - opahrs_msg_v1_init_user_tx (&user_tx_v1, OPAHRS_MSG_V1_RSP_UPDATE); - if(user_rx_v1.payload.user.v.req.update.barometer.updated) - { - altitude_data.altitude = user_rx_v1.payload.user.v.req.update.barometer.altitude; - altitude_data.updated = user_rx_v1.payload.user.v.req.update.barometer.updated; - } - 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; - } - - // send out attitude/position estimate - user_tx_v1.payload.user.v.rsp.update.quaternion.q1 = attitude_data.quaternion.q1; - user_tx_v1.payload.user.v.rsp.update.quaternion.q2 = attitude_data.quaternion.q2; - user_tx_v1.payload.user.v.rsp.update.quaternion.q3 = attitude_data.quaternion.q3; - user_tx_v1.payload.user.v.rsp.update.quaternion.q4 = attitude_data.quaternion.q4; - - // TODO: separate this from INSGPS - user_tx_v1.payload.user.v.rsp.update.NED[0] = Nav.Pos[0]; - user_tx_v1.payload.user.v.rsp.update.NED[1] = Nav.Pos[1]; - user_tx_v1.payload.user.v.rsp.update.NED[2] = Nav.Pos[2]; - user_tx_v1.payload.user.v.rsp.update.Vel[0] = Nav.Vel[0]; - user_tx_v1.payload.user.v.rsp.update.Vel[1] = Nav.Vel[1]; - user_tx_v1.payload.user.v.rsp.update.Vel[2] = Nav.Vel[2]; - - // compute the idle fraction - user_tx_v1.payload.user.v.rsp.update.load = (MAX_IDLE_COUNT - idle_counter) * 100 / MAX_IDLE_COUNT; - - dump_spi_message(PIOS_COM_AUX, "U", (uint8_t *)&user_tx_v1, sizeof(user_tx_v1)); - lfsm_user_set_tx_v1 (&user_tx_v1); - break; - default: - break; - } - - /* Finished processing the received message, requeue it */ - lfsm_user_set_rx_v1 (&user_rx_v1); - lfsm_user_done (); } /** * @} @@ -844,35 +848,35 @@ static const uint32_t ADC_CHANNEL_MAPPING[PIOS_ADC_NUM_PINS] = PIOS_ADC_CHANNEL_ */ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) { - + int32_t i; - + /* Setup analog pins */ GPIO_InitTypeDef GPIO_InitStructure; GPIO_StructInit(&GPIO_InitStructure); GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; - + /* Enable each ADC pin in the array */ for(i = 0; i < PIOS_ADC_NUM_PINS; i++) { GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); } - + /* Enable ADC clocks */ PIOS_ADC_CLOCK_FUNCTION; - + /* Map channels to conversion slots depending on the channel selection mask */ for(i = 0; i < PIOS_ADC_NUM_PINS; i++) { ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], ADC_CHANNEL_MAPPING[i], PIOS_ADC_SAMPLE_TIME); } - + #if (PIOS_ADC_USE_TEMP_SENSOR) ADC_TempSensorVrefintCmd(ENABLE); ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_14, PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, PIOS_ADC_SAMPLE_TIME); #endif - - // TODO: update ADC to continuous sampling, configure the sampling rate + + // TODO: update ADC to continuous sampling, configure the sampling rate /* Configure ADCs */ ADC_InitTypeDef ADC_InitStructure; ADC_StructInit(&ADC_InitStructure); @@ -883,27 +887,27 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) ADC_InitStructure.ADC_DataAlign = ADC_DataAlign_Right; ADC_InitStructure.ADC_NbrOfChannel = 4; //((PIOS_ADC_NUM_CHANNELS + 1) >> 1); ADC_Init(ADC1, &ADC_InitStructure); - + #if (PIOS_ADC_USE_ADC2) ADC_Init(ADC2, &ADC_InitStructure); - + /* Enable ADC2 external trigger conversion (to synch with ADC1) */ ADC_ExternalTrigConvCmd(ADC2, ENABLE); #endif - + RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); - RCC_PCLK2Config(RCC_HCLK_Div16); - + RCC_PCLK2Config(RCC_HCLK_Div16); + /* Enable ADC1->DMA request */ ADC_DMACmd(ADC1, ENABLE); - + /* ADC1 calibration */ ADC_Cmd(ADC1, ENABLE); ADC_ResetCalibration(ADC1); while(ADC_GetResetCalibrationStatus(ADC1)); ADC_StartCalibration(ADC1); while(ADC_GetCalibrationStatus(ADC1)); - + #if (PIOS_ADC_USE_ADC2) /* ADC2 calibration */ ADC_Cmd(ADC2, ENABLE); @@ -912,10 +916,10 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) ADC_StartCalibration(ADC2); while(ADC_GetCalibrationStatus(ADC2)); #endif - + /* Enable DMA1 clock */ RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); - + /* Configure DMA1 channel 1 to fetch data from ADC result register */ DMA_InitTypeDef DMA_InitStructure; DMA_StructInit(&DMA_InitStructure); @@ -923,11 +927,11 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) DMA_InitStructure.DMA_PeripheralBaseAddr = (uint32_t)&ADC1->DR; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&raw_data_buffer[0]; DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; - /* We are double buffering half words from the ADC. Make buffer appropriately sized */ + /* We are double buffering half words from the ADC. Make buffer appropriately sized */ DMA_InitStructure.DMA_BufferSize = (ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE * 2) >> 1; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; - /* Note: We read ADC1 and ADC2 in parallel making a word read, also hence the half buffer size */ + /* Note: We read ADC1 and ADC2 in parallel making a word read, also hence the half buffer size */ DMA_InitStructure.DMA_PeripheralDataSize = DMA_PeripheralDataSize_Word; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; @@ -935,11 +939,11 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) DMA_InitStructure.DMA_M2M = DMA_M2M_Disable; DMA_Init(DMA1_Channel1, &DMA_InitStructure); DMA_Cmd(DMA1_Channel1, ENABLE); - + /* Trigger interrupt when for half conversions too to indicate double buffer */ DMA_ITConfig(DMA1_Channel1, DMA_IT_TC, ENABLE); DMA_ITConfig(DMA1_Channel1, DMA_IT_HT, ENABLE); - + /* Configure and enable DMA interrupt */ NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn; @@ -947,7 +951,7 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init(&NVIC_InitStructure); - + /* Finally start initial conversion */ ADC_SoftwareStartConvCmd(ADC1, ENABLE); } @@ -963,29 +967,29 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample) */ void AHRS_ADC_DMA_Handler(void) { - if ( ahrs_state == AHRS_IDLE ) - { - // Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features) - - if( DMA_GetFlagStatus( DMA1_IT_TC1 ) ) // whole double buffer filled - valid_data_buffer = &raw_data_buffer[ 1 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; - else if ( DMA_GetFlagStatus(DMA1_IT_HT1) ) - valid_data_buffer = &raw_data_buffer[ 0 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; + if ( ahrs_state == AHRS_IDLE ) + { + // Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features) + + if( DMA_GetFlagStatus( DMA1_IT_TC1 ) ) // whole double buffer filled + valid_data_buffer = &raw_data_buffer[ 1 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; + else if ( DMA_GetFlagStatus(DMA1_IT_HT1) ) + valid_data_buffer = &raw_data_buffer[ 0 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; + else { + // lets cause a seg fault and catch whatever is going on + valid_data_buffer = 0; + } + + ahrs_state = AHRS_DATA_READY; + } else { - // lets cause a seg fault and catch whatever is going on - valid_data_buffer = 0; + // Track how many times an interrupt occurred before EKF finished processing + ekf_too_slow++; } - ahrs_state = AHRS_DATA_READY; - } - else { - // Track how many times an interrupt occurred before EKF finished processing - ekf_too_slow++; - } - - total_conversion_blocks++; - - // Clear all interrupt from DMA 1 - regardless if buffer swapped - DMA_ClearFlag( DMA1_IT_GL1 ); - + total_conversion_blocks++; + + // Clear all interrupt from DMA 1 - regardless if buffer swapped + DMA_ClearFlag( DMA1_IT_GL1 ); + } diff --git a/flight/AHRS/inc/insgps.h b/flight/AHRS/inc/insgps.h index 71a936c58..d9d570728 100644 --- a/flight/AHRS/inc/insgps.h +++ b/flight/AHRS/inc/insgps.h @@ -27,28 +27,29 @@ #ifndef INSGPS_H_ #define INSGPS_H_ - // Exposed Function Prototypes - void INSGPSInit(); - void INSPrediction(float gyro_data[3], float accel_data[3], float dT); - void INSSetGyroBias(float gyro_bias[3]); - void INSSetAccelVar(float accel_var[3]); - void INSSetGyroVar(float gyro_var[3]); - void INSSetMagNorth(float B[3]); - void INSSetMagVar(float scaled_mag_var[3]); - void MagCorrection(float mag_data[3]); - void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt); - void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]); +// Exposed Function Prototypes +void INSGPSInit(); +void INSPrediction(float gyro_data[3], float accel_data[3], float dT); +void INSSetPosVelVar(float PosVar); +void INSSetGyroBias(float gyro_bias[3]); +void INSSetAccelVar(float accel_var[3]); +void INSSetGyroVar(float gyro_var[3]); +void INSSetMagNorth(float B[3]); +void INSSetMagVar(float scaled_mag_var[3]); +void MagCorrection(float mag_data[3]); +void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt); +void GndSpeedAndMagCorrection(float Speed, float Heading, float mag_data[3]); - // Nav structure containing current solution - struct NavStruct { - float Pos[3]; // Position in meters and relative to a local NED frame - float Vel[3]; // Velocity in meters and in NED - float q[4]; // unit quaternion rotation relative to NED - } Nav; +// Nav structure containing current solution +struct NavStruct { + float Pos[3]; // Position in meters and relative to a local NED frame + float Vel[3]; // Velocity in meters and in NED + float q[4]; // unit quaternion rotation relative to NED +} Nav; - // constants - #define MagSensors 0x1C0 - #define FullSensors 0x3FF - #define GndSpeedAndMagSensors 0x1D8 +// constants +#define MagSensors 0x1C0 +#define FullSensors 0x3FF +#define GndSpeedAndMagSensors 0x1D8 #endif /* EKF_H_ */ diff --git a/flight/AHRS/insgps.c b/flight/AHRS/insgps.c index 03c0659c6..1eaf70ada 100644 --- a/flight/AHRS/insgps.c +++ b/flight/AHRS/insgps.c @@ -74,11 +74,21 @@ void INSGPSInit() //pretty much just a place holder for now R[0]=R[1]=0.004; // High freq GPS horizontal position noise variance (m^2) R[2]=0.036; // High freq GPS vertical position noise variance (m^2) R[3]=R[4]=0.004; // High freq GPS horizontal velocity noise variance (m/s)^2 - R[5]=0; // High freq GPS vertical velocity noise variance (m/s)^2 + R[5]=100; // High freq GPS vertical velocity noise variance (m/s)^2 R[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance R[9]=1; // High freq altimeter noise variance (m^2) } +void INSSetPosVelVar(float PosVar) +{ + R[0] = PosVar; + R[1] = PosVar; + R[2] = PosVar; + R[3] = PosVar; + R[4] = PosVar; +// R[5] = PosVar; // Don't change vertical velocity, not measured +} + void INSSetGyroBias(float gyro_bias[3]) { X[10] = gyro_bias[0]; diff --git a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c index 3205014f0..1455f3a9c 100644 --- a/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c +++ b/flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c @@ -407,20 +407,21 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update) update->gps.updated = TRUE; - if(home.Set == HOMELOCATION_SET_FALSE) { + if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) { PositionActualData pos; PositionActualGet(&pos); - update->gps.NED[0] = pos.NED[0]; - update->gps.NED[1] = pos.NED[1]; - update->gps.NED[2] = pos.NED[2]; + update->gps.NED[0] = 0; + update->gps.NED[1] = 0; + update->gps.NED[2] = 0; update->gps.groundspeed = 0; update->gps.heading = 0; update->gps.quality = 0; } else { update->gps.groundspeed = data.Groundspeed; update->gps.heading = data.Heading; - update->gps.quality = 0; + //Crude measure of quality that decreases with increasing dilution of precision + update->gps.quality = 1 / (data.HDOP + data.VDOP + data.PDOP); double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) (data.GeoidSeparation + data.Altitude)}; // convert from cm back to meters double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)}; diff --git a/flight/OpenPilot/UAVObjects/homelocation.c b/flight/OpenPilot/UAVObjects/homelocation.c index 17565788f..972e6a29a 100644 --- a/flight/OpenPilot/UAVObjects/homelocation.c +++ b/flight/OpenPilot/UAVObjects/homelocation.c @@ -81,6 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId) UAVObjGetInstanceData(obj, instId, &data); memset(&data, 0, sizeof(HomeLocationData)); data.Set = 0; + data.Indoor = 0; data.Latitude = 0; data.Longitude = 0; data.Altitude = 0; diff --git a/flight/OpenPilot/UAVObjects/inc/homelocation.h b/flight/OpenPilot/UAVObjects/inc/homelocation.h index 937ba7918..0762d53b8 100644 --- a/flight/OpenPilot/UAVObjects/inc/homelocation.h +++ b/flight/OpenPilot/UAVObjects/inc/homelocation.h @@ -41,7 +41,7 @@ #define HOMELOCATION_H // Object constants -#define HOMELOCATION_OBJID 3590360786U +#define HOMELOCATION_OBJID 316455980U #define HOMELOCATION_NAME "HomeLocation" #define HOMELOCATION_METANAME "HomeLocationMeta" #define HOMELOCATION_ISSINGLEINST 1 @@ -72,6 +72,7 @@ // Object data typedef struct { uint8_t Set; + uint8_t Indoor; int32_t Latitude; int32_t Longitude; float Altitude; @@ -85,6 +86,9 @@ typedef struct { // Field Set information /* Enumeration options for field Set */ typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions; +// Field Indoor information +/* Enumeration options for field Indoor */ +typedef enum { HOMELOCATION_INDOOR_FALSE=0, HOMELOCATION_INDOOR_TRUE=1 } HomeLocationIndoorOptions; // Field Latitude information // Field Longitude information // Field Altitude information diff --git a/ground/src/plugins/config/configahrswidget.cpp b/ground/src/plugins/config/configahrswidget.cpp index c275acd12..607330ca3 100644 --- a/ground/src/plugins/config/configahrswidget.cpp +++ b/ground/src/plugins/config/configahrswidget.cpp @@ -260,9 +260,12 @@ void ConfigAHRSWidget::calibPhase2() // We need to echo back the results of calibration before changing to set mode m_ahrs->calibInstructions->setText("Getting results..."); + field->setValue("ECHO"); obj->updated(); + usleep(100e3); // wait for data to come back + // Now update size of all the graphs drawVariancesGraph(); diff --git a/ground/src/plugins/uavobjects/homelocation.cpp b/ground/src/plugins/uavobjects/homelocation.cpp index 46c6a7b72..f709c05c3 100644 --- a/ground/src/plugins/uavobjects/homelocation.cpp +++ b/ground/src/plugins/uavobjects/homelocation.cpp @@ -48,6 +48,12 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM SetEnumOptions.append("FALSE"); SetEnumOptions.append("TRUE"); fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) ); + QStringList IndoorElemNames; + IndoorElemNames.append("0"); + QStringList IndoorEnumOptions; + IndoorEnumOptions.append("FALSE"); + IndoorEnumOptions.append("TRUE"); + fields.append( new UAVObjectField(QString("Indoor"), QString(""), UAVObjectField::ENUM, IndoorElemNames, IndoorEnumOptions) ); QStringList LatitudeElemNames; LatitudeElemNames.append("0"); fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) ); @@ -112,6 +118,7 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata() void HomeLocation::setDefaultFieldValues() { data.Set = 0; + data.Indoor = 0; data.Latitude = 0; data.Longitude = 0; data.Altitude = 0; diff --git a/ground/src/plugins/uavobjects/homelocation.h b/ground/src/plugins/uavobjects/homelocation.h index 1bcd8dcfc..f596bc19d 100644 --- a/ground/src/plugins/uavobjects/homelocation.h +++ b/ground/src/plugins/uavobjects/homelocation.h @@ -44,6 +44,7 @@ public: // Field structure typedef struct { quint8 Set; + quint8 Indoor; qint32 Latitude; qint32 Longitude; float Altitude; @@ -57,6 +58,9 @@ public: // Field Set information /* Enumeration options for field Set */ typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions; + // Field Indoor information + /* Enumeration options for field Indoor */ + typedef enum { INDOOR_FALSE=0, INDOOR_TRUE=1 } IndoorOptions; // Field Latitude information // Field Longitude information // Field Altitude information @@ -72,7 +76,7 @@ public: // Constants - static const quint32 OBJID = 3590360786U; + static const quint32 OBJID = 316455980U; static const QString NAME; static const bool ISSINGLEINST = 1; static const bool ISSETTINGS = 1; diff --git a/ground/src/plugins/uavobjects/homelocation.py b/ground/src/plugins/uavobjects/homelocation.py index 6de803981..9d8c76ccc 100644 --- a/ground/src/plugins/uavobjects/homelocation.py +++ b/ground/src/plugins/uavobjects/homelocation.py @@ -49,6 +49,18 @@ _fields = [ \ '1' : 'TRUE', } ), + uavobject.UAVObjectField( + 'Indoor', + 'b', + 1, + [ + '0', + ], + { + '0' : 'FALSE', + '1' : 'TRUE', + } + ), uavobject.UAVObjectField( 'Latitude', 'i', @@ -126,7 +138,7 @@ _fields = [ \ class HomeLocation(uavobject.UAVObject): ## Object constants - OBJID = 3590360786 + OBJID = 316455980 NAME = "HomeLocation" METANAME = "HomeLocationMeta" ISSINGLEINST = 1 diff --git a/ground/src/shared/uavobjectdefinition/homelocation.xml b/ground/src/shared/uavobjectdefinition/homelocation.xml index 361f2550f..a276f81c6 100644 --- a/ground/src/shared/uavobjectdefinition/homelocation.xml +++ b/ground/src/shared/uavobjectdefinition/homelocation.xml @@ -2,6 +2,7 @@ HomeLocation setting which contains the constants to tranlate from longitutde and latitude to NED reference frame. Automatically set by @ref GPSModule after acquiring a 3D lock. Used by @ref AHRSCommsModule. +