Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Driver #1

Open
wants to merge 22 commits into
base: main
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from 7 commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 3 additions & 0 deletions .gitmodules
Original file line number Diff line number Diff line change
@@ -0,0 +1,3 @@
[submodule "lib/pico-sdk"]
path = lib/pico-sdk
url = https://github.com/raspberrypi/pico-sdk.git

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
path = lib/pico-sdk
url = https://github.com/raspberrypi/pico-sdk.git
path = lib/pico-sdk
url = ../../raspberrypi/pico-sdk.git

You can do relative URLs so that this works with either HTTPS or SSH.

22 changes: 22 additions & 0 deletions CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -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)

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change

63 changes: 63 additions & 0 deletions gyro.c
Original file line number Diff line number Diff line change
@@ -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(){


Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
int main(){
int main(void)
{

Let's try to cut down on unnecessary whitespace.

stdio_init_all();

int init = mpu6050_init(i2c0,
4, /* SDA pin # */
5, /* SCL pin # */
1, /* gyro_fs */
1, /* accel_fs */
100000);/* baudrate */

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Instead of adding comments for these literals, it's better to #define constants with descriptive names so that you can immediately read off what's going on and not depend on the comments. More importantly, what happens when you want to reference any of these constant values later in your program? Right now you have magic numbers floating around.


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;

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

These constant scaling factors should be a #define in mpu6050.h.


printf("Gyro (deg/s): X = %.2f, Y = %.2f, Z = %.2f\n", x, y, z);
}

sleep_ms(100);
}


return 0;
}
1 change: 1 addition & 0 deletions lib/pico-sdk

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It's better to pin the pico-sdk to a specific release tag than whatever was the latest commit at the time you added the submodule.

Submodule pico-sdk added at ee68c7
129 changes: 129 additions & 0 deletions mpu6050.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,129 @@
#include "mpu6050.h"
#include <stdio.h>

#include "hardware/i2c.h"
#include "pico/stdlib.h"

#define MPU6050_I2C_ADDR 0x68

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
#define MPU6050_I2C_ADDR 0x68
#define MPU6050_I2C_DEFAULT_ADDRESS 0x68

I believe you can change the I2C address of the mpu6050.

#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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm under the belief that these should go into mpu6050.h.



/* store i2c instance for read and write */
static i2c_inst_t *mpu6050_i2c = NULL;

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

NO!

You've now tied the entire library to one I2C bus and mpu6050! We should instead pass around an "mpu6050" handle object between calls so that we can reuse this for multiple sensors and/or busses.


int mpu6050_init(i2c_inst_t *i2c,

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You should break up each of the individual init phases to smaller static functions to make it easier to read the init function. Static functions will be inlined with any modern compiler optimization enabled so there's really no downside to using them to encapsulate functionality.

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

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can clearing the sleeping bit also be defined as a constant? I'm not too familiar with the buffer format and if there are other configuration bits.

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 */

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Not saying you have to do this here, but just as an aside, you can define a bit-mask for a register sub-field, and use __builtin_ctzg() to count the number of trailing zero bits in an integer, giving you the implicitly encoded bit-shift of a field!

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)

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'd personally make a struct for each of these values since they repeat, or even an array of three elements, though this latter approach would require you to define some constants as to make it easier to understand which element you're accessing.

{
uint8_t gyro_reg = MPU6050_GYRO_XOUT;

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
uint8_t gyro_reg = MPU6050_GYRO_XOUT;
const uint8_t gyro_reg = MPU6050_GYRO_XOUT;

Try to use constants where possible. (This is but one instance in this code.)

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;
}
20 changes: 20 additions & 0 deletions mpu6050.h
Original file line number Diff line number Diff line change
@@ -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 */