void hmc5883Setup(void)
{
hmc5883_Init(0); // Don't set mode yet, we'll do that later on.
delay_ms(50);
HMC58X3_setMode(0);
delay_ms(50);
HMC58X3_setDOR(6); //75hz ¸
delay_ms(50);
HMC58X3_FIFO_init();
delay_ms(50);
}
gyHandle gy86_Init (Gy86_ChipType chip) {
ENTER();
retcode retVal = 0;
Gy86 *gy = (Gy86 *)malloc(sizeof(Gy86));
if (!gy) {
ALLOC_FAIL("gy");
goto END;
}
memset(gy, 0x00, sizeof(Gy86));
resetGyData(gy);
if (chip & CHIP_TYPE_MPU60X0) {
gy->m_mpu = mpu60x0_Init();
if (!gy->m_mpu) {
ERROR("Failed to Initialize the MPU Chip");
goto FAIL_MPU;
}
}
//Register the Interrupt Functions
retVal = mpu60x0_RegDataRdyCb (gy->m_mpu, mpuDataRdyIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the MPU:DataRdy");
}
retVal = mpu60x0_RegFifoOvrflowCb (gy->m_mpu, mpuFifoIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the MPU:FIFO");
}
retVal = mpu60x0_RegMotDetCb (gy->m_mpu, mpuMotDetIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the MPU:MotDet");
}
retVal = mpu60x0_RegFsynchCb (gy->m_mpu, mpuFsyncIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the MPU:Fsync");
}
retVal = mpu60x0_RegAuxI2cCb (gy->m_mpu, mpuAuxI2cIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the MPU:AuxI2c");
}
if (chip & CHIP_TYPE_HMC5883) {
gy->m_hmc = hmc5883_Init();
if (!gy->m_hmc) {
ERROR("Failed to Initialize the HMC Chip");
goto FAIL_HMC;
}
}
//Register the Interrupt Functions
retVal = hmc5883_RegDataRdyCb (gy->m_hmc, hmcDataRdyIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the HMC:DataRdy");
}
retVal = hmc5883_RegLockCb (gy->m_hmc, hmcRegLockIntHandler, (uint32)gy);
if (retVal) {
ERROR("Failed in Registering the Interrupt Handler for the HMC:Reg Lock");
}
if (chip & CHIP_TYPE_MS5611) {
gy->m_ms = ms5611_Init();
if (!gy->m_ms) {
ERROR("Failed to Initialize the MS Chip");
goto FAIL_MS;
}
}
//Init Completed Successfully
goto END;
FAIL_MS:
hmc5883_Destroy(gy->m_hmc);
FAIL_HMC:
mpu60x0_Destroy(gy->m_mpu);
FAIL_MPU:
free(gy);
gy = NULL;
END:
EXIT();
return (gyHandle)gy;
}
