[#133] WIP

parallaxinc · Dec 23, 2017 · 1b9e6d6 · 1b9e6d6
1 parent 4fae094
commit 1b9e6d6
Show file tree

Hide file tree

Showing 7 changed files with 1,338 additions and 239 deletions.
diff --git a/Examples/PropWare_BNO055/BNO055_Demo.cpp b/Examples/PropWare_BNO055/BNO055_Demo.cpp
@@ -24,9 +24,37 @@
  */
 
 #include <PropWare/sensor/fusion/bno055.h>
+#include <Simple-Libraries/Learn/Simple Libraries/Protocol/libsimplei2c/simplei2c.h>
 
 using PropWare::I2CMaster;
 using PropWare::BNO055;
+using PropWare::Pin;
+
+uint8_t get_byte (i2c *bus) {
+    i2c_start(bus);
+    i2c_writeByte(bus, BNO055::DEVICE_ADDRESS_A);
+    i2c_writeByte(bus, BNO055::convert(BNO055::Address::CHIP_ID));
+    const uint8_t result = static_cast<uint8_t>(i2c_readByte(bus, 1));
+    i2c_stop(bus);
+    return result;
+}
+
+PropWare::ErrorCode begin (i2c *bus) {
+    auto id = get_byte(bus);
+    if (id != BNO055::ID) {
+        waitcnt(SECOND + CNT); // hold on for boot
+        id = get_byte(bus);
+        if (id != BNO055::ID) {
+            pwOut << "fail 0\n";
+            return BNO055::FAILED_TO_BOOT;
+        }
+    }
+    return 0;
+}
+
+int ping (i2c *bus) {
+    return i2c_poll(bus, BNO055::DEVICE_ADDRESS_A);
+}
 
 /**
  * @example     BNO055_Demo.cpp
@@ -35,10 +63,53 @@ using PropWare::BNO055;
  * @include PropWare_BNO055/CMakeLists.txt
  */
 int main () {
-    const I2CMaster i2cBus;
-    const BNO055 sensor(i2cBus);
+    PropWare::ErrorCode error;
+    const Pin resetPin(PropWare::Port::P23, Pin::Dir::OUT);
+    waitcnt(100 * MILLISECOND + CNT);
 
+    i2c bus;
+    i2c_open(&bus, Pin::from_mask(I2CMaster::DEFAULT_SCL_MASK), Pin::from_mask(I2CMaster::DEFAULT_SDA_MASK), 0);
 
+    for (unsigned int   i = 0; i < 5; ++i) {
+        resetPin.clear();
+        waitcnt(MILLISECOND + CNT);
+        resetPin.set();
 
-    return 0;
+        while (ping(&bus))
+            waitcnt(10 * MILLISECOND + CNT);
+        int ack = i2c_writeByte(&bus, 0);
+        i2c_readByte(&bus, 0);
+        i2c_stop(&bus);
+        waitcnt(10 * MILLISECOND + CNT);
+
+        error = begin(&bus);
+        if (!error)
+            break;
+    }
+
+    //BNO055 sensor;
+    //for (int            i = 0; i < 5; ++i) {
+    //    resetPin.clear();
+    //    waitcnt(MILLISECOND + CNT);
+    //    resetPin.set();
+    //
+    //    while (!pwI2c.ping(BNO055::DEVICE_ADDRESS_A))
+    //        waitcnt(10 * MILLISECOND + CNT);
+    //    pwI2c.send_byte(0);
+    //    pwI2c.get(BNO055::DEVICE_ADDRESS_A, static_cast<uint8_t>(0));
+    //    pwI2c.stop();
+    //    waitcnt(10 * MILLISECOND + CNT);
+    //
+    //    error = sensor.begin();
+    //    if (!error)
+    //        break;
+    //}
+
+    if (error)
+        pwOut << "Error: " << error << '\n';
+    else
+        while (1) {
+            pwOut << "Temperature = " << /*sensor.get_temperature() <<*/ '\n';
+            waitcnt(SECOND + CNT);
+        }
 }
diff --git a/PropWare/CMakeLists.txt b/PropWare/CMakeLists.txt
@@ -55,6 +55,9 @@ set(PROPWARE_SOURCES
     ${CMAKE_CURRENT_LIST_DIR}/utility/comparator.cpp
     ${CMAKE_CURRENT_LIST_DIR}/utility/comparator.h
     ${CMAKE_CURRENT_LIST_DIR}/utility/utility.h
+    ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/matrix.h
+    ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/quaternion.h
+    ${CMAKE_CURRENT_LIST_DIR}/utility/ahrs/vector.h
     ${CMAKE_CURRENT_LIST_DIR}/c++allocate.h
     ${CMAKE_CURRENT_LIST_DIR}/PropWare.cpp
     ${CMAKE_CURRENT_LIST_DIR}/PropWare.h