Diff: main.cpp

Revision:

0:5bba454d6e5a

Child:

1:598008f00b35

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Mon Jan 18 19:47:17 2021 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include "MPU6050.h"
+
+class MySerial :
+    public SerialBase
+{
+public:
+    MySerial(PinName txPin, PinName rxPin, int baud) :
+    SerialBase(txPin, rxPin, baud)
+    { }
+
+    using SerialBase::_base_putc;
+};
+
+MySerial        serial(USBTX, USBRX, 460800);
+MPU6050         mpu6050(0x68, AFS_2G, GFS_250DPS, I2C_SDA, I2C_SCL, NC);
+Timer           mpu6050Timer;
+Ticker          dispEventTicker;
+volatile bool   dispEvent = false;
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void dispEventTick()
+{
+    dispEvent = true;
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+void print(int8_t val)
+{
+    if (val < 0) {
+        serial._base_putc('-');
+        val = -val;
+    }
+
+    uint16_t denom;
+    uint8_t  digit;
+    uint8_t  iter;
+
+    if (val == 0) {
+        serial._base_putc('0');
+        return;
+    }
+
+    if (val < 10) {
+        iter = 1;
+        denom = 10;
+    }
+    else if (val < 100) {
+        iter = 2;
+        denom = 100;
+    }
+    else {
+        iter = 3;
+        denom = 1000;
+    }
+
+    for (int i = 0; i < iter; i++) {
+        denom = denom / 10;
+        digit = val / denom;
+        val = val % denom;
+        serial._base_putc(digit + '0');
+    }
+}
+
+/**
+ * @brief
+ * @note
+ * @param
+ * @retval
+ */
+int main()
+{
+    uint32_t    mpu6050LastUpdate = 0;
+    bool        mpu6050GainAdjusted = false;
+    uint32_t    now;
+    float       deltaT;
+
+    printf("Starting...\r\n");
+
+    mpu6050.init();
+    mpu6050Timer.reset();
+    mpu6050Timer.start();
+#if MBED_MAJOR_VERSION > 5
+    dispEventTicker.attach(dispEventTick, 2s);
+#else
+    dispEventTicker.attach(dispEventTick, 2.0f);
+#endif
+
+    while (true) {
+        if (mpu6050.dataReady()) {
+            mpu6050.accel();
+            mpu6050.gyro();
+            now = mpu6050Timer.read_us();
+            mpu6050Timer.reset();
+            deltaT = float(now) * 1e-6;   // integration time in seconds
+            mpu6050.madgwickFilter(deltaT);
+        }
+
+        if (dispEvent) {
+            dispEvent = false;
+
+            int16_t  yaw = mpu6050.yaw();
+            int16_t  pitch = mpu6050.pitch();
+            int16_t  roll = mpu6050.roll();
+
+            print(yaw);
+            serial._base_putc(',');
+            print(pitch);
+            serial._base_putc(',');
+            print(roll);
+            serial._base_putc('\r');
+            serial._base_putc('\n');
+        }
+    }
+}