/**************************************************************************/
/*!
* @brief Gets the most recent sensor event
*/
/**************************************************************************/
public void getEvent(ref sensors_event_t Event)
{
bool readingValid = false;
Event.sensor_id = _sensorID;
Event.type = 4; // SENSOR_TYPE_GYROSCOPE;
while (!readingValid)
{
Event.timestamp = 0;
byte[] addr = new byte[] { (byte)((byte)gyroRegisters_t.GYRO_REGISTER_OUT_X_L | 0x80) };
byte[] data = new byte[6];
I2CDev.WriteRead(addr, data);
byte xlo = data[0];
byte xhi = data[1];
byte ylo = data[2];
byte yhi = data[3];
byte zlo = data[4];
byte zhi = data[5];
/* Shift values to create properly formed integer (low byte first) */
Event.gyro.x = (short)(xlo | (xhi << 8));
Event.gyro.y = (short)(ylo | (yhi << 8));
Event.gyro.z = (short)(zlo | (zhi << 8));
/* Make sure the sensor isn't saturating if auto-ranging is enabled */
if (!_autoRangeEnabled)
{
readingValid = true;
}
else
{
/* Check if the sensor is saturating or not */
if ((Event.gyro.x >= 32760) | (Event.gyro.x <= -32760) |
(Event.gyro.y >= 32760) | (Event.gyro.y <= -32760) |
(Event.gyro.z >= 32760) | (Event.gyro.z <= -32760))
{
/* Saturating .... increase the range if we can */
switch (_range)
{
case gyroRange_t.GYRO_RANGE_500DPS:
/* Push the range up to 2000dps */
_range = gyroRange_t.GYRO_RANGE_2000DPS;
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x00);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x0F);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG4, 0x20);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG5, 0x80);
readingValid = false;
// Serial.println("Changing range to 2000DPS");
break;
case gyroRange_t.GYRO_RANGE_250DPS:
/* Push the range up to 500dps */
_range = gyroRange_t.GYRO_RANGE_500DPS;
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x00);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x0F);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG4, 0x10);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG5, 0x80);
readingValid = false;
// Serial.println("Changing range to 500DPS");
break;
default:
readingValid = true;
break;
}
}
else
{
/* All values are withing range */
readingValid = true;
}
}
}
/* Compensate values depending on the resolution */
switch (_range)
{
case gyroRange_t.GYRO_RANGE_250DPS:
Event.gyro.x *= GYRO_SENSITIVITY_250DPS;
Event.gyro.y *= GYRO_SENSITIVITY_250DPS;
Event.gyro.z *= GYRO_SENSITIVITY_250DPS;
break;
case gyroRange_t.GYRO_RANGE_500DPS:
Event.gyro.x *= GYRO_SENSITIVITY_500DPS;
Event.gyro.y *= GYRO_SENSITIVITY_500DPS;
Event.gyro.z *= GYRO_SENSITIVITY_500DPS;
break;
case gyroRange_t.GYRO_RANGE_2000DPS:
Event.gyro.x *= GYRO_SENSITIVITY_2000DPS;
Event.gyro.y *= GYRO_SENSITIVITY_2000DPS;
Event.gyro.z *= GYRO_SENSITIVITY_2000DPS;
break;
}
/* Convert values to rad/s */
//Event.gyro.x *= SENSORS_DPS_TO_RADS;
//Event.gyro.y *= SENSORS_DPS_TO_RADS;
//Event.gyro.z *= SENSORS_DPS_TO_RADS;
}
/***************************************************************************
* PUBLIC FUNCTIONS
***************************************************************************/
/**************************************************************************/
/*!
* @brief Setups the HW
*/
/**************************************************************************/
public bool begin(gyroRange_t rng = gyroRange_t.GYRO_RANGE_250DPS)
{
/* Set the range the an appropriate value */
_range = rng;
/* Make sure we have the correct chip ID since this checks
* for correct address and that the IC is properly connected */
byte whoami = read8((byte)gyroRegisters_t.GYRO_REGISTER_WHO_AM_I);
if (whoami != L3GD20_ID)
{
return(false);
}
/* Set CTRL_REG1 (0x20)
* ====================================================================
* BIT Symbol Description Default
* --- ------ --------------------------------------------- -------
* 7-6 DR1/0 Output data rate 00
* 5-4 BW1/0 Bandwidth selection 00
* 3 PD 0 = Power-down mode, 1 = normal/sleep mode 0
* 2 ZEN Z-axis enable (0 = disabled, 1 = enabled) 1
* 1 YEN Y-axis enable (0 = disabled, 1 = enabled) 1
* 0 XEN X-axis enable (0 = disabled, 1 = enabled) 1 */
/* Reset then switch to normal mode and enable all three channels */
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x00);
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG1, 0x0F);
/* ------------------------------------------------------------------ */
/* Set CTRL_REG2 (0x21)
* ====================================================================
* BIT Symbol Description Default
* --- ------ --------------------------------------------- -------
* 5-4 HPM1/0 High-pass filter mode selection 00
* 3-0 HPCF3..0 High-pass filter cutoff frequency selection 0000 */
/* Nothing to do ... keep default values */
/* ------------------------------------------------------------------ */
/* Set CTRL_REG3 (0x22)
* ====================================================================
* BIT Symbol Description Default
* --- ------ --------------------------------------------- -------
* 7 I1_Int1 Interrupt enable on INT1 (0=disable,1=enable) 0
* 6 I1_Boot Boot status on INT1 (0=disable,1=enable) 0
* 5 H-Lactive Interrupt active config on INT1 (0=high,1=low) 0
* 4 PP_OD Push-Pull/Open-Drain (0=PP, 1=OD) 0
* 3 I2_DRDY Data ready on DRDY/INT2 (0=disable,1=enable) 0
* 2 I2_WTM FIFO wtrmrk int on DRDY/INT2 (0=dsbl,1=enbl) 0
* 1 I2_ORun FIFO overrun int on DRDY/INT2 (0=dsbl,1=enbl) 0
* 0 I2_Empty FIFI empty int on DRDY/INT2 (0=dsbl,1=enbl) 0 */
/* Nothing to do ... keep default values */
/* ------------------------------------------------------------------ */
/* Set CTRL_REG4 (0x23)
* ====================================================================
* BIT Symbol Description Default
* --- ------ --------------------------------------------- -------
* 7 BDU Block Data Update (0=continuous, 1=LSB/MSB) 0
* 6 BLE Big/Little-Endian (0=Data LSB, 1=Data MSB) 0
* 5-4 FS1/0 Full scale selection 00
* 00 = 250 dps
* 01 = 500 dps
* 10 = 2000 dps
* 11 = 2000 dps
* 0 SIM SPI Mode (0=4-wire, 1=3-wire) 0 */
/* Adjust resolution if requested */
switch (_range)
{
case gyroRange_t.GYRO_RANGE_250DPS:
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG4, 0x00);
break;
case gyroRange_t.GYRO_RANGE_500DPS:
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG4, 0x10);
break;
case gyroRange_t.GYRO_RANGE_2000DPS:
write8((byte)gyroRegisters_t.GYRO_REGISTER_CTRL_REG4, 0x20);
break;
}
/* ------------------------------------------------------------------ */
/* Set CTRL_REG5 (0x24)
* ====================================================================
* BIT Symbol Description Default
* --- ------ --------------------------------------------- -------
* 7 BOOT Reboot memory content (0=normal, 1=reboot) 0
* 6 FIFO_EN FIFO enable (0=FIFO disable, 1=enable) 0
* 4 HPen High-pass filter enable (0=disable,1=enable) 0
* 3-2 INT1_SEL INT1 Selection config 00
* 1-0 OUT_SEL Out selection config 00 */
/* Nothing to do ... keep default values */
/* ------------------------------------------------------------------ */
return(true);
}
