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

INS Logging: Enable temperature reading from gyro and accel (may be removed in

Browse Source 
future), push it into UAVO and introduced a logging mode for INS that outputs
all sensor data from aux port.
This commit is contained in:
James Cotton 2011-09-03 15:10:47 -05:00
parent 5eaebac8d8
commit dae11cf877
9 changed files with 50 additions and 41 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
flight/INS/inc/ins.h
View File

@ -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

59
flight/INS/ins.c
View File

@ -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++;
}

4
flight/INS/pios_board.c
View File

@ -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,

5
flight/PiOS/STM32F2xx/pios_bma180.c
View File

@ -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);
}

15
flight/PiOS/STM32F2xx/pios_imu3000.c
View File

@ -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));

1
flight/PiOS/inc/pios_bma180.h
View File

@ -87,6 +87,7 @@ struct pios_bma180_data {
int16_t x;
int16_t y;
int16_t z;
int8_t temperature;
};

1
flight/PiOS/inc/pios_imu3000.h
View File

@ -119,6 +119,7 @@ struct pios_imu3000_data {
int16_t x;
int16_t y;
int16_t z;
int16_t temperature;
};
struct pios_imu3000_cfg {

2
shared/uavobjectdefinition/attituderaw.xml
View File

@ -3,7 +3,7 @@
<description>The raw attitude sensor data from @ref AHRSCommsModule. Not always updated.</description>
<field name="magnetometers" units="mGa" type="int16" elementnames="X,Y,Z"/>
<field name="gyros" units="deg/s" type="float" elementnames="X,Y,Z"/>
<field name="gyrotemp" units="raw" type="uint16" elementnames="XY,Z"/>
<field name="temperature" units="deg C" type="float" elementnames="gyro,accel"/>
<field name="accels" units="m/s^2" type="float" elementnames="X,Y,Z"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/inssettings.xml
View File

@ -1,7 +1,7 @@
<xml>
<object name="InsSettings" singleinstance="true" settings="true">
<description>Settings for the INS to control the algorithm and what is updated</description>
<field name="Algorithm" units="" type="enum" elements="1" options="NONE,SIMPLE,CALIBRATION,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="Algorithm" units="" type="enum" elements="1" options="NONE,SIMPLE,CALIBRATION,INSGPS_INDOOR_NOMAG,INSGPS_INDOOR,INSGPS_OUTDOOR,SENSORRAW" defaultvalue="INSGPS_INDOOR_NOMAG"/>
<field name="BiasCorrectedRaw" units="" type="enum" elements="1" options="TRUE,FALSE" defaultvalue="TRUE"/>
<!-- Sensor noises -->