+/*
+ * 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 <scheduler.h>
-#include <timer.h>
-
#include <aversive/wait.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 gyro_x_resolution 66.5
-#define gyro_y_resolution 66.5
-#define gyro_z_resolution 66.5
+/* #define gyro_x_resolution 66.5 */
+/* #define gyro_y_resolution 66.5 */
+/* #define gyro_z_resolution 66.5 */
// fs_sel = 3
// 2000°/s
#define gyro_y_resolution 16.4
#define gyro_z_resolution 16.4
-#define accel_x_resolution 8192.0
-#define accel_y_resolution 8192.0
-#define accel_z_resolution 8192.0
-
-
-
+#define accel_x_resolution 2048.0
+#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 0x68
-#define MPU6050_MAGNETO_ADDRESS 0x0C
+#define MPU6050_ADDRESS 0x69
+#define MPU6050_MAGNETO_ADDRESS 0x0D
#define SWAP_16(a) ((( (a) & 0xff)<<8) | (( (a) >> 8) & 0xff))
-float mpu6050_gx;
-float mpu6050_gy;
-float mpu6050_gz;
+int16_t drift_g[3] = {0, 0, 0};
-float mpu6050_ax;
-float mpu6050_ay;
-float 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;
-float mpu6050_mx;
-float mpu6050_my;
-float mpu6050_mz;
+ err = i2cm_send(i2c_addr, ®_addr, 1);
+ if (err) {
+ printf("read reg len: i2c error send\r\n");
+ return err;
+ }
-float 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;
+}
+/* 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)
+{
+ return read_reg_len(i2c_addr, reg_addr, value, 1);;
+}
-uint8_t mpu6050_read_gyro_raw(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, 0x43, (uint8_t*)values, sizeof(int16_t)*3);
- for (i=0;i<3; 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]);
}
return err;
}
-uint8_t mpu6050_read_all_axes(int16_t *values)
+/* 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, 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_ACCEL_XOUT_H,
+ (uint8_t *)axes, sizeof(axes));
- mpu6050_ax = 9.81 * (float)values[0] / accel_x_resolution ;
- mpu6050_ay = 9.81 * (float)values[1] / accel_y_resolution ;
- mpu6050_az = 9.81 * (float)values[2] / accel_z_resolution ;
+ for (i = 0; i < 7; i++) {
+ axes[i] = SWAP_16(axes[i]);
+ }
- mpu6050_temp = (float)values[3]/340. + 36.5;
+ 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 ;
- mpu6050_gx = ToRad((float)(values[4] - drift_g[0]) / gyro_x_resolution );
- mpu6050_gy = ToRad((float)(values[5] - drift_g[1]) / gyro_y_resolution );
- mpu6050_gz = ToRad((float)(values[6] - drift_g[2]) / gyro_z_resolution );
+ imu->temp = (double)axes[3]/340. + 36.5;
- mpu6050_mx = (float) values[7] * 0.3;
- mpu6050_my = (float) values[8] * 0.3;
- mpu6050_mz = (float) values[9] * 0.3;
+ 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);
+ 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_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];
+ uint8_t check = 0;
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("error %.2X\r\n", err);
+ printf("send_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;
}
-
-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;
s_gx = s_gy = s_gz = 0;
for (i = 0; i < 0x100; i ++) {
- mpu6050_read_gyro_raw(&g_values);
+ mpu6050_read_gyro_raw(g_values);
s_gx += g_values[0];
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;
drift_g[2]);
}
-
-uint8_t setup_mpu9150_magneto()
+static uint8_t setup_mpu9150_magneto(void)
{
+ //uint8_t i, err;
+
//MPU9150_I2C_ADDRESS = 0x0C; //change Adress to Compass
+ /* XXX doesn't work, no answer from magneto */
+ /* for (i = 0; i < 127; i++) { */
+ /* printf("i=%d\r\n", i); */
+ /* err = send_mpu6050_cmd(i, 0x0A, 0x00); //PowerDownMode */
+
+ /* if (err == 0) */
+ /* printf("COIN\r\n"); */
+ /* } */
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
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); /* XXX needed ? */
- while(1) {
- err = read_reg(MPU6050_ADDRESS, 0x75, &id);
+ while (1) { /* XXX timeout */
+ err = read_reg(MPU6050_ADDRESS, MPU6050_RA_WHO_AM_I, &id);
if (err)
continue;
if (id == 0x68)
break;
}
+ /* 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
+ /* Sets sample rate to 1000/1+1 = 500Hz */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SMPLRT_DIV, 0x01);
- //Disable FSync, 48Hz DLPF
+ /* 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
+ /* 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 +-4g, no DHPF
- send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ACCEL_CONFIG, 0b00001000);
- //Freefall threshold of <|0mg|
+ /* 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
+ /* Freefall duration limit of 0 */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FF_DUR, 0x00);
- //Motion threshold of >0mg
+ /* Motion threshold of >0mg */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_THR, 0x00);
- //Motion duration of >0s
+ /* Motion duration of >0s */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_MOT_DUR, 0x00);
- //Zero motion threshold
+ /* Zero motion threshold */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_THR, 0x00);
- //Zero motion duration threshold
+ /* Zero motion duration threshold */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_ZRMOT_DUR, 0x00);
- //Disable sensor output to FIFO buffer
+ /* 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
+ /* 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
+ /* 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_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);
-
- //MPU6050_RA_I2C_MST_STATUS //Read-only
- //Setup INT pin and AUX I2C pass through
+ /* Setup INT pin and AUX I2C pass through */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_INT_PIN_CFG, 0x00);
- //Enable data ready interrupt
+ /* Enable data ready interrupt */
send_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
+ /* 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
+
+ /* More slave config */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x00);
- //Reset sensor signal paths
+ /* Reset sensor signal paths */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_SIGNAL_PATH_RESET, 0x00);
- //Motion detection control
+ /* 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
+ /* 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
+ /* 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
+ /* 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);
- //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
+ /* Data transfer to and from the FIFO buffer */
send_mpu6050_cmd(MPU6050_ADDRESS, MPU6050_RA_FIFO_R_W, 0x00);
- //MPU6050_RA_WHO_AM_I //Read-only, I2C address
setup_mpu9150_magneto();
-
printf("MPU6050 Setup Complete\r\n");
mpu6050_compute_drift();
printf("MPU6050 drift computed\r\n");
+ return 0;
}