From dae11cf877e99e0d68681934c6ca8de17d62076e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Sat, 3 Sep 2011 15:10:47 -0500 Subject: [PATCH] INS Logging: Enable temperature reading from gyro and accel (may be removed in future), push it into UAVO and introduced a logging mode for INS that outputs all sensor data from aux port. --- flight/INS/inc/ins.h | 2 + flight/INS/ins.c | 59 ++++++++++++---------- flight/INS/pios_board.c | 4 +- flight/PiOS/STM32F2xx/pios_bma180.c | 5 +- flight/PiOS/STM32F2xx/pios_imu3000.c | 15 +++--- flight/PiOS/inc/pios_bma180.h | 1 + flight/PiOS/inc/pios_imu3000.h | 1 + shared/uavobjectdefinition/attituderaw.xml | 2 +- shared/uavobjectdefinition/inssettings.xml | 2 +- 9 files changed, 50 insertions(+), 41 deletions(-) diff --git a/flight/INS/inc/ins.h b/flight/INS/inc/ins.h index d978defa5..4dcf03601 100644 --- a/flight/INS/inc/ins.h +++ b/flight/INS/inc/ins.h @@ -66,6 +66,7 @@ struct accel_sensor { float scale[3]; float variance[3]; } calibration; + float temperature; }; //! Contains the data from the gyro @@ -80,6 +81,7 @@ struct gyro_sensor { float scale[3]; float variance[3]; } calibration; + float temperature; }; //! Conains the current estimate of the attitude diff --git a/flight/INS/ins.c b/flight/INS/ins.c index c6ace4e0e..ed4fc17d6 100644 --- a/flight/INS/ins.c +++ b/flight/INS/ins.c @@ -39,6 +39,7 @@ BMA180 interrupt */ +#define TYPICAL_PERIOD 3300 #define timer_rate() 100000 #define timer_count() 1 /* OpenPilot Includes */ @@ -86,6 +87,7 @@ void calibration(float result[3], float scale[3][4], float arg[3]); extern void PIOS_Board_Init(void); static void panic(uint32_t blinks); +static void print_ekf_binary(); void simple_update(); /* Bootloader related functions and var*/ @@ -255,6 +257,11 @@ int main() case INSSETTINGS_ALGORITHM_CALIBRATION: measure_noise(); break; + case INSSETTINGS_ALGORITHM_SENSORRAW: + print_ekf_binary(); + // Run at standard rate + while(PIOS_DELAY_DiffuS(time_val1) < TYPICAL_PERIOD); + break; } status.RunningTimePerCycle = PIOS_DELAY_DiffuS(time_val2); @@ -289,36 +296,24 @@ void simple_update() { /** * @brief Output all the important inputs and states of the ekf through serial port */ -#ifdef DUMP_EKF - -extern float **P, *X; // covariance matrix and state vector - -void print_ekf_binary() +static void print_ekf_binary() { - uint16_t states = ins_get_num_states(); - uint8_t framing[16] = { 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 0 }; + uint8_t framing[2] = { 0xff, 0x00 }; // Dump raw buffer - PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], 16); // framing header (1:16) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); // dump block number (17:20) - - PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); // accel data (21:32) - PIOS_COM_SendBufferNonBlocking(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); // gyro data (33:44) - - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); // mag update (45) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); // mag data (46:57) - - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gps_data, sizeof(gps_data)); // gps data (58:85) - - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & X, 4 * states); // X (86:149) - for(uint8_t i = 0; i < states; i++) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &((*P)[i + i * states]), 4); // diag(P) (150:213) - - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); // BaroAlt (214:217) - PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & baro_offset, 4); // baro_offset (218:221) + PIOS_COM_SendBuffer(PIOS_COM_AUX, &framing[0], sizeof(framing)); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & total_conversion_blocks, sizeof(total_conversion_blocks)); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & accel_data.filtered.x, 4*3); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & gyro_data.filtered.x, 4*3); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.updated, 1); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & mag_data.scaled.axis, 3*4); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.updated, 1); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) & altitude_data.altitude, 4); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &gyro_data.temperature, 4); + PIOS_COM_SendBuffer(PIOS_COM_AUX, (uint8_t *) &accel_data.temperature, 4); + + mag_data.updated = 0; + altitude_data.updated = 0; } -#else -void print_ekf_binary() {} -#endif static void panic(uint32_t blinks) { @@ -410,6 +405,11 @@ void get_accel_gyro_data() raw.gyros[1] = gyro_data.filtered.y * RAD_TO_DEG; raw.gyros[2] = gyro_data.filtered.z * RAD_TO_DEG; + // From data sheet 35 deg C corresponds to -13200, and 280 LSB per C + gyro_data.temperature = 35.0f + ((float) gyro.temperature + 13200) / 280; + // From the data sheet 25 deg C corresponds to 2 and 2 LSB per C + accel_data.temperature = 25.0f + ((float) accel.temperature - 2) / 2; + if (bias_corrected_raw) { raw.gyros[0] -= Nav.gyro_bias[0] * RAD_TO_DEG; @@ -421,6 +421,9 @@ void get_accel_gyro_data() raw.accels[2] -= Nav.accel_bias[2]; } + raw.temperature[ATTITUDERAW_TEMPERATURE_GYRO] = gyro_data.temperature; + raw.temperature[ATTITUDERAW_TEMPERATURE_ACCEL] = accel_data.temperature; + raw.magnetometers[0] = mag_data.scaled.axis[0]; raw.magnetometers[1] = mag_data.scaled.axis[1]; raw.magnetometers[2] = mag_data.scaled.axis[2]; @@ -557,7 +560,7 @@ void measure_noise() InsSettingsSet(&settings); settings_callback(InsSettingsHandle()); } else { - PIOS_DELAY_WaituS(3300); + PIOS_DELAY_WaituS(TYPICAL_PERIOD); calibrate_count++; } diff --git a/flight/INS/pios_board.c b/flight/INS/pios_board.c index bc2a342ba..2a3e9636d 100644 --- a/flight/INS/pios_board.c +++ b/flight/INS/pios_board.c @@ -344,7 +344,7 @@ static const struct pios_usart_cfg pios_usart_aux_cfg = { .regs = UART4, .remap = GPIO_AF_UART4, .init = { - .USART_BaudRate = 57600, + .USART_BaudRate = 230400, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, @@ -657,7 +657,7 @@ static const struct pios_imu3000_cfg pios_imu3000_cfg = { .NVIC_IRQChannelCmd = ENABLE, }, }, - .Fifo_store = PIOS_IMU3000_FIFO_GYRO_X_OUT | PIOS_IMU3000_FIFO_GYRO_Y_OUT + .Fifo_store = PIOS_IMU3000_FIFO_TEMP_OUT | PIOS_IMU3000_FIFO_GYRO_X_OUT | PIOS_IMU3000_FIFO_GYRO_Y_OUT | PIOS_IMU3000_FIFO_GYRO_Z_OUT | PIOS_IMU3000_FIFO_FOOTER, // Clock at 8 khz, downsampled by 4 for 2khz .Smpl_rate_div = 3, diff --git a/flight/PiOS/STM32F2xx/pios_bma180.c b/flight/PiOS/STM32F2xx/pios_bma180.c index 357f9f93e..411003dc1 100644 --- a/flight/PiOS/STM32F2xx/pios_bma180.c +++ b/flight/PiOS/STM32F2xx/pios_bma180.c @@ -351,7 +351,7 @@ int32_t PIOS_BMA180_Test() return 0; } -static uint8_t pios_bma180_dmabuf[7]; +static uint8_t pios_bma180_dmabuf[8]; static void PIOS_BMA180_SPI_Callback() { // TODO: Make this conversion depend on configuration scale @@ -372,6 +372,7 @@ static void PIOS_BMA180_SPI_Callback() data.x /= 4; data.y /= 4; data.z /= 4; + data.temperature = pios_bma180_dmabuf[7]; fifoBuf_putData(&pios_bma180_fifo, (uint8_t *) &data, sizeof(data)); } @@ -385,7 +386,7 @@ static void PIOS_BMA180_IRQHandler(void) // If we can't get the bus then just move on for efficiency if(PIOS_BMA180_ClaimBus() == 0) PIOS_SPI_TransferBlock(PIOS_SPI_ACCEL,pios_bma180_req_buf,(uint8_t *) pios_bma180_dmabuf, - 7, PIOS_BMA180_SPI_Callback); + sizeof(pios_bma180_dmabuf), PIOS_BMA180_SPI_Callback); } diff --git a/flight/PiOS/STM32F2xx/pios_imu3000.c b/flight/PiOS/STM32F2xx/pios_imu3000.c index fb624c9ae..2ee21b12e 100644 --- a/flight/PiOS/STM32F2xx/pios_imu3000.c +++ b/flight/PiOS/STM32F2xx/pios_imu3000.c @@ -301,17 +301,18 @@ static void imu3000_callback() goto out; if(imu3000_first_read) { - data.x = imu3000_read_buffer[0] << 8 | imu3000_read_buffer[1]; - data.y = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3]; - data.z = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5]; + data.temperature = imu3000_read_buffer[0] << 8 | imu3000_read_buffer[1]; + data.x = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3]; + data.y = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5]; + data.z = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7]; imu3000_first_read = false; } else { // First two bytes are left over fifo from last call - data.x = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3]; - data.y = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5]; - data.z = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7]; - + data.temperature = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3]; + data.x = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5]; + data.y = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7]; + data.z = imu3000_read_buffer[8] << 8 | imu3000_read_buffer[9]; } fifoBuf_putData(&pios_imu3000_fifo, (uint8_t *) &data, sizeof(data)); diff --git a/flight/PiOS/inc/pios_bma180.h b/flight/PiOS/inc/pios_bma180.h index f19e3d1c8..52dfe0962 100644 --- a/flight/PiOS/inc/pios_bma180.h +++ b/flight/PiOS/inc/pios_bma180.h @@ -87,6 +87,7 @@ struct pios_bma180_data { int16_t x; int16_t y; int16_t z; + int8_t temperature; }; diff --git a/flight/PiOS/inc/pios_imu3000.h b/flight/PiOS/inc/pios_imu3000.h index 72b39eef1..c1a3f7d86 100644 --- a/flight/PiOS/inc/pios_imu3000.h +++ b/flight/PiOS/inc/pios_imu3000.h @@ -119,6 +119,7 @@ struct pios_imu3000_data { int16_t x; int16_t y; int16_t z; + int16_t temperature; }; struct pios_imu3000_cfg { diff --git a/shared/uavobjectdefinition/attituderaw.xml b/shared/uavobjectdefinition/attituderaw.xml index 8a2a69ab7..b3d11229c 100644 --- a/shared/uavobjectdefinition/attituderaw.xml +++ b/shared/uavobjectdefinition/attituderaw.xml @@ -3,7 +3,7 @@ The raw attitude sensor data from @ref AHRSCommsModule. Not always updated. - + diff --git a/shared/uavobjectdefinition/inssettings.xml b/shared/uavobjectdefinition/inssettings.xml index 340ecf9a1..f40c91b79 100644 --- a/shared/uavobjectdefinition/inssettings.xml +++ b/shared/uavobjectdefinition/inssettings.xml @@ -1,7 +1,7 @@ Settings for the INS to control the algorithm and what is updated - +