mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
IMU3000: Fix a constant that made the sampling rate 1kh instead of 8 internaly.
No downsample that (in IMU3000) by 8 to get gyros at 1khz. Well about 256 Hz nyquist and shouldn't cause substantial issues for EKF.
This commit is contained in:
parent
bdb9dc0a54
commit
c7034ef8a6
@ -74,9 +74,9 @@ void PIOS_IMU3000_Init(const struct pios_imu3000_cfg * cfg)
|
||||
PIOS_IMU3000_ConfigTypeDef IMU3000_InitStructure;
|
||||
IMU3000_InitStructure.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;
|
||||
IMU3000_InitStructure.Smpl_rate_div = 8;
|
||||
IMU3000_InitStructure.Smpl_rate_div = 7; // Clock at 8 khz, downsampled by 8 for 1khz
|
||||
IMU3000_InitStructure.DigLPF_Scale = PIOS_IMU3000_LOWPASS_256_HZ | PIOS_IMU3000_SCALE_500_DEG;
|
||||
IMU3000_InitStructure.Interrupt_cfg = PIOS_IMU3000_INT_CLR_ANYRD | PIOS_IMU3000_INT_DATA_RDY;
|
||||
IMU3000_InitStructure.Interrupt_cfg = 0; //PIOS_IMU3000_INT_CLR_ANYRD | PIOS_IMU3000_INT_DATA_RDY;
|
||||
IMU3000_InitStructure.User_ctl = PIOS_IMU3000_USERCTL_FIFO_EN;
|
||||
IMU3000_InitStructure.Pwr_mgmt_clk = PIOS_IMU3000_PWRMGMT_PLL_X_CLK;
|
||||
PIOS_IMU3000_Config(&IMU3000_InitStructure);
|
||||
@ -160,6 +160,7 @@ 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;
|
||||
uint32_t imu3000_readtime;
|
||||
int32_t PIOS_IMU3000_ReadFifo(int16_t * buffer)
|
||||
{
|
||||
// Get the number of bytes in the fifo
|
||||
@ -189,10 +190,14 @@ int32_t PIOS_IMU3000_ReadFifo(int16_t * buffer)
|
||||
if(fifo_level < (sizeof(imu3000_read_buffer) + 2))
|
||||
return -1;
|
||||
|
||||
uint32_t timeval = PIOS_DELAY_GetRaw();
|
||||
|
||||
// Leave footer in buffer
|
||||
if(PIOS_IMU3000_Read(PIOS_IMU3000_FIFO_REG, imu3000_read_buffer, sizeof(imu3000_read_buffer)) != 0)
|
||||
return -1;
|
||||
|
||||
imu3000_readtime = PIOS_DELAY_DiffuS(timeval);
|
||||
|
||||
// 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];
|
||||
|
@ -72,10 +72,10 @@
|
||||
#define PIOS_IMU3000_FIFO_FOOTER 0x01
|
||||
|
||||
/* Gyro full-scale range */
|
||||
#define PIOS_IMU3000_SCALE_250_DEG 0x00
|
||||
#define PIOS_IMU3000_SCALE_500_DEG 0x01
|
||||
#define PIOS_IMU3000_SCALE_1000_DEG 0x02
|
||||
#define PIOS_IMU3000_SCALE_2000_DEG 0x03
|
||||
#define PIOS_IMU3000_SCALE_250_DEG (0x00 << 3)
|
||||
#define PIOS_IMU3000_SCALE_500_DEG (0x01 << 3)
|
||||
#define PIOS_IMU3000_SCALE_1000_DEG (0x02 << 3)
|
||||
#define PIOS_IMU3000_SCALE_2000_DEG (0x03 << 3)
|
||||
|
||||
/* Digital low-pass filter configuration */
|
||||
#define PIOS_IMU3000_LOWPASS_256_HZ 0x00
|
||||
|
Loading…
x
Reference in New Issue
Block a user