+/*
+ * Copyright (c) 2014, Olivier MATZ <zer0@droids-corp.org>
+ * Copyright (c) 2014, Fabrice DESCLAUX <serpilliere@droids-corp.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above copyright
+ * notice, this list of conditions and the following disclaimer in the
+ * documentation and/or other materials provided with the distribution.
+ * * Neither the name of the University of California, Berkeley nor the
+ * names of its contributors may be used to endorse or promote products
+ * derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE REGENTS AND CONTRIBUTORS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE REGENTS AND CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
#include <stdio.h>
#include <string.h>
-#include <timer.h>
-
#include <aversive/wait.h>
-#include <aversive/pgmspace.h>
-
-#include <uart.h>
-#include <i2c.h>
-
-#include "i2c_helper.h"
+#include "i2cm_sw.h"
#include "mpu6050.h"
+#include "mpu6050_regs.h"
-#define ToRad(x) ((x)*0.01745329252) // *pi/180
+#define ToRad(x) ((x) * 0.01745329252) // *pi/180
// fs_sel = 1
// 500°/s
#define accel_y_resolution 2048.0
#define accel_z_resolution 2048.0
-
-
-
-#define MPU6050_OFF_MSB 1
-#define MPU6050_OFF_LSB 0
-
-
#define MPU6050_ADDRESS 0x69
#define MPU6050_MAGNETO_ADDRESS 0x0D
#define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
-double mpu6050_gx;
-double mpu6050_gy;
-double mpu6050_gz;
+int16_t drift_g[3] = {0, 0, 0};
-double mpu6050_ax;
-double mpu6050_ay;
-double mpu6050_az;
+/* read "len" bytes of mpu6050 registers starting at specified address */
+static uint8_t read_reg_len(uint8_t i2c_addr, uint8_t reg_addr,
+ uint8_t *values, uint8_t len)
+{
+ uint8_t err = 0;
-double mpu6050_mx;
-double mpu6050_my;
-double mpu6050_mz;
+ err = i2cm_send(i2c_addr, ®_addr, 1);
+ if (err) {
+ printf("read reg len: i2c error send\r\n");
+ return err;
+ }
-double mpu6050_temp;
+ err = i2cm_recv(i2c_addr, len);
+ if (err) {
+ printf("read reg len: i2c error recv\r\n");
+ return err;
+ }
-int16_t drift_g[3] = {0, 0, 0};
+ err = i2cm_get_recv_buffer(values, len);
+ if (err != len) {
+ printf("read reg len: i2c error get recv\r\n");
+ return 0xFF;
+ }
+ return 0;
+}
-uint8_t mpu6050_read_gyro_raw(int16_t *values)
+/* read one byte of mpu6050 register at specified address */
+static uint8_t read_reg(uint8_t i2c_addr, uint8_t reg_addr,
+ uint8_t *value)
{
- uint8_t err;
- uint8_t i;
- err = read_reg_len(MPU6050_ADDRESS, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
- for (i=0;i<3; i++) {
- values[i] = SWAP_16(values[i]);
- }
-
- return err;
+ return read_reg_len(i2c_addr, reg_addr, value, 1);;
}
-uint8_t mpu6050_read_all_axes(int16_t *values)
+/* fill the axes[3] pointer with the 3 axes of gyro (16bits) */
+uint8_t mpu6050_read_gyro_raw(int16_t *axes)
{
uint8_t err;
uint8_t i;
- err = read_reg_len(MPU6050_ADDRESS, 0x3b, (uint8_t*)values, sizeof(int16_t)*10);
- for (i=0;i<7; i++) {
- values[i] = SWAP_16(values[i]);
+ err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_GYRO_XOUT_H,
+ (uint8_t *)axes, sizeof(int16_t) * 3);
+ for (i = 0; i < 3; i++) {
+ axes[i] = SWAP_16(axes[i]);
}
- mpu6050_ax = 9.81 * (double)values[0] / accel_x_resolution ;
- mpu6050_ay = 9.81 * (double)values[1] / accel_y_resolution ;
- mpu6050_az = 9.81 * (double)values[2] / accel_z_resolution ;
-
- mpu6050_temp = (double)values[3]/340. + 36.5;
-
- mpu6050_gx = ToRad((double)(values[4] - drift_g[0]) / gyro_x_resolution );
- mpu6050_gy = ToRad((double)(values[5] - drift_g[1]) / gyro_y_resolution );
- mpu6050_gz = ToRad((double)(values[6] - drift_g[2]) / gyro_z_resolution );
-
- mpu6050_mx = (double) values[7] * 0.3;
- mpu6050_my = (double) values[8] * 0.3;
- mpu6050_mz = (double) values[9] * 0.3;
-
-
return err;
}
+/* fill the imu structure with axes comming from mpu6050 */
+uint8_t mpu6050_read_all_axes(struct imu_info *imu)
+{
+ int16_t axes[10];
+ uint8_t err;
+ uint8_t i;
+ err = read_reg_len(MPU6050_ADDRESS, MPU6050_RA_ACCEL_XOUT_H,
+ (uint8_t *)axes, sizeof(axes));
+ for (i = 0; i < 7; i++) {
+ axes[i] = SWAP_16(axes[i]);
+ }
+ imu->ax = 9.81 * (double)axes[0] / accel_x_resolution ;
+ imu->ay = 9.81 * (double)axes[1] / accel_y_resolution ;
+ imu->az = 9.81 * (double)axes[2] / accel_z_resolution ;
-uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
-{
- uint8_t err;
- uint8_t buffer[2];
+ imu->temp = (double)axes[3]/340. + 36.5;
- buffer[0] = reg;
- buffer[1] = val;
+ imu->gx = ToRad((double)(axes[4] - drift_g[0]) / gyro_x_resolution);
+ imu->gy = ToRad((double)(axes[5] - drift_g[1]) / gyro_y_resolution);
+ imu->gz = ToRad((double)(axes[6] - drift_g[2]) / gyro_z_resolution);
- err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
- if (err)
- printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
+ imu->mx = (double) axes[7] * 0.3;
+ imu->my = (double) axes[8] * 0.3;
+ imu->mz = (double) axes[9] * 0.3;
return err;
}
-uint8_t send_check_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
+
+/* write a 8bits value at the specified register of the mpu6050 */
+static uint8_t send_mpu6050_cmd(uint8_t address, uint8_t reg, uint8_t val)
{
uint8_t err;
uint8_t buffer[2];
buffer[0] = reg;
buffer[1] = val;
- err = i2c_send(address, (unsigned char*)buffer, 2, I2C_CTRL_SYNC);
+ err = i2cm_send(address, (unsigned char*)buffer, 2);
if (err)
- printf("send_check_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
+ printf("send_mpu6050_cmd(reg=%x): error %.2X\r\n", reg, err);
err = read_reg(address, reg, &check);
if (err)
return err;
}
-
-void mpu6050_compute_drift(void)
+/* XXX add comment */
+static void mpu6050_compute_drift(void)
{
uint16_t i;
int32_t s_gx, s_gy, s_gz;
drift_g[2]);
}
-
-uint8_t setup_mpu9150_magneto(void)
+static uint8_t setup_mpu9150_magneto(void)
{
//uint8_t i, err;
/* 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 */
+ /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
/* 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_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
+ send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x0F); //SelfTest
+ send_mpu6050_cmd(MPU6050_MAGNETO_ADDRESS, 0x0A, 0x00); //PowerDownMode
//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
+ 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
return 0;
}
-uint8_t mpu6050_init(void)
+int8_t mpu6050_init(void)
{
uint8_t err;
uint8_t id = 0;
- //uint8_t config = 0;
- wait_ms(1000);
+ wait_ms(1000); /* XXX needed ? */
- while(1) {
+ while (1) { /* XXX timeout */
err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
if (err)
continue;
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_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);
setup_mpu9150_magneto();
-
printf("MPU6050 Setup Complete\r\n");
mpu6050_compute_drift();
printf("MPU6050 drift computed\r\n");
git.droids-corp.org / protos / imu.git / blobdiff