Driver

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "lib/pico-sdk"]
+	path = lib/pico-sdk
+	url = https://github.com/raspberrypi/pico-sdk.git

CMakeLists.txt

@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.13)
+
+set(CMAKE_BUILD_TYPE Debug)
+set(PICO_BOARD pico2_w)
+
+include(${CMAKE_CURRENT_LIST_DIR}/lib/pico-sdk/pico_sdk_init.cmake)
+
+project(MPU6050_Gyro_Test)
+pico_sdk_init()
+
+add_executable(gyro_test
+    gyro.c
+    mpu6050.c
+)
+
+target_link_libraries(gyro_test
+    pico_stdlib
+    hardware_i2c
+)
+
+pico_add_extra_outputs(gyro_test)
+

gyro.c

@@ -0,0 +1,63 @@
+#include <stdio.h>
+#include "pico/stdlib.h"
+#include "hardware/i2c.h"
+#include "mpu6050.h"
+
+/*
+ * Gyro Full Scale	/ Sensitivity
+ * 0 => 250			131
+ * 1 => 500			65.5
+ * 2 => 1000			32.8
+ * 3 => 2000 deg/s		16.4 LSB/(deg/s)
+ *
+ * Accel Full Scale	/ Sensitivity
+ * 0 => 2			16384
+ * 1 => 4			8192
+ * 2 => 8			4096
+ * 3 => 16g			2048 LSB/g
+ */
+
+
+int main(){
+
+
+	stdio_init_all();
+
+	int init = mpu6050_init(i2c0,
+			4,	/* SDA pin # */
+			5,	/* SCL pin # */
+			1,	/* gyro_fs */
+			1,	/* accel_fs */
+			100000);/* baudrate */
+
+	if(init != 0){
+		printf("Error: MPU6050 initialization failed\n");
+		while(true){
+			tight_loop_contents();
+		}
+	}
+
+	printf("MPU6050 initialized\n");
+
+	int16_t gx, gy, gz;
+	float x, y, z;
+
+	while(true){
+		int read = mpu6050_read_gyro(&gx, &gy, &gz);
+		if(read != 0){
+			printf("Error: Failed to read gyro\n");
+		}
+		else{
+			x = gx / 65.5f;
+			y = gy / 65.5f;
+			z = gz / 65.5f;
+
+			printf("Gyro (deg/s): X = %.2f, Y = %.2f, Z = %.2f\n", x, y, z);
+		}
+
+		sleep_ms(100);
+	}
+
+
+	return 0;
+}

mpu6050.c

