AMiRo-OS

/*

 * sd_hal_mpu6050.c

 *

 *  Created on: Feb 19, 2016

 *      Author: Sina Darvishi

 *  Edited on: Jan 15, 2019

 *      Editor: Simon Welzel

 */

/**

 * |----------------------------------------------------------------------

 * | Copyright (C) Sina Darvishi,2016

 * |

 * | This program is free software: you can redistribute it and/or modify

 * | it under the terms of the GNU General Public License as published by

 * | the Free Software Foundation, either version 3 of the License, or

 * | any later version.

 * |

 * | This program is distributed in the hope that it will be useful,

 * | but WITHOUT ANY WARRANTY; without even the implied warranty of

 * | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the

 * | GNU General Public License for more details.

 * |

 * | You should have received a copy of the GNU General Public License

 * | along with this program.  If not, see <http://www.gnu.org/licenses/>.

 * |----------------------------------------------------------------------

 */

#include "alld_mpu6050.h"

#include <amiro-lld.h>

#if defined(AMIROLLD_CFG_USE_MPU6050) || defined(__DOXYGEN__)

#include <stdint.h>

/* Default I2C address */

#define MPU6050_I2C_ADDR   0xD0

/* Who I am register value */

#define MPU6050_I_AM    0x68

/* MPU6050 registers */

#define MPU6050_AUX_VDDIO   0x01

#define MPU6050_SMPLRT_DIV   0x19

#define MPU6050_CONFIG    0x1A

#define MPU6050_GYRO_CONFIG   0x1B

#define MPU6050_ACCEL_CONFIG  0x1C

#define MPU6050_MOTION_THRESH  0x1F

#define MPU6050_INT_PIN_CFG   0x37

#define MPU6050_INT_ENABLE   0x38

#define MPU6050_INT_STATUS   0x3A

#define MPU6050_ACCEL_XOUT_H  0x3B

#define MPU6050_ACCEL_XOUT_L  0x3C

#define MPU6050_ACCEL_YOUT_H  0x3D

#define MPU6050_ACCEL_YOUT_L  0x3E

#define MPU6050_ACCEL_ZOUT_H  0x3F

#define MPU6050_ACCEL_ZOUT_L  0x40

#define MPU6050_TEMP_OUT_H   0x41

#define MPU6050_TEMP_OUT_L   0x42

#define MPU6050_GYRO_XOUT_H   0x43

#define MPU6050_GYRO_XOUT_L   0x44

#define MPU6050_GYRO_YOUT_H   0x45

#define MPU6050_GYRO_YOUT_L   0x46

#define MPU6050_GYRO_ZOUT_H   0x47

#define MPU6050_GYRO_ZOUT_L   0x48

#define MPU6050_MOT_DETECT_STATUS 0x61

#define MPU6050_SIGNAL_PATH_RESET 0x68

#define MPU6050_MOT_DETECT_CTRL  0x69

#define MPU6050_USER_CTRL   0x6A

#define MPU6050_PWR_MGMT_1   0x6B

#define MPU6050_PWR_MGMT_2   0x6C

#define MPU6050_FIFO_COUNTH   0x72

#define MPU6050_FIFO_COUNTL   0x73

#define MPU6050_FIFO_R_W   0x74

#define MPU6050_WHO_AM_I   0x75

/* Gyro sensitivities in degrees/s */

#define MPU6050_GYRO_SENS_250  ((float) 131)

#define MPU6050_GYRO_SENS_500  ((float) 65.5)

#define MPU6050_GYRO_SENS_1000  ((float) 32.8)

#define MPU6050_GYRO_SENS_2000  ((float) 16.4)

/* Acce sensitivities in g/s */

#define MPU6050_ACCE_SENS_2   ((float) 16384)

#define MPU6050_ACCE_SENS_4   ((float) 8192)

#define MPU6050_ACCE_SENS_8   ((float) 4096)

#define MPU6050_ACCE_SENS_16  ((float) 2048)

// from alld_ina219.c

/**

 * @brief Read the value of one or more of the registers.

 * @param[in]   i2cd        i2c driver

 * @param[in]   inad        ina219 driver

 * @param[in]   addr        register address

 * @param[out]  data        register content

 * @param[in]   num         number of subsequent registers to read

 * @param[in]   timeout     timeout

 * @return                  An indicator whether the call was successfull

 */

inline apalExitStatus_t

mpu6050_lld_read_register(const MPU6050Driver* const mpu6050, const mpu6050_lld_register_t addr, uint16_t* const data, const uint8_t num, const apalTime_t timeout)

{

  apalDbgAssert(mpu6050 != 0);

  apalDbgAssert(mpu6050->i2cd != 0);

  apalDbgAssert(data != 0);

  uint8_t buffer[num*2];

  apalExitStatus_t status = apalI2CMasterTransmit(mpu6050->i2cd, (MPU6050_LLD_I2C_ADDR_FIXED | mpu6050->addr), (uint8_t*)&addr, 1, buffer, 2*num, timeout);

  for (uint8_t dataIdx = 0; dataIdx < num; dataIdx++) {

    data[dataIdx] = (buffer[2*dataIdx] << 8) | buffer[2*dataIdx+1];

  }

  return status;

}

// from alld_ina219.c

/**

 * @brief Write the value of one or more of the registers.

 * @param[in]   i2cd        i2c driver

 * @param[in]   inad        ina219 driver

 * @param[in]   addr        register address

 * @param[in]   data        data to write

 * @param[in]   num         number of subsequent registers to read

 * @param[in]   timeout     timeout

 * @return                  An indicator whether the call was successfull

 */

inline apalExitStatus_t

mpu6050_lld_write_register(const MPU6050Driver* const mpu6050, const mpu6050_lld_register_t addr, const uint16_t* const data, const uint8_t num, const apalTime_t timeout)

{

  apalDbgAssert(mpu6050 != 0);

  apalDbgAssert(mpu6050->i2cd != 0);

  apalDbgAssert(data != 0);

  uint8_t buffer[1+2*num];

  buffer[0] = addr;

  for (uint8_t dataIdx = 0; dataIdx < num; dataIdx++) {

    buffer[dataIdx*2+1] = data[dataIdx] >> 8;

    buffer[dataIdx*2+2] = data[dataIdx] & (0x00FFu);

  }

  return apalI2CMasterTransmit(mpu6050->i2cd, (MPU6050_LLD_I2C_ADDR_FIXED | mpu6050->addr), buffer, 1+2*num, NULL, 0, timeout);

}

inline apalExitStatus_t mpu6050_get_axis_x(const MPU6050Driver* const mpu6050, const mpu6050_lld_register_t addr, const uint16_t* const data, const uint8_t num, const apalTime_t timeout)

{

}

#endif /* AMIROLLD_CFG_USE_MPU6050*/