Nuvoton / Mbed OS NuMaker-mbed-I2C0_MPU6500_Tiltangle

NuMaker-PFM-NUC472: I2C0 read on-board MPU6500 and calculate tilt angle

mpu6500.cpp@6:911c09e70fe4, 14 months ago (annotated)

Committer:
cyliang
Date:
Wed Mar 01 03:29:49 2023 +0000
Revision:
6:911c09e70fe4
Parent:
4:cf21ad364a28 
Update OS v6.17.0 for M467 target

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rkuo2000 0:d1cab7bc0553 1 #include "mbed.h"
rkuo2000 0:d1cab7bc0553 2 #include "mpu6500.h"
rkuo2000 0:d1cab7bc0553 3
cyliang 4:cf21ad364a28 4 //I2C mpu6500_i2c(PC_9, PA_15); // I2C0_SDA, I2C0_SCL
cyliang 4:cf21ad364a28 5 extern I2C i2c0;
cyliang 4:cf21ad364a28 6 I2C *mpu6500_i2c;
rkuo2000 0:d1cab7bc0553 7
rkuo2000 0:d1cab7bc0553 8 void MPU6500_WriteByte(uint8_t MPU6500_reg, uint8_t MPU6500_data)
rkuo2000 0:d1cab7bc0553 9 {
rkuo2000 0:d1cab7bc0553 10 char data_out[2];
rkuo2000 0:d1cab7bc0553 11 data_out[0]=MPU6500_reg;
rkuo2000 0:d1cab7bc0553 12 data_out[1]=MPU6500_data;
cyliang 4:cf21ad364a28 13 mpu6500_i2c->write(MPU6500_slave_addr, data_out, 2, 0);
rkuo2000 0:d1cab7bc0553 14 }
rkuo2000 0:d1cab7bc0553 15
rkuo2000 0:d1cab7bc0553 16 uint8_t MPU6500_ReadByte(uint8_t MPU6500_reg)
rkuo2000 0:d1cab7bc0553 17 {
rkuo2000 0:d1cab7bc0553 18 char data_out[1], data_in[1];
rkuo2000 0:d1cab7bc0553 19 data_out[0] = MPU6500_reg;
cyliang 4:cf21ad364a28 20 mpu6500_i2c->write(MPU6500_slave_addr, data_out, 1, 1);
cyliang 4:cf21ad364a28 21 mpu6500_i2c->read(MPU6500_slave_addr, data_in, 1, 0);
rkuo2000 0:d1cab7bc0553 22 return (data_in[0]);
rkuo2000 0:d1cab7bc0553 23 }
rkuo2000 0:d1cab7bc0553 24
rkuo2000 0:d1cab7bc0553 25 void MPU6500::initialize()
rkuo2000 0:d1cab7bc0553 26 {
cyliang 4:cf21ad364a28 27 mpu6500_i2c = &i2c0;
rkuo2000 0:d1cab7bc0553 28 MPU6500_WriteByte(MPU6500_PWR_MGMT_1, 0x00); // CLK_SEL=0: internal 8MHz, TEMP_DIS=0, SLEEP=0
rkuo2000 0:d1cab7bc0553 29 MPU6500_WriteByte(MPU6500_SMPLRT_DIV, 0x07); // Gyro output sample rate = Gyro Output Rate/(1+SMPLRT_DIV)
rkuo2000 0:d1cab7bc0553 30 MPU6500_WriteByte(MPU6500_CONFIG, 0x06); // set TEMP_OUT_L, DLPF=2 (Fs=1KHz)
rkuo2000 0:d1cab7bc0553 31 MPU6500_WriteByte(MPU6500_GYRO_CONFIG, 0x18); // bit[4:3] 0=+-250d/s,1=+-500d/s,2=+-1000d/s,3=+-2000d/s
rkuo2000 0:d1cab7bc0553 32 MPU6500_WriteByte(MPU6500_ACCEL_CONFIG, 0x01);// bit[4:3] 0=+-2g,1=+-4g,2=+-8g,3=+-16g, ACC_HPF=On (5Hz)
rkuo2000 0:d1cab7bc0553 33 }
rkuo2000 0:d1cab7bc0553 34
rkuo2000 0:d1cab7bc0553 35 int16_t MPU6500::getAccelXvalue()
rkuo2000 0:d1cab7bc0553 36 {
rkuo2000 0:d1cab7bc0553 37 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 38 LoByte = MPU6500_ReadByte(MPU6500_ACCEL_XOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 39 HiByte = MPU6500_ReadByte(MPU6500_ACCEL_XOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 40 return((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 41 }
rkuo2000 0:d1cab7bc0553 42
rkuo2000 0:d1cab7bc0553 43 int16_t MPU6500::getAccelYvalue()
rkuo2000 0:d1cab7bc0553 44 {
rkuo2000 0:d1cab7bc0553 45 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 46 LoByte = MPU6500_ReadByte(MPU6500_ACCEL_YOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 47 HiByte = MPU6500_ReadByte(MPU6500_ACCEL_YOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 48 return ((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 49 }
rkuo2000 0:d1cab7bc0553 50
rkuo2000 0:d1cab7bc0553 51 int16_t MPU6500::getAccelZvalue()
rkuo2000 0:d1cab7bc0553 52 {
rkuo2000 0:d1cab7bc0553 53 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 54 LoByte = MPU6500_ReadByte(MPU6500_ACCEL_ZOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 55 HiByte = MPU6500_ReadByte(MPU6500_ACCEL_ZOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 56 return ((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 57 }
rkuo2000 0:d1cab7bc0553 58
rkuo2000 0:d1cab7bc0553 59 int16_t MPU6500::getGyroXvalue()
rkuo2000 0:d1cab7bc0553 60 {
rkuo2000 0:d1cab7bc0553 61 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 62 LoByte = MPU6500_ReadByte(MPU6500_GYRO_XOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 63 HiByte = MPU6500_ReadByte(MPU6500_GYRO_XOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 64 return ((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 65 }
rkuo2000 0:d1cab7bc0553 66
rkuo2000 0:d1cab7bc0553 67 int16_t MPU6500::getGyroYvalue()
rkuo2000 0:d1cab7bc0553 68 {
rkuo2000 0:d1cab7bc0553 69 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 70 LoByte = MPU6500_ReadByte(MPU6500_GYRO_YOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 71 HiByte = MPU6500_ReadByte(MPU6500_GYRO_YOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 72 return ((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 73 }
rkuo2000 0:d1cab7bc0553 74
rkuo2000 0:d1cab7bc0553 75 int16_t MPU6500::getGyroZvalue()
rkuo2000 0:d1cab7bc0553 76 {
rkuo2000 0:d1cab7bc0553 77 uint8_t LoByte, HiByte;
rkuo2000 0:d1cab7bc0553 78 LoByte = MPU6500_ReadByte(MPU6500_GYRO_ZOUT_L); // read Accelerometer X_Low value
rkuo2000 0:d1cab7bc0553 79 HiByte = MPU6500_ReadByte(MPU6500_GYRO_ZOUT_H); // read Accelerometer X_High value
rkuo2000 0:d1cab7bc0553 80 return ((HiByte<<8) | LoByte);
rkuo2000 0:d1cab7bc0553 81 }