bool mpu6000SpiGyroDetect(const mpu6000Config_t *configToUse, gyro_t *gyro, uint16_t lpf)
{
mpu6000Config = configToUse;
if (!mpu6000SpiDetect()) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
if (lpf == 256)
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
else if (lpf >= 188)
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
else if (lpf >= 98)
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
else if (lpf >= 42)
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
else if (lpf >= 20)
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
else if (lpf >= 10)
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
else if (lpf > 0)
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
else
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Determine the new sample divider
mpu6000WriteRegister(MPU6000_SMPLRT_DIV, gyroMPU6xxxGetDividerDrops());
delayMicroseconds(1);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
gyro->intStatus = checkMPU6000DataReady;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}
void mpu6000SpiGyroInit(uint8_t lpf)
{
debug[3]++;
mpuIntExtiInit();
mpu6000AccAndGyroInit(lpf);
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_FAST_CLOCK); //high speed now that we don't need to write to the slow registers
int16_t data[3];
mpuGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
void mpu6000SpiGyroInit(uint8_t lpf)
{
mpuIntExtiInit();
mpu6000AccAndGyroInit();
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
spiSetDivisor(MPU6000_SPI_INSTANCE, LOW_SPEED_SPI);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, lpf);
delayMicroseconds(1);
int16_t data[3];
mpuGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
failureMode(FAILURE_GYRO_INIT_FAILED);
}
}
bool hmc5983DetectSpi(sensor_t *mag)
{
int16_t data[3];
int32_t xyz_total[3] = { 0, 0, 0 }; // 32 bit totals so they won't overflow.
spiResetErrorCounter();
uint8_t hmc5983Status = 0;
uint8_t i, calibrationSteps = 2;
setSPIdivisor( 8); // 4.5 MHz SPI clock
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(TS | SENSOR_CONFIG | POSITIVE_BIAS_CONFIGURATION);
DISABLE_HMC5983;
delay(50);
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_B);
spiTransferByte(SENSOR_INIT_GAIN);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
for (i = 0; i < 10; i++) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_SINGLE);
DISABLE_HMC5983;
delay(20);
while ((hmc5983Status && STATUS_RDY) == 0x00) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_STATUS_REG + 0x80);
hmc5983Status = spiTransferByte(0x00);
DISABLE_HMC5983;
}
if (!readHMC5983(data)) // check for valid data
break; // breaks out of the for loop if sensor saturated.
xyz_total[X] += data[X];
xyz_total[Y] += data[Y];
xyz_total[Z] += data[Z];
}
if (i == 10) // loop completed
calibrationSteps--;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(SENSOR_CONFIG | NEGATIVE_BIAS_CONFIGURATION);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
for (i = 0; i < 10; i++) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_SINGLE);
DISABLE_HMC5983;
delay(20);
while ((hmc5983Status && STATUS_RDY) == 0x00) {
ENABLE_HMC5983;
spiTransferByte(HMC5983_STATUS_REG + 0x80);
hmc5983Status = spiTransferByte(0x00);
DISABLE_HMC5983;
}
if (!readHMC5983(data))
break;
xyz_total[X] += data[X];
xyz_total[Y] += data[Y];
xyz_total[Z] += data[Z];
}
if (i == 10) // loop completed
calibrationSteps--;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_A);
spiTransferByte(TS | SENSOR_CONFIG | NORMAL_MEASUREMENT_CONFIGURATION);
DISABLE_HMC5983;
ENABLE_HMC5983;
spiTransferByte(HMC5983_CONFIG_REG_B);
spiTransferByte(SENSOR_GAIN);
DISABLE_HMC5983;
delay(50);
ENABLE_HMC5983;
spiTransferByte(HMC5983_MODE_REG);
spiTransferByte(OP_MODE_CONTINUOUS);
DISABLE_HMC5983;
delay(20);
readHMC5983(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter() != 0) {
spiResetErrorCounter();
return false;
}
// magGain[X] = fabsf(660.0f * HMC5983_X_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[X]);
// magGain[Y] = fabsf(660.0f * HMC5983_Y_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Y]);
// magGain[Z] = fabsf(660.0f * HMC5983_Z_SELF_TEST_GAUSS * 2.0f * 10.0f / xyz_total[Z]);
mag->init = hmc5983Init;
mag->read = readHMC5983;
return (calibrationSteps == 0);
}
bool mpu6000SpiGyroDetect(gyro_t *gyro, uint16_t lpf)
{
if (!mpu6000SpiDetect()) {
return false;
}
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
mpu6000AccAndGyroInit();
uint8_t mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
int16_t data[3];
// default lpf is 42Hz
switch (lpf) {
case 256:
mpuLowPassFilter = BITS_DLPF_CFG_256HZ;
break;
case 188:
mpuLowPassFilter = BITS_DLPF_CFG_188HZ;
break;
case 98:
mpuLowPassFilter = BITS_DLPF_CFG_98HZ;
break;
default:
case 42:
mpuLowPassFilter = BITS_DLPF_CFG_42HZ;
break;
case 20:
mpuLowPassFilter = BITS_DLPF_CFG_20HZ;
break;
case 10:
mpuLowPassFilter = BITS_DLPF_CFG_10HZ;
break;
case 5:
mpuLowPassFilter = BITS_DLPF_CFG_5HZ;
break;
case 0:
mpuLowPassFilter = BITS_DLPF_CFG_2100HZ_NOLPF;
break;
}
spiSetDivisor(MPU6000_SPI_INSTANCE, SPI_0_5625MHZ_CLOCK_DIVIDER);
// Accel and Gyro DLPF Setting
mpu6000WriteRegister(MPU6000_CONFIG, mpuLowPassFilter);
delayMicroseconds(1);
mpu6000SpiGyroRead(data);
if ((((int8_t)data[1]) == -1 && ((int8_t)data[0]) == -1) || spiGetErrorCounter(MPU6000_SPI_INSTANCE) != 0) {
spiResetErrorCounter(MPU6000_SPI_INSTANCE);
return false;
}
gyro->init = mpu6000SpiGyroInit;
gyro->read = mpu6000SpiGyroRead;
// 16.4 dps/lsb scalefactor
gyro->scale = 1.0f / 16.4f;
//gyro->scale = (4.0f / 16.4f) * (M_PIf / 180.0f) * 0.000001f;
delay(100);
return true;
}
