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. +