From 87955d2dfce021e2d5ecc7dec71fa14b180d16f9 Mon Sep 17 00:00:00 2001
From: Pim van Pelt <pim@ipng.nl>
Date: Fri, 4 Jan 2019 16:31:20 +0100
Subject: [PATCH] Add a test for IMU

---
 tests/imu.c | 49 +++++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 49 insertions(+)
 create mode 100644 tests/imu.c

diff --git a/tests/imu.c b/tests/imu.c
new file mode 100644
index 0000000..693a9f8
--- /dev/null
+++ b/tests/imu.c
@@ -0,0 +1,49 @@
+#include "mgos.h"
+#include "tests_autogen.h"
+#include "mgos_imu.h"
+
+uint32_t test_imu_period_ms = 100; // 10Hz
+bool test_imu_enabled = true;
+
+static struct mgos_imu *s_imu = NULL;
+
+bool test_imu_create(void) {
+  if (!(s_imu = mgos_imu_create())) {
+    LOG(LL_ERROR, ("Cannot create IMU"));
+    return false;
+  }
+  
+  if (!mgos_imu_accelerometer_create_i2c(s_imu, mgos_i2c_get_global(), 0x1d, ACC_LSM303D)) {
+    LOG(LL_ERROR, ("Cannot create accelerometer on IMU"));
+  }
+  if (!mgos_imu_gyroscope_create_i2c(s_imu, mgos_i2c_get_global(), 0x6b, GYRO_L3GD20)) {
+    LOG(LL_ERROR, ("Cannot create gyroscope on IMU"));
+  }
+  if (!mgos_imu_magnetometer_create_i2c(s_imu, mgos_i2c_get_global(), 0x1d, MAG_LSM303D)) {
+    LOG(LL_ERROR, ("Cannot create magnetometer on IMU"));
+  }
+
+  return true;
+}
+
+bool test_imu_run(void) {
+  float ax, ay, az;
+  float gx, gy, gz;
+  float mx, my, mz;
+
+  if (mgos_imu_accelerometer_get(s_imu, &ax, &ay, &az))
+    LOG(LL_INFO, ("type=%-10s Accel X=%+.2f Y=%+.2f Z=%+.2f (m/s/s)",
+                  mgos_imu_accelerometer_get_name(s_imu), ax, ay, az));
+  if (mgos_imu_gyroscope_get(s_imu, &gx, &gy, &gz))
+    LOG(LL_INFO, ("type=%-10s Gyro  X=%+.2f Y=%+.2f Z=%+.2f (Rad/sec)",
+                  mgos_imu_gyroscope_get_name(s_imu), gx, gy, gz));
+  if (mgos_imu_magnetometer_get(s_imu, &mx, &my, &mz))
+    LOG(LL_INFO, ("type=%-10s Mag   X=%+.2f Y=%+.2f Z=%+.2f (uTesla)",
+                  mgos_imu_magnetometer_get_name(s_imu), mx, my, mz));
+  return true;
+}
+
+bool test_imu_destroy(void) {
+  mgos_imu_destroy(&s_imu);
+  return true;
+}