uint8_t hmc5983Init(void) { int i = HMC5983_RETRIES; switch ((int)p[IMU_FLIP]) { case 1: hmc5983Data.magSign[0] = 1.0f; hmc5983Data.magSign[1] = -1.0f; hmc5983Data.magSign[2] = -1.0f; break; case 2: hmc5983Data.magSign[0] = -1.0f; hmc5983Data.magSign[1] = 1.0f; hmc5983Data.magSign[2] = -1.0f; break; case 0: default: hmc5983Data.magSign[0] = 1.0f; hmc5983Data.magSign[1] = 1.0f; hmc5983Data.magSign[2] = 1.0f; break; } // wait for a valid response while (--i && hmc5983GetReg(0x0a) != 'H') delay(100); if (i > 0) { // 75Hz, 8x oversample hmc5983ReliablySetReg(0x00, 0b11111000); delay(10); // // highest gain (+-0.88 Ga) // hmc5983ReliablySetReg(0x01, 0b00000000); // gain (+-2.5 Ga) hmc5983ReliablySetReg(0x01, 0b01100000); delay(10); hmc5983ReliablySetReg(0x02, 0b00000000); delay(10); hmc5983Data.readCmd = HMC5983_READ_MULT_BIT | 0x03; spiChangeCallback(hmc5983Data.spi, hmc5983TransferComplete); // External Interrupt line for data ready extRegisterCallback(DIMU_HMC5983_INT_PORT, DIMU_HMC5983_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, hmc5983IntHandler); hmc5983Data.initialized = 1; } else { hmc5983Data.initialized = 0; } return hmc5983Data.initialized; }
void max21100Init(void) { switch ((int)p[IMU_FLIP]) { case 1: max21100Data.accSign[0] = 1.0f; max21100Data.accSign[1] = -1.0f; max21100Data.accSign[2] = -1.0f; max21100Data.gyoSign[0] = 1.0f; max21100Data.gyoSign[1] = -1.0f; max21100Data.gyoSign[2] = -1.0f; break; case 2: max21100Data.accSign[0] = -1.0f; max21100Data.accSign[1] = 1.0f; max21100Data.accSign[2] = -1.0f; max21100Data.gyoSign[0] = -1.0f; max21100Data.gyoSign[1] = 1.0f; max21100Data.gyoSign[2] = -1.0f; break; case 0: default: max21100Data.accSign[0] = 1.0f; max21100Data.accSign[1] = 1.0f; max21100Data.accSign[2] = 1.0f; max21100Data.gyoSign[0] = 1.0f; max21100Data.gyoSign[1] = 1.0f; max21100Data.gyoSign[2] = 1.0f; break; } utilFilterInit(&max21100Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP); max21100ReliablySetReg(0x22, 0x00); // BNK 00 // turn off I2C max21100ReliablySetReg(0x16, 0x01); // check device ID while (max21100GetReg(0x20) != 0xb2) ; // power up max21100ReliablySetReg(0x00, 0b01111111); // Normal Mode GYO+ACC // GYO scale @ 2KHz #if MAX21100_GYO_SCALE == 250 max21100ReliablySetReg(0x01, 0b00100011); #endif #if MAX21100_GYO_SCALE == 500 max21100ReliablySetReg(0x01, 0b00100010); #endif #if MAX21100_GYO_SCALE == 1000 max21100ReliablySetReg(0x01, 0b00100001); #endif #if MAX21100_GYO_SCALE == 2000 max21100ReliablySetReg(0x01, 0b00100000); #endif max21100ReliablySetReg(0x02, 0b00100000); // GYO 8KHz ODR // ACC scale #if MAX21100_ACC_SCALE == 2 max21100ReliablySetReg(0x04, 0b11000111); #endif #if MAX21100_ACC_SCALE == 4 max21100ReliablySetReg(0x04, 0b10000111); #endif #if MAX21100_ACC_SCALE == 8 max21100ReliablySetReg(0x04, 0b01000111); #endif #if MAX21100_ACC_SCALE == 16 max21100ReliablySetReg(0x04, 0b00000111); #endif max21100ReliablySetReg(0x05, 0b00110000); // ACC 2KHz ODR max21100SetReg(0x22, 0x01); // BNK 01 max21100ReliablySetReg(0x0b, 0b00100000); // IRQ config max21100ReliablySetReg(0x0c, 0b00000000); // IRQ config max21100SetReg(0x0d, 0b10000000); // IRQ config max21100Data.readReg = MAX21100_READ_BIT | 0x24; // start of sensor registers // External Interrupt line for data ready extRegisterCallback(DIMU_MAX21100_INT_PORT, DIMU_MAX21100_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, max21100IntHandler); spiChangeCallback(max21100Data.spi, max21100TransferComplete); }
void mpu6000Init(void) { switch ((int)p[IMU_FLIP]) { case 1: mpu6000Data.accSign[0] = 1.0f; mpu6000Data.accSign[1] = -1.0f; mpu6000Data.accSign[2] = -1.0f; mpu6000Data.gyoSign[0] = 1.0f; mpu6000Data.gyoSign[1] = -1.0f; mpu6000Data.gyoSign[2] = -1.0f; break; case 2: mpu6000Data.accSign[0] = -1.0f; mpu6000Data.accSign[1] = 1.0f; mpu6000Data.accSign[2] = -1.0f; mpu6000Data.gyoSign[0] = -1.0f; mpu6000Data.gyoSign[1] = 1.0f; mpu6000Data.gyoSign[2] = -1.0f; break; case 0: default: mpu6000Data.accSign[0] = 1.0f; mpu6000Data.accSign[1] = 1.0f; mpu6000Data.accSign[2] = 1.0f; mpu6000Data.gyoSign[0] = 1.0f; mpu6000Data.gyoSign[1] = 1.0f; mpu6000Data.gyoSign[2] = 1.0f; break; } utilFilterInit(&mpu6000Data.tempFilter, DIMU_OUTER_DT, DIMU_TEMP_TAU, IMU_ROOM_TEMP); // reset mpu6000SetReg(107, 0b10000000); delay(100); // wake up w/ Z axis clock reg mpu6000ReliablySetReg(107, 0x03); // disable I2C interface mpu6000ReliablySetReg(106, 0b010000); // wait for a valid response while (mpu6000GetReg(117) != 0x68) delay(10); // GYO scale #if MPU6000_GYO_SCALE == 250 mpu6000ReliablySetReg(27, 0b00000); #endif #if MPU6000_GYO_SCALE == 500 mpu6000ReliablySetReg(27, 0b01000); #endif #if MPU6000_GYO_SCALE == 1000 mpu6000ReliablySetReg(27, 0b10000); #endif #if MPU6000_GYO_SCALE == 2000 mpu6000ReliablySetReg(27, 0b11000); #endif // ACC scale #if MPU6000_ACC_SCALE == 2 mpu6000ReliablySetReg(28, 0b00000); #endif #if MPU6000_ACC_SCALE == 4 mpu6000ReliablySetReg(28, 0b01000); #endif #if MPU6000_ACC_SCALE == 8 mpu6000ReliablySetReg(28, 0b10000); #endif #if MPU6000_ACC_SCALE == 16 mpu6000ReliablySetReg(28, 0b11000); #endif // Sample rate mpu6000ReliablySetReg(25, 0x00); // LPF mpu6000ReliablySetReg(26, 0x00); // Interrupt setup mpu6000ReliablySetReg(55, 0x01<<4); mpu6000ReliablySetReg(56, 0x01); // bump clock rate up to 21MHz spiChangeBaud(mpu6000Data.spi, MPU6000_SPI_RUN_BAUD); mpu6000Data.readReg = MPU6000_READ_BIT | 0x3b; // start of sensor registers spiChangeCallback(mpu6000Data.spi, mpu6000TransferComplete); // External Interrupt line for data ready extRegisterCallback(DIMU_MPU6000_INT_PORT, DIMU_MPU6000_INT_PIN, EXTI_Trigger_Rising, 1, GPIO_PuPd_NOPULL, mpu6000IntHandler); }