From 226fef1618dd9a37c050bb79a24c77396cd14c46 Mon Sep 17 00:00:00 2001 From: jaseg Date: Sun, 11 Oct 2020 23:31:12 +0200 Subject: Initial fw commit --- fw/mpu6050.c | 244 +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ 1 file changed, 244 insertions(+) create mode 100644 fw/mpu6050.c (limited to 'fw/mpu6050.c') diff --git a/fw/mpu6050.c b/fw/mpu6050.c new file mode 100644 index 0000000..dc374d8 --- /dev/null +++ b/fw/mpu6050.c @@ -0,0 +1,244 @@ +/* MPU6050 device I2C library code for ARM STM32F103xx is placed under the MIT license + Copyright (c) 2012 Harinadha Reddy Chintalapalli + + Permission is hereby granted, free of charge, to any person obtaining a copy + of this software and associated documentation files (the "Software"), to deal + in the Software without restriction, including without limitation the rights + to use, copy, modify, merge, publish, distribute, sublicense, and/or sell + copies of the Software, and to permit persons to whom the Software is + furnished to do so, subject to the following conditions: + + The above copyright notice and this permission notice shall be included in + all copies or substantial portions of the Software. + + THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR + IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, + FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE + AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER + LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, + OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN + THE SOFTWARE. +*/ + +#include "mpu6050.h" +#include "i2c.h" + +/** @defgroup MPU_Library + * @{ + */ + +/** Power on and prepare for general usage. + * This will activate the device and take it out of sleep mode (which must be done + * after start-up). This function also sets both the accelerometer and the gyroscope + * to their most sensitive settings, namely +/- 2g and +/- 250 degrees/sec, and sets + * the clock source to use the X Gyro for reference, which is slightly better than + * the default internal clock source. + */ +void mpu_init() +{ + mpu_reg_write(MPU_RA_PWR_MGMT1, MPU_CLOCK_PLL_XGYRO); + mpu_set_gyro_fs(MPU_GYRO_FS_250); + mpu_set_accel_fs(MPU_ACCEL_FS_2); +} + +void mpu_init_low_power(uint8_t wake_freq, bool enable_interrupt) +{ + mpu_set_gyro_fs(MPU_GYRO_FS_250); + mpu_set_accel_fs(MPU_ACCEL_FS_2); + /* For these two regs, see pwr mgmt reg 2 desc in reg map ref manual, pg. 42 */ + mpu_reg_write(MPU_RA_PWR_MGMT1, MPU_PWR_MGMT1_CYCLE | MPU_PWR_MGMT1_TEMP_DIS | MPU_CLOCK_INTERNAL); + mpu_reg_write(MPU_RA_PWR_MGMT2, MPU_PWR_MGMT2_STBY_XG | MPU_PWR_MGMT2_STBY_YG | MPU_PWR_MGMT2_STBY_ZG | wake_freq); + + if (enable_interrupt) { + mpu_reg_write(MPU_RA_INT_PIN_CFG, MPU_INT_PIN_CFG_RD_CLEAR); + mpu_reg_write(MPU_RA_INT_ENABLE, MPU_INT_ENABLE_DATA_RDY); + } +} + +/** Verify the I2C connection. + * Make sure the device is connected and responds as expected. + * @return True if connection is valid, FALSE otherwise + */ +bool mpu_test_connection() +{ + return mpu_device_id() == MPU_DEVICE_ID; +} + +/** Get Device ID. + * This register is used to verify the identity of the device (0b110100). + * @return Device ID (should be 0x68, 104 dec, 150 oct) + * @see MPU_RA_WHO_AM_I + * @see MPU_WHO_AM_I_BIT + * @see MPU_WHO_AM_I_LENGTH + */ +uint8_t mpu_device_id() +{ + return mpu_reg_read(MPU_RA_WHO_AM_I) >> MPU_WHO_AM_I_Pos; +} + +/** Set full-scale gyroscope range. + * @param range New full-scale gyroscope range value + * @see MPU_GetFullScaleGyroRange() + * @see MPU_GYRO_FS_250 + * @see MPU_RA_GYRO_CONFIG + * @see MPU_GCONFIG_FS_SEL_BIT + * @see MPU_GCONFIG_FS_SEL_LENGTH + */ +void mpu_set_gyro_fs(uint8_t range) +{ + mpu_reg_write(MPU_RA_GYRO_CONFIG, range); +} + + +/** Get full-scale gyroscope range. + * The FS_SEL parameter allows setting the full-scale range of the gyro sensors, + * as described in the table below. + * + * 
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ *
+ * + * @return Current full-scale gyroscope range setting + * @see MPU_GYRO_FS_250 + * @see MPU_RA_GYRO_CONFIG + * @see MPU_GCONFIG_FS_SEL_BIT + * @see MPU_GCONFIG_FS_SEL_LENGTH + */ +uint8_t mpu_get_gyro_fs() +{ + return mpu_reg_read(MPU_RA_GYRO_CONFIG) & MPU_GYRO_FS_SEL_Msk; +} + +/** Get full-scale accelerometer range. + * The FS_SEL parameter allows setting the full-scale range of the accelerometer + * sensors, as described in the table below. + * + * 
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ *
+ * + * @return Current full-scale accelerometer range setting + * @see MPU_ACCEL_FS_2 + * @see MPU_RA_ACCEL_CONFIG + */ +uint8_t mpu_get_accel_fs() +{ + return mpu_reg_read(MPU_RA_ACCEL_CONFIG) & MPU_ACCEL_CONFIG_FS_SEL_Msk; +} + +/** Set full-scale accelerometer range. + * @param range New full-scale accelerometer range setting + * @see MPU_GetFullScaleAccelRange() + */ +void mpu_set_accel_fs(uint8_t range) +{ + mpu_reg_write(MPU_RA_ACCEL_CONFIG, range); +} + +/** Get sleep mode status. + * Setting the SLEEP bit in the register puts the device into very low power + * sleep mode. In this mode, only the serial interface and internal registers + * remain active, allowing for a very low standby current. Clearing this bit + * puts the device back into normal mode. To save power, the individual standby + * selections for each of the gyros should be used if any gyro axis is not used + * by the application. + * @return Current sleep mode bit + * @see MPU_RA_PWR_MGMT_1 + * @see MPU_PWR1_SLEEP_BIT + */ +bool mpu_get_sleep_mode() +{ + return mpu_reg_read(MPU_RA_PWR_MGMT1) & MPU_PWR_MGMT1_SLEEP; +} + +/** Set sleep mode status. + * @param enabled New sleep mode enabled status + * @see MPU_GetSleepModeStatus() + * @see MPU_RA_PWR_MGMT_1 + * @see MPU_PWR1_SLEEP_BIT + */ +void mpu_set_sleep_mode(bool val) +{ + int tmp = mpu_reg_read(MPU_RA_PWR_MGMT1); + tmp = (tmp & ~MPU_PWR_MGMT1_SLEEP) | (val ? MPU_PWR_MGMT1_SLEEP : 0); + mpu_reg_write(MPU_RA_PWR_MGMT1, tmp); +} + +/** Get raw 6-axis motion sensor readings (accel/gyro). + * Retrieves all currently available motion sensor values. + * @see MPU_RA_ACCEL_XOUT_H + */ +void mpu_read_accel_gyro(struct mpu_raw_data *out) +{ + struct mpu_raw_data buf; + mpu_reg_read_multiple(MPU_RA_ACCEL_XOUT_H, (uint8_t *)&buf, sizeof(buf)); + + for (uint8_t i=0; ichannels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]); +} + +int16_t mpu_read_temp() { + uint16_t buf; + mpu_reg_read_multiple(MPU_RA_TEMP_OUT_H, (uint8_t *)&buf, sizeof(buf)); + return (int16_t)be16toh(buf); +} + +void mpu_read_accel(struct mpu_accel_data *out) +{ + struct mpu_accel_data buf; + mpu_reg_read_multiple(MPU_RA_ACCEL_XOUT_H, (uint8_t *)&buf, sizeof(buf)); + + for (uint8_t i=0; ichannels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]); +} + +void mpu_read_gyro(struct mpu_gyro_data *out) +{ + struct mpu_gyro_data buf; + mpu_reg_read_multiple(MPU_RA_GYRO_XOUT_H, (uint8_t *)&buf, sizeof(buf)); + + for (uint8_t i=0; ichannels[i] = (int16_t)be16toh((uint16_t)buf.channels[i]); +} + +/** + * @brief Writes one byte to the MPU6050. + * @param reg : address of the register in which the data will be written + * @param val : the data to be written to the MPU6050. + * @return None + */ +void mpu_reg_write(uint8_t reg, uint8_t val) +{ + uint8_t tx[2] = {reg, val}; + i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, tx, sizeof(tx), I2C_GENSTOP_YES); +} + +/** + * @brief Reads a block of data from the MPU6050. + * @param buf : pointer to the buffer that receives the data read from the MPU6050. + * @param addr : MPU6050's internal address to read from. + * @param len : number of bytes to read from the MPU6050 + * @return None + */ +void mpu_reg_read_multiple(uint8_t addr, uint8_t* buf, size_t len) +{ + i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &addr, 1, I2C_GENSTOP_NO); + i2c_receive(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, buf, len); +} + +uint8_t mpu_reg_read(uint8_t addr) +{ + uint8_t buf; + i2c_transmit(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &addr, 1, I2C_GENSTOP_NO); + i2c_receive(MPU_I2C_PERIPH, MPU_DEFAULT_ADDRESS, &buf, 1); + return buf; +} +/** + * @} + *//* end of group MPU_Library */ -- cgit