From bba36191b10401e5cdfd9eb1e3b056dd140e77dc Mon Sep 17 00:00:00 2001
From: Eiren Rain <eirenliel@users.noreply.github.com>
Date: Mon, 5 Apr 2021 09:22:25 +0300
Subject: [PATCH] Minor cleanup, added tap packet

---
 lib/bno080/BNO080.cpp |  8 ++++----
 src/bno080sensor.cpp  |  2 +-
 src/defines.h         |  2 +-
 src/main.cpp          |  2 +-
 src/sensor.h          |  1 +
 src/udpclient.cpp     | 20 ++++++++++++++++++++
 src/udpclient.h       |  2 ++
 7 files changed, 30 insertions(+), 7 deletions(-)

diff --git a/lib/bno080/BNO080.cpp b/lib/bno080/BNO080.cpp
index 9a69362..62e2787 100644
--- a/lib/bno080/BNO080.cpp
+++ b/lib/bno080/BNO080.cpp
@@ -214,10 +214,10 @@ uint16_t BNO080::getReadings(void)
 		{
 			return parseCommandReport(); //This will update responses to commands, calibrationStatus, etc.
 		}
-    else if(shtpHeader[2] == CHANNEL_GYRO)
-    {
-      return parseInputReport(); //This will update the rawAccelX, etc variables depending on which feature report is found
-    }
+		else if(shtpHeader[2] == CHANNEL_GYRO)
+		{
+		return parseInputReport(); //This will update the rawAccelX, etc variables depending on which feature report is found
+		}
 	}
 	return 0;
 }
diff --git a/src/bno080sensor.cpp b/src/bno080sensor.cpp
index e43259f..6e594ab 100644
--- a/src/bno080sensor.cpp
+++ b/src/bno080sensor.cpp
@@ -72,7 +72,7 @@ void BNO080Sensor::motionSetup(DeviceConfig * config)
 void BNO080Sensor::motionLoop()
 {
     //Look for reports from the IMU
-    if (imu.dataAvailable() == true)
+    if (imu.dataAvailable())
     {
         quaternion.x = imu.getQuatI();
         quaternion.y = imu.getQuatJ();
diff --git a/src/defines.h b/src/defines.h
index 546cc2b..8b13746 100644
--- a/src/defines.h
+++ b/src/defines.h
@@ -29,7 +29,7 @@
 #define IMU_BNO085 4
 #define IMU_BNO055 5
 
-#define IMU IMU_BNO080
+#define IMU IMU_BNO085
 
 #if IMU == IMU_BNO085
   #define IMU_NAME "BNO085"
diff --git a/src/main.cpp b/src/main.cpp
index e1077f0..92b9972 100644
--- a/src/main.cpp
+++ b/src/main.cpp
@@ -84,7 +84,7 @@ void setup()
     // join I2C bus
     Wire.flush();
     Wire.begin(D2, D1);
-    Wire.setClockStretchLimit(4000);
+    Wire.setClockStretchLimit(1000);
     Serial.begin(serialBaudRate);
     while (!Serial)
         ; // wait for connection
diff --git a/src/sensor.h b/src/sensor.h
index 86bed1c..e5801ed 100644
--- a/src/sensor.h
+++ b/src/sensor.h
@@ -56,6 +56,7 @@ class BNO080Sensor : public Sensor {
         BNO080 imu {};
         bool newData {false};
         Quat sensorOffset {Quat(Vector3(0, 0, 1), PI / 2.0)};
+        uint8_t tap;
 };
 
 class BNO055Sensor : public Sensor {
diff --git a/src/udpclient.cpp b/src/udpclient.cpp
index 54fc7dd..99a408d 100644
--- a/src/udpclient.cpp
+++ b/src/udpclient.cpp
@@ -146,6 +146,26 @@ void sendFloat(float const value, int type)
     }
 }
 
+void sendByte(char const value, int type)
+{
+    if (Udp.beginPacket(host, port) > 0)
+    {
+        sendType(type);
+        sendPacketNumber();
+        Udp.write(&value, 1);
+        if (Udp.endPacket() == 0)
+        {
+            //Serial.print("Write error: ");
+            //Serial.println(Udp.getWriteError());
+        }
+    }
+    else
+    {
+        //Serial.print("Write error: ");
+        //Serial.println(Udp.getWriteError());
+    }
+}
+
 void sendQuat(Quat *const quaternion, int type)
 {
     if (Udp.beginPacket(host, port) > 0)
diff --git a/src/udpclient.h b/src/udpclient.h
index c5ac6db..6e4a7d1 100644
--- a/src/udpclient.h
+++ b/src/udpclient.h
@@ -42,6 +42,7 @@
 #define PACKET_PING_PONG 10
 #define PACKET_SERIAL 11
 #define PACKET_BATTERY_LEVEL 12
+#define PACKET_TAP 13
 
 #define PACKET_RECIEVE_HEARTBEAT 1
 #define PACKET_RECIEVE_VIBRATE 2
@@ -62,6 +63,7 @@ void sendQuat(Quat * const quaternion, int type);
 void sendVector(float * const result, int type);
 void sendConfig(DeviceConfig * const config, int type);
 void sendFloat(float const value, int type);
+void sendByte(char const value, int type);
 void sendRawCalibrationData(int * const data, int type);
 void setConfigRecievedCallback(configRecievedCallback);
 void setCommandRecievedCallback(commandRecievedCallback);