rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-05 21:52:10 +01:00
Code Issues Releases Activity

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.

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@1474 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-08-31 02:11:09 +00:00 committed by peabody124
parent f632e9c568
commit 12657440c3
11 changed files with 667 additions and 619 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1182
flight/AHRS/ahrs.c
View File

@ -97,62 +97,62 @@ void DMA1_Channel1_IRQHandler() __attribute__ ((alias ("AHRS_ADC_DMA_Handler")))
* @{ * @{
*/ */
struct mag_sensor { struct mag_sensor {
uint8_t id[4]; uint8_t id[4];
struct { struct {
int16_t axis[3]; int16_t axis[3];
} raw; } raw;
}; };
struct accel_sensor { struct accel_sensor {
struct { struct {
uint16_t x; uint16_t x;
uint16_t y; uint16_t y;
uint16_t z; uint16_t z;
} raw; } raw;
struct { struct {
float x; float x;
float y; float y;
float z; float z;
} filtered; } filtered;
}; };
struct gyro_sensor { struct gyro_sensor {
struct { struct {
uint16_t x; uint16_t x;
uint16_t y; uint16_t y;
uint16_t z; uint16_t z;
} raw; } raw;
struct { struct {
float x; float x;
float y; float y;
float z; float z;
} filtered; } filtered;
struct { struct {
uint16_t xy; uint16_t xy;
uint16_t z; uint16_t z;
} temp; } temp;
}; };
struct attitude_solution { struct attitude_solution {
struct { struct {
float q1; float q1;
float q2; float q2;
float q3; float q3;
float q4; float q4;
} quaternion; } quaternion;
}; };
struct altitude_sensor { struct altitude_sensor {
float altitude; float altitude;
bool updated; bool updated;
}; };
struct gps_sensor { struct gps_sensor {
float NED[3]; float NED[3];
float heading; float heading;
float groundspeed; float groundspeed;
float quality; float quality;
bool updated; bool updated;
}; };
static struct mag_sensor mag_data; 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() int main()
{ {
float gyro[3], accel[3], mag[3]; float gyro[3], accel[3], mag[3];
float vel[3] = {0,0,0}; float vel[3] = {0,0,0};
ahrs_algorithm = INSGPS_Algo; ahrs_algorithm = INSGPS_Algo;
/* Brings up System using CMSIS functions, enables the LEDs. */ /* Brings up System using CMSIS functions, enables the LEDs. */
PIOS_SYS_Init(); PIOS_SYS_Init();
/* Delay system */ /* Delay system */
PIOS_DELAY_Init(); PIOS_DELAY_Init();
/* Communication system */ /* Communication system */
PIOS_COM_Init(); PIOS_COM_Init();
/* ADC system */ /* ADC system */
AHRS_ADC_Config(EKF_RATE, ADC_OVERSAMPLE); AHRS_ADC_Config(EKF_RATE, ADC_OVERSAMPLE);
/* Magnetic sensor system */ /* Magnetic sensor system */
PIOS_I2C_Init(); PIOS_I2C_Init();
PIOS_HMC5843_Init(); PIOS_HMC5843_Init();
/* Setup the Accelerometer FS (Full-Scale) GPIO */ /* Setup the Accelerometer FS (Full-Scale) GPIO */
PIOS_GPIO_Enable(0); PIOS_GPIO_Enable(0);
SET_ACCEL_2G; SET_ACCEL_2G;
/* Configure the HMC5843 Sensor */ /* Configure the HMC5843 Sensor */
PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure; PIOS_HMC5843_ConfigTypeDef HMC5843_InitStructure;
HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10; HMC5843_InitStructure.M_ODR = PIOS_HMC5843_ODR_10;
HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL; HMC5843_InitStructure.Meas_Conf = PIOS_HMC5843_MEASCONF_NORMAL;
HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2; HMC5843_InitStructure.Gain = PIOS_HMC5843_GAIN_2;
HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS; HMC5843_InitStructure.Mode = PIOS_HMC5843_MODE_CONTINUOUS;
PIOS_HMC5843_Config(&HMC5843_InitStructure); PIOS_HMC5843_Config(&HMC5843_InitStructure);
// Get 3 ID bytes // Get 3 ID bytes
strcpy ((char *)mag_data.id, "ZZZ"); strcpy ((char *)mag_data.id, "ZZZ");
PIOS_HMC5843_ReadID(mag_data.id); PIOS_HMC5843_ReadID(mag_data.id);
/* SPI link to master */ /* SPI link to master */
PIOS_SPI_Init(); PIOS_SPI_Init();
lfsm_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 #ifdef DUMP_RAW
while(1) { while(1) {
int result; int result;
@ -286,7 +286,7 @@ int main()
downsample_data(); downsample_data();
ahrs_state = AHRS_IDLE;; ahrs_state = AHRS_IDLE;;
// Dump raw buffer // Dump raw buffer
result = PIOS_COM_SendBuffer(PIOS_COM_AUX, &sync[0], 4); // dump block number 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 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 #endif
/******************* Main EKF loop ****************************/ /******************* Main EKF loop ****************************/
while (1) { while (1) {
// Alive signal // Alive signal
PIOS_LED_Toggle(LED1); PIOS_LED_Toggle(LED1);
if(calibration_pending) if(calibration_pending)
{ {
calibrate_sensors(); calibrate_sensors();
calibration_pending = FALSE; calibration_pending = FALSE;
} }
// Get magnetic readings // Get magnetic readings
PIOS_HMC5843_ReadMag(mag_data.raw.axis); PIOS_HMC5843_ReadMag(mag_data.raw.axis);
// Delay for valid data // Delay for valid data
idle_counter = 0; idle_counter = 0;
while( ahrs_state != AHRS_DATA_READY ) while( ahrs_state != AHRS_DATA_READY )
idle_counter ++; idle_counter ++;
ahrs_state = AHRS_PROCESSING; ahrs_state = AHRS_PROCESSING;
downsample_data(); downsample_data();
/***************** SEND BACK SOME RAW DATA ************************/ /***************** SEND BACK SOME RAW DATA ************************/
// Hacky - grab one sample from buffer to populate this. Need to send back // Hacky - grab one sample from buffer to populate this. Need to send back
// all raw data if it's happening // all raw data if it's happening
accel_data.raw.x = valid_data_buffer[0]; accel_data.raw.x = valid_data_buffer[0];
accel_data.raw.y = valid_data_buffer[2]; accel_data.raw.y = valid_data_buffer[2];
accel_data.raw.z = valid_data_buffer[4]; accel_data.raw.z = valid_data_buffer[4];
gyro_data.raw.x = valid_data_buffer[1]; gyro_data.raw.x = valid_data_buffer[1];
gyro_data.raw.y = valid_data_buffer[3]; gyro_data.raw.y = valid_data_buffer[3];
gyro_data.raw.z = valid_data_buffer[5]; gyro_data.raw.z = valid_data_buffer[5];
gyro_data.temp.xy = valid_data_buffer[6]; gyro_data.temp.xy = valid_data_buffer[6];
gyro_data.temp.z = valid_data_buffer[7]; gyro_data.temp.z = valid_data_buffer[7];
if(ahrs_algorithm == INSGPS_Algo) if(ahrs_algorithm == INSGPS_Algo)
{ {
/******************** INS ALGORITHM **************************/ /******************** 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]);
INSPrediction(gyro, accel, 1 / (float) EKF_RATE); // format data for INS algo
gyro[0] = gyro_data.filtered.x;
if ( gps_data.updated ) gyro[1] = gyro_data.filtered.y;
{ gyro[2] = gyro_data.filtered.z;
// Compute velocity from Heading and groundspeed accel[0] = accel_data.filtered.x,
vel[0] = gps_data.groundspeed * cos(gps_data.heading * M_PI / 180); accel[1] = accel_data.filtered.y,
vel[1] = gps_data.groundspeed * sin(gps_data.heading * M_PI / 180); accel[2] = accel_data.filtered.z,
vel[2] = 0;
// 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 #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, "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, "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, "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, "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 #endif
process_spi_request(); process_spi_request();
}
} return 0;
return 0;
} }
/** /**
@ -421,48 +425,48 @@ int main()
*/ */
void downsample_data() void downsample_data()
{ {
int16_t temp; int16_t temp;
int32_t accel_raw[3], gyro_raw[3]; int32_t accel_raw[3], gyro_raw[3];
uint16_t i; uint16_t i;
// Get the Y data. Third byte in. Convert to m/s // Get the Y data. Third byte in. Convert to m/s
accel_raw[0] = 0; accel_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) for( i = 0; i < ADC_OVERSAMPLE; i++ )
{ {
temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[1] ) * fir_coeffs[i]; temp = ( valid_data_buffer[0 + i * ADC_CONTINUOUS_CHANNELS] + accel_bias[1] ) * fir_coeffs[i];
accel_raw[0] += temp; accel_raw[0] += temp;
} }
accel_data.filtered.y = (float) accel_raw[0] / (float) fir_coeffs[ADC_OVERSAMPLE] * accel_scale[1]; 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 // Get the X data which projects forward/backwards. Fifth byte in. Convert to m/s
accel_raw[1] = 0; accel_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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_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]; 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 // Get the Z data. Third byte in. Convert to m/s
accel_raw[2] = 0; accel_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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_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]; 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. // Get the X gyro data. Seventh byte in. Convert to deg/s.
gyro_raw[0] = 0; gyro_raw[0] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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_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]; 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. // Get the Y gyro data. Second byte in. Convert to deg/s.
gyro_raw[1] = 0; gyro_raw[1] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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_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]; 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. // Get the Z gyro data. Fifth byte in. Convert to deg/s.
gyro_raw[2] = 0; gyro_raw[2] = 0;
for( i = 0; i < ADC_OVERSAMPLE; i++ ) 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_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]; 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. * aircraft is in stable state and then the data stored to SD card.
*/ */
void calibrate_sensors() { void calibrate_sensors() {
int i; int i;
int16_t mag_raw[3]; int16_t mag_raw[3];
// local biases for noise analysis // local biases for noise analysis
float accel_bias[3], gyro_bias[3], mag_bias[3]; 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];
ahrs_state = AHRS_IDLE; // run few loops to get mean
process_spi_request(); gyro_bias[0] = gyro_bias[1] = gyro_bias[2] = 0;
} accel_bias[0] = accel_bias[1] = accel_bias[2] = 0;
gyro_bias[0] /= i; mag_bias[0] = mag_bias[1] = mag_bias[2] = 0;
gyro_bias[1] /= i; for(i = 0; i < 50; i++) {
gyro_bias[2] /= i; while( ahrs_state != AHRS_DATA_READY );
accel_bias[0] /= i; ahrs_state = AHRS_PROCESSING;
accel_bias[1] /= i; downsample_data();
accel_bias[2] /= i; gyro_bias[0] += gyro_data.filtered.x;
mag_bias[0] /= i; gyro_bias[1] += gyro_data.filtered.y;
mag_bias[1] /= i; gyro_bias[2] += gyro_data.filtered.z;
mag_bias[2] /= i; accel_bias[0] += accel_data.filtered.x;
accel_bias[1] += accel_data.filtered.y;
// more iterations for variance accel_bias[2] += accel_data.filtered.z;
accel_var[0] = accel_var[1] = accel_var[2] = 0; PIOS_HMC5843_ReadMag(mag_raw);
gyro_var[0] = gyro_var[1] = gyro_var[2] = 0; mag_bias[0] += mag_raw[0];
mag_var[0] = mag_var[1] = mag_var[2] = 0; mag_bias[1] += mag_raw[1];
for(i = 0; i < 500; i++) mag_bias[2] += mag_raw[2];
{
while( ahrs_state != AHRS_DATA_READY ); ahrs_state = AHRS_IDLE;
ahrs_state = AHRS_PROCESSING; process_spi_request();
downsample_data(); }
gyro_var[0] += (gyro_data.filtered.x - gyro_bias[0]) * (gyro_data.filtered.x - gyro_bias[0]); gyro_bias[0] /= i;
gyro_var[1] += (gyro_data.filtered.y - gyro_bias[1]) * (gyro_data.filtered.y - gyro_bias[1]); gyro_bias[1] /= i;
gyro_var[2] += (gyro_data.filtered.z - gyro_bias[2]) * (gyro_data.filtered.z - gyro_bias[2]); gyro_bias[2] /= i;
accel_var[0] += (accel_data.filtered.x - accel_bias[0]) * (accel_data.filtered.x - accel_bias[0]); accel_bias[0] /= i;
accel_var[1] += (accel_data.filtered.y - accel_bias[1]) * (accel_data.filtered.y - accel_bias[1]); accel_bias[1] /= i;
accel_var[2] += (accel_data.filtered.z - accel_bias[2]) * (accel_data.filtered.z - accel_bias[2]); accel_bias[2] /= i;
PIOS_HMC5843_ReadMag(mag_raw); mag_bias[0] /= i;
mag_var[0] += (mag_raw[0] - mag_bias[0]) * (mag_raw[0] - mag_bias[0]); mag_bias[1] /= i;
mag_var[1] += (mag_raw[1] - mag_bias[1]) * (mag_raw[1] - mag_bias[1]); mag_bias[2] /= i;
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) // more iterations for variance
converge_insgps(); 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() void converge_insgps()
{ {
float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {0, 0, 0}; float pos[3] = {0,0,0}, vel[3] = {0,0,0}, BaroAlt = 0, mag[3], accel[3], temp_gyro[3] = {0, 0, 0};
INSGPSInit(); INSGPSInit();
INSSetAccelVar(accel_var); INSSetAccelVar(accel_var);
INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data INSSetGyroBias(temp_gyro); // set this to zero - crude bias corrected from downsample_data
INSSetGyroVar(gyro_var); INSSetGyroVar(gyro_var);
INSSetMagVar(mag_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) void process_spi_request(void)
{ {
bool msg_to_process = FALSE; bool msg_to_process = FALSE;
PIOS_IRQ_Disable(); PIOS_IRQ_Disable();
/* Figure out if we're in an interesting stable state */ /* Figure out if we're in an interesting stable state */
switch (lfsm_get_state()) { switch (lfsm_get_state()) {
case LFSM_STATE_USER_BUSY: case LFSM_STATE_USER_BUSY:
msg_to_process = TRUE; msg_to_process = TRUE;
break; break;
case LFSM_STATE_INACTIVE: case LFSM_STATE_INACTIVE:
/* Queue up a receive buffer */ /* 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_set_rx_v1 (&user_rx_v1);
lfsm_user_done (); 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) void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample)
{ {
int32_t i; int32_t i;
/* Setup analog pins */ /* Setup analog pins */
GPIO_InitTypeDef GPIO_InitStructure; GPIO_InitTypeDef GPIO_InitStructure;
GPIO_StructInit(&GPIO_InitStructure); GPIO_StructInit(&GPIO_InitStructure);
GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz; GPIO_InitStructure.GPIO_Speed = GPIO_Speed_2MHz;
GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN; GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AIN;
/* Enable each ADC pin in the array */ /* Enable each ADC pin in the array */
for(i = 0; i < PIOS_ADC_NUM_PINS; i++) { for(i = 0; i < PIOS_ADC_NUM_PINS; i++) {
GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i]; GPIO_InitStructure.GPIO_Pin = ADC_GPIO_PIN[i];
GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure); GPIO_Init(ADC_GPIO_PORT[i], &GPIO_InitStructure);
} }
/* Enable ADC clocks */ /* Enable ADC clocks */
PIOS_ADC_CLOCK_FUNCTION; PIOS_ADC_CLOCK_FUNCTION;
/* Map channels to conversion slots depending on the channel selection mask */ /* Map channels to conversion slots depending on the channel selection mask */
for(i = 0; i < PIOS_ADC_NUM_PINS; i++) { 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); ADC_RegularChannelConfig(ADC_MAPPING[i], ADC_CHANNEL[i], ADC_CHANNEL_MAPPING[i], PIOS_ADC_SAMPLE_TIME);
} }
#if (PIOS_ADC_USE_TEMP_SENSOR) #if (PIOS_ADC_USE_TEMP_SENSOR)
ADC_TempSensorVrefintCmd(ENABLE); ADC_TempSensorVrefintCmd(ENABLE);
ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_14, PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, PIOS_ADC_SAMPLE_TIME); ADC_RegularChannelConfig(PIOS_ADC_TEMP_SENSOR_ADC, ADC_Channel_14, PIOS_ADC_TEMP_SENSOR_ADC_CHANNEL, PIOS_ADC_SAMPLE_TIME);
#endif #endif
// TODO: update ADC to continuous sampling, configure the sampling rate // TODO: update ADC to continuous sampling, configure the sampling rate
/* Configure ADCs */ /* Configure ADCs */
ADC_InitTypeDef ADC_InitStructure; ADC_InitTypeDef ADC_InitStructure;
ADC_StructInit(&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_DataAlign = ADC_DataAlign_Right;
ADC_InitStructure.ADC_NbrOfChannel = 4; //((PIOS_ADC_NUM_CHANNELS + 1) >> 1); ADC_InitStructure.ADC_NbrOfChannel = 4; //((PIOS_ADC_NUM_CHANNELS + 1) >> 1);
ADC_Init(ADC1, &ADC_InitStructure); ADC_Init(ADC1, &ADC_InitStructure);
#if (PIOS_ADC_USE_ADC2) #if (PIOS_ADC_USE_ADC2)
ADC_Init(ADC2, &ADC_InitStructure); ADC_Init(ADC2, &ADC_InitStructure);
/* Enable ADC2 external trigger conversion (to synch with ADC1) */ /* Enable ADC2 external trigger conversion (to synch with ADC1) */
ADC_ExternalTrigConvCmd(ADC2, ENABLE); ADC_ExternalTrigConvCmd(ADC2, ENABLE);
#endif #endif
RCC_ADCCLKConfig(PIOS_ADC_ADCCLK); RCC_ADCCLKConfig(PIOS_ADC_ADCCLK);
RCC_PCLK2Config(RCC_HCLK_Div16); RCC_PCLK2Config(RCC_HCLK_Div16);
/* Enable ADC1->DMA request */ /* Enable ADC1->DMA request */
ADC_DMACmd(ADC1, ENABLE); ADC_DMACmd(ADC1, ENABLE);
/* ADC1 calibration */ /* ADC1 calibration */
ADC_Cmd(ADC1, ENABLE); ADC_Cmd(ADC1, ENABLE);
ADC_ResetCalibration(ADC1); ADC_ResetCalibration(ADC1);
while(ADC_GetResetCalibrationStatus(ADC1)); while(ADC_GetResetCalibrationStatus(ADC1));
ADC_StartCalibration(ADC1); ADC_StartCalibration(ADC1);
while(ADC_GetCalibrationStatus(ADC1)); while(ADC_GetCalibrationStatus(ADC1));
#if (PIOS_ADC_USE_ADC2) #if (PIOS_ADC_USE_ADC2)
/* ADC2 calibration */ /* ADC2 calibration */
ADC_Cmd(ADC2, ENABLE); ADC_Cmd(ADC2, ENABLE);
@ -912,10 +916,10 @@ void AHRS_ADC_Config(int32_t ekf_rate, int32_t adc_oversample)
ADC_StartCalibration(ADC2); ADC_StartCalibration(ADC2);
while(ADC_GetCalibrationStatus(ADC2)); while(ADC_GetCalibrationStatus(ADC2));
#endif #endif
/* Enable DMA1 clock */ /* Enable DMA1 clock */
RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE); RCC_AHBPeriphClockCmd(RCC_AHBPeriph_DMA1, ENABLE);
/* Configure DMA1 channel 1 to fetch data from ADC result register */ /* Configure DMA1 channel 1 to fetch data from ADC result register */
DMA_InitTypeDef DMA_InitStructure; DMA_InitTypeDef DMA_InitStructure;
DMA_StructInit(&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_PeripheralBaseAddr = (uint32_t)&ADC1->DR;
DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&raw_data_buffer[0]; DMA_InitStructure.DMA_MemoryBaseAddr = (uint32_t)&raw_data_buffer[0];
DMA_InitStructure.DMA_DIR = DMA_DIR_PeripheralSRC; 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_BufferSize = (ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE * 2) >> 1;
DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable; DMA_InitStructure.DMA_PeripheralInc = DMA_PeripheralInc_Disable;
DMA_InitStructure.DMA_MemoryInc = DMA_MemoryInc_Enable; 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_PeripheralDataSize = DMA_PeripheralDataSize_Word;
DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word; DMA_InitStructure.DMA_MemoryDataSize = DMA_MemoryDataSize_Word;
DMA_InitStructure.DMA_Mode = DMA_Mode_Circular; 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_InitStructure.DMA_M2M = DMA_M2M_Disable;
DMA_Init(DMA1_Channel1, &DMA_InitStructure); DMA_Init(DMA1_Channel1, &DMA_InitStructure);
DMA_Cmd(DMA1_Channel1, ENABLE); DMA_Cmd(DMA1_Channel1, ENABLE);
/* Trigger interrupt when for half conversions too to indicate double buffer */ /* 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_TC, ENABLE);
DMA_ITConfig(DMA1_Channel1, DMA_IT_HT, ENABLE); DMA_ITConfig(DMA1_Channel1, DMA_IT_HT, ENABLE);
/* Configure and enable DMA interrupt */ /* Configure and enable DMA interrupt */
NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitTypeDef NVIC_InitStructure;
NVIC_InitStructure.NVIC_IRQChannel = DMA1_Channel1_IRQn; 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_IRQChannelSubPriority = 0;
NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
NVIC_Init(&NVIC_InitStructure); NVIC_Init(&NVIC_InitStructure);
/* Finally start initial conversion */ /* Finally start initial conversion */
ADC_SoftwareStartConvCmd(ADC1, ENABLE); 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) void AHRS_ADC_DMA_Handler(void)
{ {
if ( ahrs_state == AHRS_IDLE ) if ( ahrs_state == AHRS_IDLE )
{ {
// Ideally this would have a mutex, but I think we can avoid it (and don't have RTOS features) // 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 if( DMA_GetFlagStatus( DMA1_IT_TC1 ) ) // whole double buffer filled
valid_data_buffer = &raw_data_buffer[ 1 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; valid_data_buffer = &raw_data_buffer[ 1 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ];
else if ( DMA_GetFlagStatus(DMA1_IT_HT1) ) else if ( DMA_GetFlagStatus(DMA1_IT_HT1) )
valid_data_buffer = &raw_data_buffer[ 0 * ADC_CONTINUOUS_CHANNELS * ADC_OVERSAMPLE ]; 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 { else {
// lets cause a seg fault and catch whatever is going on // Track how many times an interrupt occurred before EKF finished processing
valid_data_buffer = 0; ekf_too_slow++;
} }
ahrs_state = AHRS_DATA_READY; total_conversion_blocks++;
}
else { // Clear all interrupt from DMA 1 - regardless if buffer swapped
// Track how many times an interrupt occurred before EKF finished processing DMA_ClearFlag( DMA1_IT_GL1 );
ekf_too_slow++;
}
total_conversion_blocks++;
// Clear all interrupt from DMA 1 - regardless if buffer swapped
DMA_ClearFlag( DMA1_IT_GL1 );
} }

43
flight/AHRS/inc/insgps.h
View File

@ -27,28 +27,29 @@
#ifndef INSGPS_H_ #ifndef INSGPS_H_
#define INSGPS_H_ #define INSGPS_H_
// Exposed Function Prototypes // Exposed Function Prototypes
void INSGPSInit(); void INSGPSInit();
void INSPrediction(float gyro_data[3], float accel_data[3], float dT); void INSPrediction(float gyro_data[3], float accel_data[3], float dT);
void INSSetGyroBias(float gyro_bias[3]); void INSSetPosVelVar(float PosVar);
void INSSetAccelVar(float accel_var[3]); void INSSetGyroBias(float gyro_bias[3]);
void INSSetGyroVar(float gyro_var[3]); void INSSetAccelVar(float accel_var[3]);
void INSSetMagNorth(float B[3]); void INSSetGyroVar(float gyro_var[3]);
void INSSetMagVar(float scaled_mag_var[3]); void INSSetMagNorth(float B[3]);
void MagCorrection(float mag_data[3]); void INSSetMagVar(float scaled_mag_var[3]);
void FullCorrection(float mag_data[3], float Pos[3], float Vel[3], float BaroAlt); void MagCorrection(float mag_data[3]);
void GndSpeedAndMagCorrection(float Speed, float Heading, 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 // Nav structure containing current solution
struct NavStruct { struct NavStruct {
float Pos[3]; // Position in meters and relative to a local NED frame float Pos[3]; // Position in meters and relative to a local NED frame
float Vel[3]; // Velocity in meters and in NED float Vel[3]; // Velocity in meters and in NED
float q[4]; // unit quaternion rotation relative to NED float q[4]; // unit quaternion rotation relative to NED
} Nav; } Nav;
// constants // constants
#define MagSensors 0x1C0 #define MagSensors 0x1C0
#define FullSensors 0x3FF #define FullSensors 0x3FF
#define GndSpeedAndMagSensors 0x1D8 #define GndSpeedAndMagSensors 0x1D8
#endif /* EKF_H_ */ #endif /* EKF_H_ */

12
flight/AHRS/insgps.c
View File

@ -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[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[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[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[6]=R[7]=R[8]=0.005; // magnetometer unit vector noise variance
R[9]=1; // High freq altimeter noise variance (m^2) 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]) void INSSetGyroBias(float gyro_bias[3])
{ {
X[10] = gyro_bias[0]; X[10] = gyro_bias[0];

11
flight/OpenPilot/Modules/AHRSComms/ahrs_comms.c
View File

@ -407,20 +407,21 @@ static void load_gps_position(struct opahrs_msg_v1_req_update * update)
update->gps.updated = TRUE; update->gps.updated = TRUE;
if(home.Set == HOMELOCATION_SET_FALSE) { if(home.Set == HOMELOCATION_SET_FALSE || home.Indoor == HOMELOCATION_INDOOR_TRUE) {
PositionActualData pos; PositionActualData pos;
PositionActualGet(&pos); PositionActualGet(&pos);
update->gps.NED[0] = pos.NED[0]; update->gps.NED[0] = 0;
update->gps.NED[1] = pos.NED[1]; update->gps.NED[1] = 0;
update->gps.NED[2] = pos.NED[2]; update->gps.NED[2] = 0;
update->gps.groundspeed = 0; update->gps.groundspeed = 0;
update->gps.heading = 0; update->gps.heading = 0;
update->gps.quality = 0; update->gps.quality = 0;
} else { } else {
update->gps.groundspeed = data.Groundspeed; update->gps.groundspeed = data.Groundspeed;
update->gps.heading = data.Heading; 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)}; double LLA[3] = {(double) data.Latitude / 1e7, (double) data.Longitude / 1e7, (double) (data.GeoidSeparation + data.Altitude)};
// convert from cm back to meters // convert from cm back to meters
double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)}; double ECEF[3] = {(double) (home.ECEF[0] / 100), (double) (home.ECEF[1] / 100), (double) (home.ECEF[2] / 100)};

1
flight/OpenPilot/UAVObjects/homelocation.c
View File

@ -81,6 +81,7 @@ static void setDefaults(UAVObjHandle obj, uint16_t instId)
UAVObjGetInstanceData(obj, instId, &data); UAVObjGetInstanceData(obj, instId, &data);
memset(&data, 0, sizeof(HomeLocationData)); memset(&data, 0, sizeof(HomeLocationData));
data.Set = 0; data.Set = 0;
data.Indoor = 0;
data.Latitude = 0; data.Latitude = 0;
data.Longitude = 0; data.Longitude = 0;
data.Altitude = 0; data.Altitude = 0;

6
flight/OpenPilot/UAVObjects/inc/homelocation.h
View File

@ -41,7 +41,7 @@
#define HOMELOCATION_H #define HOMELOCATION_H
// Object constants // Object constants
#define HOMELOCATION_OBJID 3590360786U #define HOMELOCATION_OBJID 316455980U
#define HOMELOCATION_NAME "HomeLocation" #define HOMELOCATION_NAME "HomeLocation"
#define HOMELOCATION_METANAME "HomeLocationMeta" #define HOMELOCATION_METANAME "HomeLocationMeta"
#define HOMELOCATION_ISSINGLEINST 1 #define HOMELOCATION_ISSINGLEINST 1
@ -72,6 +72,7 @@
// Object data // Object data
typedef struct { typedef struct {
uint8_t Set; uint8_t Set;
uint8_t Indoor;
int32_t Latitude; int32_t Latitude;
int32_t Longitude; int32_t Longitude;
float Altitude; float Altitude;
@ -85,6 +86,9 @@ typedef struct {
// Field Set information // Field Set information
/* Enumeration options for field Set */ /* Enumeration options for field Set */
typedef enum { HOMELOCATION_SET_FALSE=0, HOMELOCATION_SET_TRUE=1 } HomeLocationSetOptions; 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 Latitude information
// Field Longitude information // Field Longitude information
// Field Altitude information // Field Altitude information

3
ground/src/plugins/config/configahrswidget.cpp
View File

@ -260,9 +260,12 @@ void ConfigAHRSWidget::calibPhase2()
// We need to echo back the results of calibration before changing to set mode // We need to echo back the results of calibration before changing to set mode
m_ahrs->calibInstructions->setText("Getting results..."); m_ahrs->calibInstructions->setText("Getting results...");
field->setValue("ECHO"); field->setValue("ECHO");
obj->updated(); obj->updated();
usleep(100e3); // wait for data to come back
// Now update size of all the graphs // Now update size of all the graphs
drawVariancesGraph(); drawVariancesGraph();

7
ground/src/plugins/uavobjects/homelocation.cpp
View File

@ -48,6 +48,12 @@ HomeLocation::HomeLocation(): UAVDataObject(OBJID, ISSINGLEINST, ISSETTINGS, NAM
SetEnumOptions.append("FALSE"); SetEnumOptions.append("FALSE");
SetEnumOptions.append("TRUE"); SetEnumOptions.append("TRUE");
fields.append( new UAVObjectField(QString("Set"), QString(""), UAVObjectField::ENUM, SetElemNames, SetEnumOptions) ); 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; QStringList LatitudeElemNames;
LatitudeElemNames.append("0"); LatitudeElemNames.append("0");
fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) ); fields.append( new UAVObjectField(QString("Latitude"), QString("deg * 10e6"), UAVObjectField::INT32, LatitudeElemNames, QStringList()) );
@ -112,6 +118,7 @@ UAVObject::Metadata HomeLocation::getDefaultMetadata()
void HomeLocation::setDefaultFieldValues() void HomeLocation::setDefaultFieldValues()
{ {
data.Set = 0; data.Set = 0;
data.Indoor = 0;
data.Latitude = 0; data.Latitude = 0;
data.Longitude = 0; data.Longitude = 0;
data.Altitude = 0; data.Altitude = 0;

6
ground/src/plugins/uavobjects/homelocation.h
View File

@ -44,6 +44,7 @@ public:
// Field structure // Field structure
typedef struct { typedef struct {
quint8 Set; quint8 Set;
quint8 Indoor;
qint32 Latitude; qint32 Latitude;
qint32 Longitude; qint32 Longitude;
float Altitude; float Altitude;
@ -57,6 +58,9 @@ public:
// Field Set information // Field Set information
/* Enumeration options for field Set */ /* Enumeration options for field Set */
typedef enum { SET_FALSE=0, SET_TRUE=1 } SetOptions; 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 Latitude information
// Field Longitude information // Field Longitude information
// Field Altitude information // Field Altitude information
@ -72,7 +76,7 @@ public:
// Constants // Constants
static const quint32 OBJID = 3590360786U; static const quint32 OBJID = 316455980U;
static const QString NAME; static const QString NAME;
static const bool ISSINGLEINST = 1; static const bool ISSINGLEINST = 1;
static const bool ISSETTINGS = 1; static const bool ISSETTINGS = 1;

14
ground/src/plugins/uavobjects/homelocation.py
View File

@ -49,6 +49,18 @@ _fields = [ \
'1' : 'TRUE', '1' : 'TRUE',
} }
), ),
uavobject.UAVObjectField(
'Indoor',
'b',
1,
[
'0',
],
{
'0' : 'FALSE',
'1' : 'TRUE',
}
),
uavobject.UAVObjectField( uavobject.UAVObjectField(
'Latitude', 'Latitude',
'i', 'i',
@ -126,7 +138,7 @@ _fields = [ \
class HomeLocation(uavobject.UAVObject): class HomeLocation(uavobject.UAVObject):
## Object constants ## Object constants
OBJID = 3590360786 OBJID = 316455980
NAME = "HomeLocation" NAME = "HomeLocation"
METANAME = "HomeLocationMeta" METANAME = "HomeLocationMeta"
ISSINGLEINST = 1 ISSINGLEINST = 1

1
ground/src/shared/uavobjectdefinition/homelocation.xml
View File

@ -2,6 +2,7 @@
<object name="HomeLocation" singleinstance="true" settings="true"> <object name="HomeLocation" singleinstance="true" settings="true">
<description>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.</description> <description>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.</description>
<field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/> <field name="Set" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="Indoor" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
<field name="Latitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/> <field name="Latitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Longitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/> <field name="Longitude" units="deg * 10e6" type="int32" elements="1" defaultvalue="0"/>
<field name="Altitude" units="m over geoid" type="float" elements="1" defaultvalue="0"/> <field name="Altitude" units="m over geoid" type="float" elements="1" defaultvalue="0"/>