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
}
