Initial fw commit

1 files changed, 244 insertions, 0 deletions

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.
+ *
+ * <pre>
+ * 0 = +/- 250 degrees/sec
+ * 1 = +/- 500 degrees/sec
+ * 2 = +/- 1000 degrees/sec
+ * 3 = +/- 2000 degrees/sec
+ * </pre>
+ *
+ * @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.
+ *
+ * <pre>
+ * 0 = +/- 2g
+ * 1 = +/- 4g
+ * 2 = +/- 8g
+ * 3 = +/- 16g
+ * </pre>
+ *
+ * @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; i<ARRAY_SIZE(buf.channels); i++)
+        out->channels[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; i<ARRAY_SIZE(buf.channels); i++)
+        out->channels[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; i<ARRAY_SIZE(buf.channels); i++)
+        out->channels[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 */