void MPU6000_init16(void) { MPUSPI_SS = 1; // deassert MPU SS MPUSPI_TRIS = 0; // make MPU SS an output // set prescaler for FCY/24 = 666 KHz at 16MIPS initMPUSPI_master16(SEC_PRESCAL_6_1, PRI_PRESCAL_4_1); // Use the following after we raise MatrixPilot from 16 to 40 MIPS on UDB4 and UDB5 // set prescaler for FCY/64 = 625KHz at 40MIPS // initMPUSPI_master16(SEC_PRESCAL_4_1, PRI_PRESCAL_16_1); // need at least 60 msec delay here __delay_ms(60); writeMPUSPIreg16(MPUREG_PWR_MGMT_1, BIT_H_RESET); // 1msec delay seems to be needed __delay_ms(1); // Wake up device and select GyroZ clock (better performance) writeMPUSPIreg16(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); // Disable I2C bus (recommended on datasheet) writeMPUSPIreg16(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); // SAMPLE RATE writeMPUSPIreg16(MPUREG_SMPLRT_DIV, 4); // Sample rate = 200Hz Fsample= 1Khz/(N+1) = 200Hz // scaling & DLPF writeMPUSPIreg16(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); // writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000º/s writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_500DPS); // Gyro scale 500º/s #if ( ACCEL_RANGE == 2 ) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_2G); // Accel scele 2g, g = 8192 #elif ( ACCEL_RANGE == 4 ) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_4G); // Accel scale g = 4096 #elif ( ACCEL_RANGE == 8 ) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_8G); // Accel scale g = 2048 #else #error "Invalid ACCEL_RANGE" #endif #if 0 // Legacy from Mark Whitehorn's testing, we might need it some day. // SAMPLE RATE writeMPUSPIreg16(MPUREG_SMPLRT_DIV, 7); // Sample rate = 1KHz Fsample= 8Khz/(N+1) // no DLPF, gyro sample rate 8KHz writeMPUSPIreg16(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_500DPS); // Gyro scale 500º/s // writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_2G); // Accel scele 2g, g = 16384 writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_4G); // Accel scale g = 8192 // writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_8G); // Accel scale g = 4096 #endif // INT CFG => Interrupt on Data Ready, totem-pole (push-pull) output writeMPUSPIreg16(MPUREG_INT_PIN_CFG, BIT_INT_LEVEL | BIT_INT_RD_CLEAR); // INT: Clear on any read writeMPUSPIreg16(MPUREG_INT_ENABLE, BIT_DATA_RDY_EN); // INT: Raw data ready #if (BOARD_TYPE == UDB4_BOARD||BOARD_TYPE == UDB5_BOARD) // UDB4_BOARD otion is for testing purposes // older versions of MatrixPilot run at 16 MIPS on UDB4 and UDB5 // set prescaler for FCY/2 = 8MHz at 16 MIPS initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_1_1); // When MP is revised to run at 40 MIPS, use this instead: // set prescaler for FCY/5 = 8MHz at 40MIPS // initMPUSPI_master16(SEC_PRESCAL_5_1, PRI_PRESCAL_1_1); #elif (BOARD_TYPE & AUAV2_BOARD) // set prescaler for FCY/2 = 20MHz at 40MIPS initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_1_1); // // set prescaler for FCY/4 = 10MHz at 40MIPS // initMPUSPI_master16(SEC_PRESCAL_4_1, PRI_PRESCAL_1_1); // // set prescaler for FCY/8 = 5MHz at 40MIPS // initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_4_1); //TODO: using XC16 compiler this doesn't work at 8MHz, drop to 1.25MHz // initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_16_1); #if ((BOARD_TYPE & AUAV2_REV) < 2) _TRISE8 = 1; // make INT1 an input #else _TRISMPUINT = 1; #endif #else #error "Invalid BOARD_TYPE for MPU6000" #endif #if ( MPU_SPI == 1) _INT1EP = 1; // Setup INT1 pin to interrupt on falling edge _INT1IF = 0; // Reset INT1 interrupt flag _INT1IE = 1; // Enable INT1 Interrupt Service Routine _INT1IP = 6; #elif ( MPU_SPI == 2 ) _INT3EP = 1; // Setup INT3 pin to interrupt on falling edge _INT3IF = 0; // Reset INT3 interrupt flag _INT3IE = 1; // Enable INT3 Interrupt Service Routine _INT3IP = 6; #endif }
void MPU6000_init16(void) { // MPU-6000 maximum SPI clock is specified as 1 MHz for all registers // however the datasheet states that the sensor and interrupt registers // may be read using an SPI clock of 20 Mhz // NOTE!!: the SPI limit on the dsPIC is 9 Mhz // Primary prescaler options 1:1/4/16/64 // Secondary prescaler options 1:1 to 1:8 // As these register accesses are one time only during initial setup lets be // conservative and only run the SPI bus at half the maximum specified speed #if (MIPS == 70) // set prescaler for FCY/112 = 625 kHz at 70MIPS initMPUSPI_master16(SEC_PRESCAL_7_1, PRI_PRESCAL_16_1); #elif (MIPS == 64) // set prescaler for FCY/96 = 667 kHz at 64MIPS initMPUSPI_master16(SEC_PRESCAL_6_1, PRI_PRESCAL_16_1); #elif (MIPS == 40) // set prescaler for FCY/64 = 625 KHz at 40MIPS initMPUSPI_master16(SEC_PRESCAL_4_1, PRI_PRESCAL_16_1); #elif (MIPS == 32) // set prescaler for FCY/48 = 667 kHz at 32 MIPS initMPUSPI_master16(SEC_PRESCAL_3_1, PRI_PRESCAL_16_1); #elif (MIPS == 16) // set prescaler for FCY/24 = 667 kHz at 16MIPS initMPUSPI_master16(SEC_PRESCAL_6_1, PRI_PRESCAL_4_1); #else #error Invalid MIPS Configuration #endif // MIPS // need at least 60 msec delay here delay_ms(60); writeMPUSPIreg16(MPUREG_PWR_MGMT_1, BIT_H_RESET); // 10msec delay seems to be needed for AUAV3 (MW's prototype) delay_ms(10); // Wake up device and select GyroZ clock (better performance) writeMPUSPIreg16(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); // Disable I2C bus (recommended on datasheet) writeMPUSPIreg16(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); // SAMPLE RATE writeMPUSPIreg16(MPUREG_SMPLRT_DIV, 4); // Sample rate = 200Hz Fsample= 1Khz/(N+1) = 200Hz // scaling & DLPF writeMPUSPIreg16(MPUREG_CONFIG, BITS_DLPF_CFG_42HZ); // writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_2000DPS); // Gyro scale 2000º/s writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_500DPS); // Gyro scale 500º/s #if (ACCEL_RANGE == 2) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_2G); // Accel scele 2g, g = 8192 #elif (ACCEL_RANGE == 4) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_4G); // Accel scale g = 4096 #elif (ACCEL_RANGE == 8) writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_8G); // Accel scale g = 2048 #else #error "Invalid ACCEL_RANGE" #endif #if 0 // Legacy from Mark Whitehorn's testing, we might need it some day. // SAMPLE RATE writeMPUSPIreg16(MPUREG_SMPLRT_DIV, 7); // Sample rate = 1KHz Fsample= 8Khz/(N+1) // no DLPF, gyro sample rate 8KHz writeMPUSPIreg16(MPUREG_CONFIG, BITS_DLPF_CFG_256HZ_NOLPF2); writeMPUSPIreg16(MPUREG_GYRO_CONFIG, BITS_FS_500DPS); // Gyro scale 500º/s // writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_2G); // Accel scale 2g, g = 16384 writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_4G); // Accel scale g = 8192 // writeMPUSPIreg16(MPUREG_ACCEL_CONFIG, BITS_FS_8G); // Accel scale g = 4096 #endif // INT CFG => Interrupt on Data Ready, totem-pole (push-pull) output writeMPUSPIreg16(MPUREG_INT_PIN_CFG, BIT_INT_LEVEL | BIT_INT_RD_CLEAR); // INT: Clear on any read writeMPUSPIreg16(MPUREG_INT_ENABLE, BIT_DATA_RDY_EN); // INT: Raw data ready // Bump the SPI clock up towards 20 MHz for ongoing sensor and interrupt register reads // 20 MHz is the maximum specified for the MPU-6000 // however 9 MHz is the maximum specified for the dsPIC33EP // Primary prescaler options 1:1/4/16/64 // Secondary prescaler options 1:1 to 1:8 #if (MIPS == 70) // set prescaler for FCY/32 = 2.2 MHz at 70MIPS initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_16_1); #elif (MIPS == 64) // set prescaler for FCY/8 = 8 MHz at 64 MIPS initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_4_1); #elif (MIPS == 40) // UDB5 only // set prescaler for FCY/5 = 8 MHz at 40MIPS initMPUSPI_master16(SEC_PRESCAL_5_1, PRI_PRESCAL_1_1); #elif (MIPS == 32) // set prescaler for FCY/4 = 8 MHz at 32 MIPS initMPUSPI_master16(SEC_PRESCAL_1_1, PRI_PRESCAL_4_1); #elif (MIPS == 16) // set prescaler for FCY/2 = 8 MHz at 16 MIPS initMPUSPI_master16(SEC_PRESCAL_2_1, PRI_PRESCAL_1_1); #else #error Invalid MIPS Configuration #endif // MIPS _TRISMPUINT = 1; // this is probably already taken care of in mcu.c for most boards #if (MPU_SPI == 1) _INT1EP = 1; // Setup INT1 pin to interrupt on falling edge _INT1IP = INT_PRI_INT1; _INT1IF = 0; // Reset INT1 interrupt flag _INT1IE = 1; // Enable INT1 Interrupt Service Routine #elif (MPU_SPI == 2) _INT3EP = 1; // Setup INT3 pin to interrupt on falling edge _INT3IP = INT_PRI_INT3; _INT3IF = 0; // Reset INT3 interrupt flag _INT3IE = 1; // Enable INT3 Interrupt Service Routine #endif }