- /* use one of the gyro for clock ref: if we don't do that, some i2c commands fail ?? */
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
-
- //Sets sample rate to 1000/1+1 = 500Hz
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
- //Disable FSync, 48Hz DLPF
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
- //Disable gyro self tests, scale of 500 degrees/s
- //send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000);
- //Disable gyro self tests, scale of 2000 degrees/s
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
-
- //Disable accel self tests, scale of +-16g, no DHPF
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
-
- //Freefall threshold of <|0mg|
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
- //Freefall duration limit of 0
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
- //Motion threshold of >0mg
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
- //Motion duration of >0s
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
- //Zero motion threshold
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
- //Zero motion duration threshold
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
- //Disable sensor output to FIFO buffer
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
-
- //AUX I2C setup
- //Sets AUX I2C to single master control, plus other config
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
- //Setup AUX I2C slaves
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
-
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
-
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
-
-
- //MPU6050_RA_I2C_MST_STATUS //Read-only
- //Setup INT pin and AUX I2C pass through
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
- //Enable data ready interrupt
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
-
- //MPU6050_RA_DMP_INT_STATUS //Read-only
- //MPU6050_RA_INT_STATUS 3A //Read-only
- //MPU6050_RA_ACCEL_XOUT_H //Read-only
- //MPU6050_RA_ACCEL_XOUT_L //Read-only
- //MPU6050_RA_ACCEL_YOUT_H //Read-only
- //MPU6050_RA_ACCEL_YOUT_L //Read-only
- //MPU6050_RA_ACCEL_ZOUT_H //Read-only
- //MPU6050_RA_ACCEL_ZOUT_L //Read-only
- //MPU6050_RA_TEMP_OUT_H //Read-only
- //MPU6050_RA_TEMP_OUT_L //Read-only
- //MPU6050_RA_GYRO_XOUT_H //Read-only
- //MPU6050_RA_GYRO_XOUT_L //Read-only
- //MPU6050_RA_GYRO_YOUT_H //Read-only
- //MPU6050_RA_GYRO_YOUT_L //Read-only
- //MPU6050_RA_GYRO_ZOUT_H //Read-only
- //MPU6050_RA_GYRO_ZOUT_L //Read-only
- //MPU6050_RA_EXT_SENS_DATA_00 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_01 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_02 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_03 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_04 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_05 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_06 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_07 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_08 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_09 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_10 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_11 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_12 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_13 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_14 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_15 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_16 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_17 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_18 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_19 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_20 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_21 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_22 //Read-only
- //MPU6050_RA_EXT_SENS_DATA_23 //Read-only
- //MPU6050_RA_MOT_DETECT_STATUS //Read-only
-
- //Slave out, dont care
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
-
- //More slave config
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
- //Reset sensor signal paths
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
- //Motion detection control
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
- //Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
- //Sets clock source to gyro reference w/ PLL
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
- //Controls frequency of wakeups in accel low power mode plus the sensor standby modes
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
- //MPU6050_RA_BANK_SEL //Not in datasheet
- //MPU6050_RA_MEM_START_ADDR //Not in datasheet
- //MPU6050_RA_MEM_R_W //Not in datasheet
- //MPU6050_RA_DMP_CFG_1 //Not in datasheet
- //MPU6050_RA_DMP_CFG_2 //Not in datasheet
- //MPU6050_RA_FIFO_COUNTH //Read-only
- //MPU6050_RA_FIFO_COUNTL //Read-only
- //Data transfer to and from the FIFO buffer
- send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
- //MPU6050_RA_WHO_AM_I //Read-only, I2C address
+ /* XX use one of the gyro for clock ref: if we don't do that, some i2c
+ * commands fail... why ?? */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
+
+ /* Sets sample rate to 1000/1+1 = 500Hz */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
+ /* Disable FSync, 48Hz DLPF */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
+ /* Disable gyro self tests, scale of 500 degrees/s */
+ /* send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00001000); */
+ /* Disable gyro self tests, scale of 2000 degrees/s */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_GYRO_CONFIG, 0b00011000);
+
+ /* Disable accel self tests, scale of +-16g, no DHPF */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
+
+ /* Freefall threshold of <|0mg| */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
+ /* Freefall duration limit of 0 */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
+ /* Motion threshold of >0mg */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
+ /* Motion duration of >0s */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
+ /* Zero motion threshold */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
+ /* Zero motion duration threshold */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
+ /* Disable sensor output to FIFO buffer */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_EN, 0x00);
+
+ /* AUX I2C setup */
+ /* Sets AUX I2C to single master control, plus other config */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
+ /* Setup AUX I2C slaves */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_REG, 0x00);
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_ADDR, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_ADDR, 0x00);
+
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_REG, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_CTRL, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV4_DI, 0x00);
+
+ /* Setup INT pin and AUX I2C pass through */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
+ /* Enable data ready interrupt */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
+
+ /* Slave out, dont care */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV0_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV1_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV2_DO, 0x00);
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_SLV3_DO, 0x00);
+
+ /* More slave config */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
+ /* Reset sensor signal paths */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
+ /* Motion detection control */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
+ /* Disables FIFO, AUX I2C, FIFO and I2C reset bits to 0 */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_USER_CTRL, 0x00);
+ /* Sets clock source to gyro reference w/ PLL */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_1, 0b00000010);
+ /* Controls frequency of wakeups in accel low power mode plus the sensor
+ * standby modes */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
+ /* Data transfer to and from the FIFO buffer */
+ send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);