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

IMU3000: Change IMU3000 api right now so it only reads one element from the

Browse Source 
fifo but swaps the endian appropriately.
This commit is contained in:
James Cotton 2011-08-18 11:03:56 -05:00
parent 11ac0707da
commit 294e0cbdff
3 changed files with 70 additions and 40 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

40
flight/INS/ins.c
View File

@ -171,15 +171,7 @@ int main()
PIOS_Board_Init();
PIOS_LED_Off(LED1);
PIOS_LED_On(LED2);
#if defined(PIOS_INCLUDE_HMC5883) && defined(PIOS_INCLUDE_I2C)
// Get 3 ID bytes
strcpy((char *)mag_data.id, "ZZZ");
PIOS_HMC5883_ReadID(mag_data.id);
#endif
PIOS_LED_Off(LED1);
PIOS_LED_Off(LED2);
// Sensors need a second to start
PIOS_DELAY_WaitmS(100);
@ -217,7 +209,6 @@ int main()
/* we didn't connect the callbacks before because we have to wait
for all data to be up to date before doing anything*/
AHRSCalibrationConnectCallback(calibration_callback);
AHRSSettingsConnectCallback(settings_callback);
HomeLocationConnectCallback(homelocation_callback);
@ -227,8 +218,6 @@ int main()
/******************* Main EKF loop ****************************/
while(1) {
AhrsPoll();
AhrsStatusData status;
AhrsStatusGet(&status);
@ -256,7 +245,7 @@ int main()
continue;
}
// print_ekf_binary();
//print_ekf_binary();
/* If algorithm changed reinit. This could go in callback but wouldn't be synchronous */
if (ahrs_algorithm != last_ahrs_algorithm)
@ -360,6 +349,8 @@ static void panic(uint32_t blinks)
*/
uint16_t accel_samples = 0;
uint8_t gyro_len;
int32_t gyro_accum[3];
int16_t gyro_fifo[4];
bool get_accel_gyro_data()
{
int16_t accel[3];
@ -379,15 +370,22 @@ bool get_accel_gyro_data()
accel[1] = accel_accum[1] / accel_samples;
accel[2] = accel_accum[2] / accel_samples;
int16_t gyro_fifo[4 * 100];
int32_t gyro_accum[3] = {0, 0, 0};
gyro_accum[0] = 0;
gyro_accum[1] = 0;
gyro_accum[2] = 0;
int16_t gyro[3];
gyro_len = PIOS_IMU3000_ReadFifo((uint8_t *) gyro_fifo, sizeof(gyro_fifo));
gyro_len /= 8;
for(int i = 0; i < gyro_len; i++) {
gyro_accum[0] += gyro_fifo[i * 4];
gyro_accum[1] += gyro_fifo[1 + i * 4];
gyro_accum[2] += gyro_fifo[2 + i * 4];
gyro_len = 0;
int32_t read_good;
// Make sure we get one sample
while((read_good = PIOS_IMU3000_ReadFifo(gyro_fifo)) != 0);
while(read_good == 0) {
gyro_len++;
gyro_accum[0] += gyro_fifo[0];
gyro_accum[1] += gyro_fifo[1];
gyro_accum[2] += gyro_fifo[2];
read_good = PIOS_IMU3000_ReadFifo(gyro_fifo);
}
gyro[0] = gyro_accum[0] / gyro_len;
gyro[1] = gyro_accum[1] / gyro_len;

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

@ -92,6 +92,11 @@ static void PIOS_IMU3000_Config(PIOS_IMU3000_ConfigTypeDef * IMU3000_Config_Stru
{
// TODO: Add checks against current config so we only update what has changed
// Reset chip and fifo
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, 0x01 | 0x02) != 0);
PIOS_DELAY_WaituS(20);
while (PIOS_IMU3000_Write(PIOS_IMU3000_USER_CTRL_REG, 0x00) != 0);
// FIFO storage
while (PIOS_IMU3000_Write(PIOS_IMU3000_FIFO_EN_REG, IMU3000_Config_Struct->Fifo_store) != 0);
@ -142,36 +147,62 @@ int32_t PIOS_IMU3000_ReadID()
}
/**
* @brief Reads the data from the IMU3000 FIFO
* \brief Reads the data from the IMU3000 FIFO
* \param[out] buffer destination buffer
* \param[in] len maximum number of bytes which should be read
* \note This returns the data as X, Y, Z the temperature
* \return number of bytes transferred if operation was successful
* \return -1 if error during I2C transfer
*/
uint32_t imu_read = 0;
int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len)
uint16_t fifo_level;
uint8_t fifo_level_data[2];
int16_t footer_flush;
uint8_t imu3000_read_buffer[10]; // Right now using temp,X,Y,Z,fifo
bool first_read = true;
int32_t PIOS_IMU3000_ReadFifo(int16_t * buffer)
{
uint16_t fifo_level;
int8_t retval1, retval2;
// Get the number of bytes in the fifo
retval1 = PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_CNT_MSB, (uint8_t *) &fifo_level, sizeof(fifo_level));
if(retval1 != 0)
if(PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_CNT_MSB, (uint8_t *) &fifo_level_data, sizeof(fifo_level_data)) != 0)
return -1;
if(len > fifo_level)
len = fifo_level;
len -= (len % 10); // only read chunks of 10 bytes (includes footer and temperature measurement)
// No bytes available to transfer
if(len == 0)
return 0;
fifo_level = (fifo_level_data[0] << 8) + fifo_level_data[1];
if(first_read) {
// Stupid system for IMU3000. If first read from buffer then we will read 4 sensors without fifo
// footer. After this we will read out a fifo footer
if(fifo_level < sizeof(imu3000_read_buffer))
return -1;
// Leave footer in buffer
if(PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer) - 2) != 0)
return -1;
imu_read += len;
buffer[0] = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3];
buffer[1] = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5];
buffer[2] = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7];
buffer[3] = imu3000_read_buffer[0] << 8 | imu3000_read_buffer[1];
first_read = false;
} else {
// Stupid system for IMU3000. Ensure something is left in buffer
if(fifo_level < (sizeof(imu3000_read_buffer) + 2))
return -1;
retval2 = PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_REG, (uint8_t *) &buffer, len);
return (retval2 < 0) ? -1 : len;
// Leave footer in buffer
if(PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer)) != 0)
return -1;
// First two bytes are left over fifo from last call
buffer[0] = imu3000_read_buffer[4] << 8 | imu3000_read_buffer[5];
buffer[1] = imu3000_read_buffer[6] << 8 | imu3000_read_buffer[7];
buffer[2] = imu3000_read_buffer[8] << 8 | imu3000_read_buffer[9];
buffer[3] = imu3000_read_buffer[2] << 8 | imu3000_read_buffer[3];
}
imu_read += sizeof(imu3000_read_buffer);
return 0;
}
/**

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

@ -116,6 +116,7 @@ struct pios_imu3000_data {
int16_t x;
int16_t y;
int16_t z;
int16_t temp;
};
struct pios_imu3000_cfg {
@ -127,7 +128,7 @@ struct pios_imu3000_cfg {
/* Public Functions */
extern void PIOS_IMU3000_Init(const struct pios_imu3000_cfg * cfg);
extern bool PIOS_IMU3000_NewDataAvailable(void);
extern int32_t PIOS_IMU3000_ReadFifo(uint8_t * buffer, uint16_t len);
extern int32_t PIOS_IMU3000_ReadFifo(int16_t * buffer);
extern int32_t PIOS_IMU3000_ReadGyros(int16_t * data);
extern int32_t PIOS_IMU3000_ReadID();
extern uint8_t PIOS_IMU3000_Test();