Skip to content

Commit 60c24b9

Browse files
fix scale test
1 parent 9a24fb2 commit 60c24b9

File tree

5 files changed

+28
-13
lines changed

5 files changed

+28
-13
lines changed

i2c/mpu6050_i2c/mpu6050.cpp

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -27,15 +27,22 @@ void MPU6050::reset(void) {
2727
void MPU6050::setscale_accel(MPU6050::Scale scale) {
2828
mpu6050_setbusaddr(bus_addr);
2929
mpu6050_setscale_accel((MPU6050_Scale)scale);
30+
accel_scale = scale;
3031
}
3132

3233
void MPU6050::setscale_gyro(MPU6050::Scale scale) {
3334
mpu6050_setbusaddr(bus_addr);
3435
mpu6050_setscale_gyro((MPU6050_Scale)scale);
36+
gyro_scale = scale;
3537
}
3638

3739
void MPU6050::read(void) {
3840
mpu6050_setbusaddr(bus_addr);
3941
mpu6050_read(accel, gyro, &temp, (MPU6050_Scale)accel_scale, (MPU6050_Scale)gyro_scale);
4042
}
43+
44+
bool MPU6050::is_connected() {
45+
mpu6050_setbusaddr(bus_addr);
46+
return mpu6050_is_connected();
47+
}
4148

i2c/mpu6050_i2c/mpu6050.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -19,9 +19,11 @@ class MPU6050 {
1919
void reset(void);
2020

2121
enum Scale {Scale_0 = 0, Scale_1, Scale_2, Scale_3};
22+
// Warning: The first call to read() after setscale() might not have the updated scaling.
2223
void setscale_accel(Scale scale); //scale 0-3 is 2g, 4g, 8g, or 16g
2324
void setscale_gyro(Scale scale); // scale 0-3 is 250, 500, 1000, or 2000 deg/s
2425
void read(void);
26+
bool is_connected(void);
2527
private:
2628
float *accel, *gyro, temp;
2729
enum Scale accel_scale, gyro_scale;

i2c/mpu6050_i2c/mpu6050_i2c.c

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -142,7 +142,7 @@ void mpu6050_accel_selftest_on(void) {
142142
}
143143

144144
bool mpu6050_is_connected(void) {
145-
uint8_t who_are_you;
145+
uint8_t who_are_you = 0;
146146
mpu6050_readreg(REG_WHO_AM_I, &who_are_you, 1);
147147
return who_are_you == 0x68;
148148
}
@@ -192,7 +192,7 @@ int run_MPU6050_demo() {
192192
printf("Raw Gyro. X = %d, Y = %d, Z = %d\n", gyro_raw[0], gyro_raw[1], gyro_raw[2]);
193193
// This is chip temperature.
194194
printf("Temp [C] = %f\n", mpu6050_scale_temp(temp_raw));
195-
printf("Read time: %d us; \t\t Accel and Gyro Scale = %d\n", time_us, testscale);
195+
printf("Read time: %llu us; \t\t Accel and Gyro Scale = %d\n", time_us, (int)testscale);
196196
mpu6050_scale_accel(acceleration, accel_raw, testscale);
197197
mpu6050_scale_gyro_rad(gyro, gyro_raw, testscale);
198198
printf("Acc [m/s^2] X = %f, Y = %f, Z = %f\n", acceleration[0], acceleration[1], acceleration[2]);

i2c/mpu6050_i2c/mpu6050_i2c.h

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -31,7 +31,7 @@ bool mpu6050_is_connected();
3131
void mpu6050_accel_selftest_on(); // TODO: find out what "self test" does
3232
// turn on the mpu6050's gyroscope self test. Turn it off after a few 100ms with mpu6050_setscale_gyro
3333
void mpu6050_gyro_selftest_on();
34-
//set and use scaling
34+
//set and use scaling. The first read() after setscale() might not have the updated scaling.
3535
void mpu6050_setscale_accel(MPU6050_Scale accel_scale);
3636
void mpu6050_setscale_gyro(MPU6050_Scale gyro_scale);
3737
void mpu6050_scale_accel(float accel[3], int16_t accel_raw[3], MPU6050_Scale scale); //sets accel[3] in m/s^2 from accel_raw[3]
Lines changed: 16 additions & 10 deletions
Original file line numberDiff line numberDiff line change
@@ -1,32 +1,38 @@
11
/**
22
* mpu6050 I2c C++ example demonstrating the mpu6050's scaling feature
3+
* Scaled Accelerometer and Gyroscope readings
4+
* Should be roughly equal before and after scale changes
35
*/
46

57
#include <stdio.h>
68
#include "mpu6050.hpp"
79
#include "pico/stdlib.h"
810

11+
const int READ_DELAY_MS = 100, READS_PER_SCALE = 50, PRINTS_PER_SCALE = 2;
12+
913
int main() {
10-
float accel[3], gyro[3]; // TODO we shouldn't need this
14+
stdio_init_all();
15+
float accel[3] = {0}, gyro[3] = {0}; // TODO we shouldn't need this
1116
MPU6050 *IMU = new MPU6050(accel, gyro);
12-
printf("MPU6050 Scaling test\n");
1317
IMU->reset();
1418
IMU->power(1, false, false, false);
15-
printf("Scaled Accelerometer and Gyroscope readings\n");
16-
printf("Should be roughly equal before and after scale changes\n");
1719
int accel_scale = 0, gyro_scale = 2;
1820
while (true) {
19-
IMU->setscale_accel((MPU6050::Scale)(accel_scale));
20-
IMU->setscale_gyro((MPU6050::Scale)(gyro_scale));
21-
printf("\nAcclerometer scale: %i, Gyroscope scale: %i\n", accel_scale, gyro_scale);
22-
for (int i = 0; i < 100; i++) {
21+
while (!IMU->is_connected()) {
22+
printf("MPU6050 is not connected...\n");
23+
sleep_ms(1000);
24+
}
25+
printf("\nAccelerometer scale: %d, Gyroscope scale: %d\n", accel_scale, gyro_scale);
26+
for (int i = 0; i < READS_PER_SCALE; i++) {
2327
IMU->read();
2428
printf("%+6f %+6f %+6f | %+6f %+6f %+6f\r",
2529
accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2]);
26-
if (i == 0) { printf("\n"); }
27-
sleep_ms(10);
30+
if (i < PRINTS_PER_SCALE - 1) { printf("\n"); }
31+
sleep_ms(READ_DELAY_MS);
2832
}
2933
accel_scale = (accel_scale + 1) % 4;
3034
gyro_scale = (gyro_scale + 1) % 4;
35+
IMU->setscale_accel((MPU6050::Scale)(accel_scale));
36+
IMU->setscale_gyro((MPU6050::Scale)(gyro_scale));
3137
}
3238
}

0 commit comments

Comments
 (0)