/**************************************************************************************************
Function:
GYRO_RESULT GyroInit(gyro_t *const gyro, const I2C_MODULE i2c, const UINT8 i2c_address,
const UINT8 dlpf_lpf, const UINT8 sample_rate_div, const UINT8 power_mgmt_sel)
Author(s):
mkobit
Summary:
Initializes the gyroscope
Description:
Typically will be called from the IMU code to set the sampling rate of the IMU and the resolution on the data
Preconditions:
I2C module previously enabled
Parameters:
gyro_t *const gyro - gyro to initialized
const I2C_MODULE i2c - I2C module to connect with
const UINT8 i2c_address - I2C address of gyroscope
const UINT8 dlpf_lpf - low pass filter configuration for sensor acquisition
GYRO_DLPF_LPF_256HZ - results in 8 kHz sample rate
GYRO_DLPF_LPF_188HZ - results in 1 kHz sample rate
GYRO_DLPF_LPF_98HZ - *
GYRO_DLPF_LPF_42HZ - *
GYRO_DLPF_LPF_20HZ - *
GYRO_DLPF_LPF_10HZ - *
GYRO_DLPF_LPF_5HZ - *
const UINT8 sample_rate_div - sample rate divider, F = F_internal / (sample_rate_div + 1)
E.g. -> 1kHz sample rate from dlpf_lpf, sample_rate_div = 9, F = 1 kHz / (9 _ 1) = 100 Hz
const UINT8 power_mgmt_sel - device clock selector
GYRO_PWR_MGM_CLK_SEL_INTERNAL - internal oscillator
GYRO_PWR_MGM_CLK_SEL_X - X as clock reference
GYRO_PWR_MGM_CLK_SEL_Y - Y as clock reference
GYRO_PWR_MGM_CLK_SEL_Z - Z as clock reference
Returns:
GYRO_SUCCESS - If successful
GYRO_FAIL - If any GyroWrite fails
Example:
<code>
GyroInit(I2C1, GYRO_DLPF_LPF_42HZ, 5, GYRO_PWR_MGM_CLK_SEL_X)
</code>
Conditions at Exit:
Gyro set to take samples at set data rate and the power management is also set. If either of these do not get set, GYRO_FAIL is returned
I2C bus is in idle state
**************************************************************************************************/
GYRO_RESULT GyroInit(gyro_t *const gyro, const I2C_MODULE i2c, const UINT8 i2c_address,
const UINT8 dlpf_lpf, const UINT8 sample_rate_div, const UINT8 power_mgmt_sel) {
// Set default polarities, offsets, and gains
GyroSetOffsets(gyro, 0, 0, 0);
GyroSetRevPolarity(gyro, FALSE, FALSE, FALSE);
GyroSetGains(gyro, 1.0f, 1.0f, 1.0f);
// Assign it the I2C Module and address
gyro->i2c = i2c;
gyro->i2c_addr = i2c_address;
// OR the low pass frequency passed with dflp_config with full scale operation and write it to the gyro
// Set internal clock and full scale operation
if (GyroWrite(gyro, GYRO_DLPF_FS, dlpf_lpf | GYRO_DLPF_FS_ON) == GYRO_FAIL) {
//printf("GyroInit: Error, could not write 0x%x to register GYRO_DLPF_FS 0x%x\n", (UINT8) dlpf_lpf | GYRO_DLPF_FS_ON, (UINT8) GYRO_DLPF_FS);
return GYRO_FAIL;
}
// Set sample rate divider
// If dlpf_lpf == GYRO_DLPF_LPF_256HZ, sample rate = 8 kHz / sample_rate_div
// Else, sample rate = 1 kHz / (sample_rate_div + 1)
if (GyroWrite(gyro, GYRO_SMPLRT_DIV, sample_rate_div) == GYRO_FAIL) {
//printf("GyroInit: Error, could not write 0x%x to register GYRO_DLPF_FS 0x%x\n", (UINT8) dlpf_lpf | GYRO_DLPF_FS_ON, (UINT8) GYRO_DLPF_FS);
return GYRO_FAIL;
}
// Select a gyro PLL for clock source (more stable)
if (GyroWrite(gyro, GYRO_PWR_MGM, power_mgmt_sel) == GYRO_FAIL) {
//printf("GyroInit: Error, could not write 0x%x to register GYRO_DLPF_FS 0x%x\n", (UINT8) dlpf_lpf | GYRO_DLPF_FS_ON, (UINT8) GYRO_DLPF_FS);
return GYRO_FAIL;
}
// Set interrupts - NOT BEING USED CURRENTLY
return GYRO_SUCCESS;
}
void GyroStartupWith(unsigned char ctrlreg1,unsigned char ctrlreg2,unsigned char ctrlreg3,unsigned char ctrlreg4,unsigned char ctrlreg5)
{
if ((!gyroPresent) && (ctrlreg1 & GYRO_POWER_ON)) // Ignore startup unless its powering the device off
{ return; }
// Stop interrupts during config
GYRO_INT2_IE = 0;
// Startup by setting ctrl regs
GyroOpen();
GyroAddressWrite(GYRO_CTRL_REG1 | GYRO_MASK_BURST);
GyroWrite(0); // GYRO_CTRL_REG1 - Device off first
GyroWrite(ctrlreg2); // GYRO_CTRL_REG2
GyroWrite(ctrlreg3); // GYRO_CTRL_REG3
GyroWrite(ctrlreg4); // GYRO_CTRL_REG4
GyroWrite(ctrlreg5); // GYRO_CTRL_REG5
GyroClose();
DelayMs(10);
switch (ctrlreg1 & 0xC0) {
case (GYRO_RATE_800) : {gyroFrequency = 800; break;}
case (GYRO_RATE_400) : {gyroFrequency = 400; break;}
case (GYRO_RATE_200) : {gyroFrequency = 200; break;}
case (GYRO_RATE_100) : {gyroFrequency = 100; break;}
default : {gyroFrequency = 0; break;}
}
switch (ctrlreg4 & 0x30) {
case (GYRO_250DPS) : {gyroRange = 250; break;}
case (GYRO_500DPS) : {gyroRange = 500; break;}
case (GYRO_2000DPS) : {gyroRange = 2000; break;}
default : {gyroRange = 0; break;}
}
// Fifo ints - clear fifo etc
if(ctrlreg3 == GYRO_FIFO_INT_ON)
{
// Setup watermark
GyroOpen();
GyroAddressWrite(GYRO_FIFO_CTRL_REG);
GyroWrite(0b01000000 | GYRO_FIFO_WATERMARK);// Stream to fifo mode - set water mark
GyroClose();
}
// else if (ctrlreg3 == ANY_OTHER_INTERRUPTS)
// Setup other specific regs here such as activity thresholds etc
// Now turn on the device
GyroOpen();
GyroAddressWrite(GYRO_CTRL_REG1);
GyroWrite(ctrlreg1); // GYRO_CTRL_REG1 - Device on
GyroClose();
if(ctrlreg3 == GYRO_FIFO_INT_ON)
{
// Empty fifo
DelayMs(50);
GyroReadFIFO(NULL, GYRO_MAX_FIFO_SAMPLES);
// Enable ints
GYRO_INT2_IP = GYRO_INT_PRIORITY;
GYRO_INT2_IF = 0;
GYRO_INT2_IE = 1;
}
}
