imu: more checks and debug at init

diff --git a/mpu6050.c b/mpu6050.c
index 143d0e99be63b40d67859236b724d4f0b93b2a56..fe5399296d522340ecd1b68a56da08a5d4d9d5ce 100644 (file)
--- a/mpu6050.c
+++ b/mpu6050.c
@@ -113,7 +113,33 @@ uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
 
        err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
        if (err)
-               printf("error %.2X\r\n", err);
+               printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
+
+       return err;
+}
+
+uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
+{
+       uint8_t err;
+       uint8_t buffer[2];
+       uint8_t check = 0;
+
+       buffer[0] = reg;
+       buffer[1] = val;
+
+       err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
+       if (err)
+               printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
+
+       err = read_reg(address, reg, &check);
+       if (err)
+               return err;
+
+       if (check != val) {
+               printf("reg %x: %x != %x\r\n", reg, check, val);
+               return 0xff;
+       }
+
        return err;
 }
 
@@ -136,7 +162,7 @@ void mpu6050_compute_drift(void)
                s_gy += g_values[1];
                s_gz += g_values[2];
        }
-       printf("%"PRId32" %"PRId32" %"PRId32" (%"PRId32") \r\n", s_gx, s_gy, s_gz, i);
+       printf("%"PRId32" %"PRId32" %"PRId32" (%"PRIu16") \r\n", s_gx, s_gy, s_gz, i);
        s_gx /= i;
        s_gy /= i;
        s_gz /= i;
@@ -152,44 +178,56 @@ void mpu6050_compute_drift(void)
 }
 
 
-uint8_t setup_mpu9150_magneto()
+uint8_t setup_mpu9150_magneto(void)
 {
+       //uint8_t i, err;
+
        //MPU9150_I2C_ADDRESS = 0x0C;      //change Adress to Compass
 
-       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
-       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
-       send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+       /* XXX doesn't work, no answer from magneto */
+       /* for (i = 0; i < 127; i++) { */
+       /*      printf("i=%d\r\n", i); */
+       /*      err = send_check_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
 
-       //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
+       /*      if (err == 0) */
+       /*              printf("COIN\r\n"); */
+       /* } */
+       send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+       send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
+       send_check_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
 
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
-       //send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
-       //send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
-
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
-       send_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
+       //MPU9150_I2C_ADDRESS = 0x69;      //change Adress to MPU
 
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x24, 0x40); //Wait for Data at Slave0
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x25, 0x8C); //Set i2c address at slave0 at 0x0C
+       //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x02); //Set where reading at slave 0 starts
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x26, 0x03); //Set where reading at slave 0 starts
+       //send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x88); //set offset at start reading and enable
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x27, 0x86); //set offset at start reading and enable
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x28, 0x0C); //set i2c address at slv1 at 0x0C
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x29, 0x0A); //Set where reading at slave 1 starts
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x2A, 0x81); //Enable at set length to 1
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //overvride register
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x67, 0x03); //set delay rate
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x01, 0x80);
+
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x04); //set i2c slv4 delay
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x00); //override register
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x00); //clear usr setting
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x64, 0x01); //override register
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x6A, 0x20); //enable master i2c mode
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, 0x34, 0x13); //disable slv4
+
+       return 0;
 }
 
 uint8_t mpu6050_init(void)
 {
        uint8_t err;
        uint8_t id = 0;
-       uint8_t config = 0;
+       //uint8_t config = 0;
 
+       wait_ms(1000);
 
        while(1) {
                err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
@@ -199,63 +237,66 @@ uint8_t mpu6050_init(void)
                        break;
        }
 
-
+       /* 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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
        //Disable FSync, 48Hz DLPF
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_CONFIG, 0x03);
+       send_check_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);
-
+       //send_check_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);
+       send_check_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);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00011000);
 
        //Freefall threshold of <|0mg|
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_THR, 0x00);
        //Freefall duration limit of 0
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
        //Motion threshold of >0mg
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
        //Motion duration of >0s
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
        //Zero motion threshold
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
        //Zero motion duration threshold
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
+       send_check_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);
+       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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_CTRL, 0x00);
+       send_check_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);
+       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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
        //Enable data ready interrupt
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_ENABLE, 0x00);
+       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
@@ -300,22 +341,23 @@ uint8_t mpu6050_init(void)
        //MPU6050_RA_MOT_DETECT_STATUS         //Read-only
 
        //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);
+       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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
+       send_check_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);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
        //Motion detection control
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DETECT_CTRL, 0x00);
+       send_check_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);
+       send_check_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);
+       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_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_PWR_MGMT_2, 0x00);
+       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
@@ -324,7 +366,7 @@ uint8_t mpu6050_init(void)
        //MPU6050_RA_FIFO_COUNTH                //Read-only
        //MPU6050_RA_FIFO_COUNTL                //Read-only
        //Data transfer to and from the FIFO buffer
-       send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
+       send_check_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
        //MPU6050_RA_WHO_AM_I                         //Read-only, I2C address
 
        setup_mpu9150_magneto();
@@ -334,4 +376,5 @@ uint8_t mpu6050_init(void)
        mpu6050_compute_drift();
        printf("MPU6050 drift computed\r\n");
 
+       return 0;
 }