Zoltan Hudak / Mbed OS MPU6050_Hello

Example of using the MPU6050 library.

Dependencies:   MPU6050

main.cpp@1:598008f00b35, 2021-01-18 (annotated)

Committer:
hudakz
Date:
Mon Jan 18 19:58:43 2021 +0000
Revision:
1:598008f00b35
Parent:
0:5bba454d6e5a 
Example of using the MCPU6050 library.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
hudakz 0:5bba454d6e5a 1 #include "mbed.h"
hudakz 0:5bba454d6e5a 2 #include "MPU6050.h"
hudakz 0:5bba454d6e5a 3
hudakz 0:5bba454d6e5a 4 class MySerial :
hudakz 0:5bba454d6e5a 5 public SerialBase
hudakz 0:5bba454d6e5a 6 {
hudakz 0:5bba454d6e5a 7 public:
hudakz 0:5bba454d6e5a 8 MySerial(PinName txPin, PinName rxPin, int baud) :
hudakz 0:5bba454d6e5a 9 SerialBase(txPin, rxPin, baud)
hudakz 0:5bba454d6e5a 10 { }
hudakz 0:5bba454d6e5a 11
hudakz 0:5bba454d6e5a 12 using SerialBase::_base_putc;
hudakz 0:5bba454d6e5a 13 };
hudakz 0:5bba454d6e5a 14
hudakz 0:5bba454d6e5a 15 MySerial serial(USBTX, USBRX, 460800);
hudakz 0:5bba454d6e5a 16 MPU6050 mpu6050(0x68, AFS_2G, GFS_250DPS, I2C_SDA, I2C_SCL, NC);
hudakz 0:5bba454d6e5a 17 Timer mpu6050Timer;
hudakz 0:5bba454d6e5a 18 Ticker dispEventTicker;
hudakz 0:5bba454d6e5a 19 volatile bool dispEvent = false;
hudakz 0:5bba454d6e5a 20
hudakz 0:5bba454d6e5a 21 /**
hudakz 0:5bba454d6e5a 22 * @brief
hudakz 0:5bba454d6e5a 23 * @note
hudakz 0:5bba454d6e5a 24 * @param
hudakz 0:5bba454d6e5a 25 * @retval
hudakz 0:5bba454d6e5a 26 */
hudakz 0:5bba454d6e5a 27 void dispEventTick()
hudakz 0:5bba454d6e5a 28 {
hudakz 0:5bba454d6e5a 29 dispEvent = true;
hudakz 0:5bba454d6e5a 30 }
hudakz 0:5bba454d6e5a 31
hudakz 0:5bba454d6e5a 32 /**
hudakz 0:5bba454d6e5a 33 * @brief
hudakz 0:5bba454d6e5a 34 * @note
hudakz 0:5bba454d6e5a 35 * @param
hudakz 0:5bba454d6e5a 36 * @retval
hudakz 0:5bba454d6e5a 37 */
hudakz 1:598008f00b35 38 void print(int16_t val)
hudakz 0:5bba454d6e5a 39 {
hudakz 0:5bba454d6e5a 40 if (val < 0) {
hudakz 0:5bba454d6e5a 41 serial._base_putc('-');
hudakz 0:5bba454d6e5a 42 val = -val;
hudakz 0:5bba454d6e5a 43 }
hudakz 0:5bba454d6e5a 44
hudakz 0:5bba454d6e5a 45 uint16_t denom;
hudakz 0:5bba454d6e5a 46 uint8_t digit;
hudakz 0:5bba454d6e5a 47 uint8_t iter;
hudakz 0:5bba454d6e5a 48
hudakz 0:5bba454d6e5a 49 if (val == 0) {
hudakz 0:5bba454d6e5a 50 serial._base_putc('0');
hudakz 0:5bba454d6e5a 51 return;
hudakz 0:5bba454d6e5a 52 }
hudakz 0:5bba454d6e5a 53
hudakz 0:5bba454d6e5a 54 if (val < 10) {
hudakz 0:5bba454d6e5a 55 iter = 1;
hudakz 0:5bba454d6e5a 56 denom = 10;
hudakz 0:5bba454d6e5a 57 }
hudakz 0:5bba454d6e5a 58 else if (val < 100) {
hudakz 0:5bba454d6e5a 59 iter = 2;
hudakz 0:5bba454d6e5a 60 denom = 100;
hudakz 0:5bba454d6e5a 61 }
hudakz 0:5bba454d6e5a 62 else {
hudakz 0:5bba454d6e5a 63 iter = 3;
hudakz 0:5bba454d6e5a 64 denom = 1000;
hudakz 0:5bba454d6e5a 65 }
hudakz 0:5bba454d6e5a 66
hudakz 0:5bba454d6e5a 67 for (int i = 0; i < iter; i++) {
hudakz 0:5bba454d6e5a 68 denom = denom / 10;
hudakz 0:5bba454d6e5a 69 digit = val / denom;
hudakz 0:5bba454d6e5a 70 val = val % denom;
hudakz 0:5bba454d6e5a 71 serial._base_putc(digit + '0');
hudakz 0:5bba454d6e5a 72 }
hudakz 0:5bba454d6e5a 73 }
hudakz 0:5bba454d6e5a 74
hudakz 0:5bba454d6e5a 75 /**
hudakz 0:5bba454d6e5a 76 * @brief
hudakz 0:5bba454d6e5a 77 * @note
hudakz 0:5bba454d6e5a 78 * @param
hudakz 0:5bba454d6e5a 79 * @retval
hudakz 0:5bba454d6e5a 80 */
hudakz 0:5bba454d6e5a 81 int main()
hudakz 0:5bba454d6e5a 82 {
hudakz 0:5bba454d6e5a 83 uint32_t mpu6050LastUpdate = 0;
hudakz 0:5bba454d6e5a 84 bool mpu6050GainAdjusted = false;
hudakz 0:5bba454d6e5a 85 uint32_t now;
hudakz 0:5bba454d6e5a 86 float deltaT;
hudakz 0:5bba454d6e5a 87
hudakz 0:5bba454d6e5a 88 printf("Starting...\r\n");
hudakz 0:5bba454d6e5a 89
hudakz 0:5bba454d6e5a 90 mpu6050.init();
hudakz 0:5bba454d6e5a 91 mpu6050Timer.reset();
hudakz 0:5bba454d6e5a 92 mpu6050Timer.start();
hudakz 0:5bba454d6e5a 93 #if MBED_MAJOR_VERSION > 5
hudakz 0:5bba454d6e5a 94 dispEventTicker.attach(dispEventTick, 2s);
hudakz 0:5bba454d6e5a 95 #else
hudakz 0:5bba454d6e5a 96 dispEventTicker.attach(dispEventTick, 2.0f);
hudakz 0:5bba454d6e5a 97 #endif
hudakz 0:5bba454d6e5a 98
hudakz 0:5bba454d6e5a 99 while (true) {
hudakz 0:5bba454d6e5a 100 if (mpu6050.dataReady()) {
hudakz 0:5bba454d6e5a 101 mpu6050.accel();
hudakz 0:5bba454d6e5a 102 mpu6050.gyro();
hudakz 0:5bba454d6e5a 103 now = mpu6050Timer.read_us();
hudakz 0:5bba454d6e5a 104 mpu6050Timer.reset();
hudakz 0:5bba454d6e5a 105 deltaT = float(now) * 1e-6; // integration time in seconds
hudakz 0:5bba454d6e5a 106 mpu6050.madgwickFilter(deltaT);
hudakz 0:5bba454d6e5a 107 }
hudakz 0:5bba454d6e5a 108
hudakz 0:5bba454d6e5a 109 if (dispEvent) {
hudakz 0:5bba454d6e5a 110 dispEvent = false;
hudakz 0:5bba454d6e5a 111
hudakz 0:5bba454d6e5a 112 int16_t yaw = mpu6050.yaw();
hudakz 0:5bba454d6e5a 113 int16_t pitch = mpu6050.pitch();
hudakz 0:5bba454d6e5a 114 int16_t roll = mpu6050.roll();
hudakz 0:5bba454d6e5a 115
hudakz 0:5bba454d6e5a 116 print(yaw);
hudakz 0:5bba454d6e5a 117 serial._base_putc(',');
hudakz 0:5bba454d6e5a 118 print(pitch);
hudakz 0:5bba454d6e5a 119 serial._base_putc(',');
hudakz 0:5bba454d6e5a 120 print(roll);
hudakz 0:5bba454d6e5a 121 serial._base_putc('\r');
hudakz 0:5bba454d6e5a 122 serial._base_putc('\n');
hudakz 0:5bba454d6e5a 123 }
hudakz 0:5bba454d6e5a 124 }
hudakz 0:5bba454d6e5a 125 }