@@ -0,0 +1,129 @@
+#include "mpu6050.h"
+#include <stdio.h>
+
+#include "hardware/i2c.h"
+#include "pico/stdlib.h"
+
+#define MPU6050_I2C_ADDR   0x68
+#define MPU6050_PWR_MGMT_1 0x6B
+
+#define MPU6050_GYRO_CONFIG  0x1B
+#define MPU6050_ACCEL_CONFIG 0x1C
+
+#define MPU6050_GYRO_XOUT  0x43
+#define MPU6050_ACCEL_XOUT 0x3B
+
+
+/* store i2c instance for read and write */
+static i2c_inst_t *mpu6050_i2c = NULL;
+
+int mpu6050_init(i2c_inst_t *i2c,
+	uint		      sda,
+	uint		      scl,
+	uint8_t		      gyro_fs,
+	uint8_t		      accel_fs,
+	uint		      baudrate)
+{
+	mpu6050_i2c = i2c;
+
+
+	i2c_init(mpu6050_i2c, baudrate);
+	gpio_set_function(sda, GPIO_FUNC_I2C);
+	gpio_set_function(scl, GPIO_FUNC_I2C);
+	gpio_pull_up(sda);
+	gpio_pull_up(scl);
+
+
+	/* wake up */
+	uint8_t buffer[2];
+	buffer[0] = MPU6050_PWR_MGMT_1;
+	buffer[1] = 0x00;  // clear sleeping bit
+	int ret	  = i2c_write_blocking(
+		  mpu6050_i2c, MPU6050_I2C_ADDR, buffer, 2, false);
+	if (ret != 2) {
+		printf("Error: Failed to wake up. Expected 2 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+
+	/* full scale config */
+	uint8_t gyro_config_buf[2];
+	gyro_config_buf[0] = MPU6050_GYRO_CONFIG;
+	gyro_config_buf[1] = gyro_fs << 3; /* uses bits 4,3 */
+	ret		   = i2c_write_blocking(
+		       mpu6050_i2c, MPU6050_I2C_ADDR, gyro_config_buf, 2, false);
+	if (ret != 2) {
+		printf("Error: Failed to configure gyro. Expected 2 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+
+	uint8_t accel_config_buf[2];
+	accel_config_buf[0] = MPU6050_ACCEL_CONFIG;
+	accel_config_buf[1] = accel_fs << 3;
+	ret		    = i2c_write_blocking(
+		mpu6050_i2c, MPU6050_I2C_ADDR, accel_config_buf, 2, false);
+	if (ret != 2) {
+		printf("Error: Failed to configure accel. Expected 2 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+	return 0;
+}
+
+int mpu6050_read_gyro(int16_t *gx, int16_t *gy, int16_t *gz)
+{
+	uint8_t gyro_reg = MPU6050_GYRO_XOUT;
+	uint8_t data[6];
+
+	int ret = i2c_write_blocking(
+		mpu6050_i2c, MPU6050_I2C_ADDR, &gyro_reg, 1, true);
+	if (ret != 1) {
+		printf("Error: Failed to write gyro address. Expected 1 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+	ret = i2c_read_blocking(mpu6050_i2c, MPU6050_I2C_ADDR, data, 6, false);
+	if (ret != 6) {
+		printf("Error: Failed to read gyro data. Expected 6 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+	*gx = (int16_t) ((data[0] << 8) | data[1]);
+	*gy = (int16_t) ((data[2] << 8) | data[3]);
+	*gz = (int16_t) ((data[4] << 8) | data[5]);
+
+	return 0;
+}
+
+int mpu6050_read_accel(int16_t *ax, int16_t *ay, int16_t *az)
+{
+	uint8_t accel_reg = MPU6050_ACCEL_XOUT;
+	uint8_t data[6];
+
+	int ret = i2c_write_blocking(
+		mpu6050_i2c, MPU6050_I2C_ADDR, &accel_reg, 1, true);
+	if (ret != 1) {
+		printf("Error: Failed to write accel address. Expected 1 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+	ret = i2c_read_blocking(mpu6050_i2c, MPU6050_I2C_ADDR, data, 6, false);
+	if (ret != 6) {
+		printf("Error: Failed to read accel data. Expected 6 bytes, got %d.\n",
+			ret);
+		return -1;
+	}
+
+	*ax = (int16_t) ((data[0] << 8) | data[1]);
+	*ay = (int16_t) ((data[2] << 8) | data[3]);
+	*az = (int16_t) ((data[4] << 8) | data[5]);
+
+	return 0;
+}

mpu6050.h

@@ -0,0 +1,20 @@
+#ifndef MPU6050_H
+#define MPU6050_H
+
+#include "hardware/i2c.h"
+
+#include <stdint.h>
+
+
+int mpu6050_init(i2c_inst_t *i2c,
+	uint		      sda,
+	uint		      scl,
+	uint8_t		      gyro_fs,
+	uint8_t		      accel_fs,
+	uint		      baudrate);
+
+int mpu6050_read_gyro(int16_t *gx, int16_t *gy, int16_t *gz);
+
+int mpu6050_read_accel(int16_t *ax, int16_t *ay, int16_t *az);
+
+#endif /* MPU6050_H */