From 744c7ca5b90f4a73d2a9ff9431be6e85e02ae2aa Mon Sep 17 00:00:00 2001
From: JbLb <jerome@jblb.net>
Date: Sat, 8 Feb 2020 00:28:26 +0100
Subject: [PATCH] add serial receive code

---
 hoverserial.ino | 104 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 102 insertions(+), 2 deletions(-)

diff --git a/hoverserial.ino b/hoverserial.ino
index e3b6605..56febc4 100644
--- a/hoverserial.ino
+++ b/hoverserial.ino
@@ -2,6 +2,20 @@
 #include "splash-h.h"
 #include <Adafruit_GFX.h>
 #include <Adafruit_SSD1306.h>
+#include <Adafruit_NeoPixel.h>
+
+#define PIN PB_5 // Pin where NeoPixels are connected
+
+// Declare our NeoPixel strip object:
+Adafruit_NeoPixel strip(32, PIN, NEO_GRB + NEO_KHZ800);
+// Argument 1 = Number of pixels in NeoPixel strip
+// Argument 2 = Arduino pin number (most are valid)
+// Argument 3 = Pixel type flags, add together as needed:
+//   NEO_KHZ800  800 KHz bitstream (most NeoPixel products w/WS2812 LEDs)
+//   NEO_KHZ400  400 KHz (classic 'v1' (not v2) FLORA pixels, WS2811 drivers)
+//   NEO_GRB     Pixels are wired for GRB bitstream (most NeoPixel products)
+//   NEO_RGB     Pixels are wired for RGB bitstream (v1 FLORA pixels, not v2)
+//   NEO_RGBW    Pixels are wired for RGBW bitstream (NeoPixel RGBW products)
 
 #include <NintendoExtensionCtrl.h> // https://github.com/dmadison/NintendoExtensionCtrl
 
@@ -76,6 +90,9 @@ void setup()
 
   Serial.begin(115200);
   Serial1.begin(HOVER_SERIAL_BAUD);
+  
+  strip.begin(); // INITIALIZE NeoPixel strip object
+  strip.show();  // Turn OFF all pixels ASAP
 
   while (!Serial)
     ;
@@ -105,6 +122,90 @@ void Send(int16_t uSteer, int16_t uSpeed)
   // Write to Serial
   Serial1.write((uint8_t *)&Command, sizeof(Command));
 }
+// ########################## RECEIVE ##########################
+void Receive()
+
+{
+    int old_cursorX, old_cursorY;
+    // Check for new data availability in the Serial buffer
+    if (Serial1.available()) {
+        incomingByte = Serial1.read();  // Read the incoming byte
+        bufStartFrame = ((uint16_t) (incomingBytePrev) << 8) + incomingByte;    // Construct the start frame
+    } else {
+        return;
+    }
+
+    // If DEBUG_RX is defined print all incoming bytes
+#ifdef DEBUG_RX
+    Serial.print(incomingByte);
+    return;
+#endif
+
+    // Copy received data
+    if (bufStartFrame == START_FRAME) { // Initialize if new data is detected
+        p = (byte *) & NewFeedback;
+        *p++ = incomingBytePrev;
+        *p++ = incomingByte;
+        idx = 2;
+    } else if (idx >= 2 && idx < sizeof(SerialFeedback)) {      // Save the new received data
+        *p++ = incomingByte;
+        idx++;
+    }
+    // Check if we reached the end of the package
+    if (idx == sizeof(SerialFeedback)) {
+        uint16_t checksum;
+        checksum =
+            (uint16_t) (NewFeedback.start ^ NewFeedback.cmd1 ^ NewFeedback.
+                        cmd2 ^ NewFeedback.speedR ^ NewFeedback.
+                        speedL ^ NewFeedback.speedR_meas ^ NewFeedback.
+                        speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.
+                        boardTemp);
+
+        // Check validity of the new data
+        if (NewFeedback.start == START_FRAME
+            && checksum == NewFeedback.checksum) {
+            // Copy the new data
+            memcpy(&Feedback, &NewFeedback, sizeof(SerialFeedback));
+
+            // Print data to CDC Serial if available
+            if (Serial) {
+                Serial.print("1: ");
+                Serial.print(Feedback.cmd1);
+                Serial.print(" 2: ");
+                Serial.print(Feedback.cmd2);
+                Serial.print(" 3: ");
+                Serial.print(Feedback.speedR);
+                Serial.print(" 4: ");
+                Serial.print(Feedback.speedL);
+                Serial.print(" 5: ");
+                Serial.print(Feedback.speedR_meas);
+                Serial.print(" 6: ");
+                Serial.print(Feedback.speedL_meas);
+                Serial.print(" 7: ");
+                Serial.print(Feedback.batVoltage);
+                Serial.print(" 8: ");
+                Serial.println(Feedback.boardTemp);
+            }
+            display.setCursor(0, 30);
+            display.setTextSize(1);     // Draw 1X-scale text
+            display.setTextColor(SSD1306_WHITE);
+            display.print("V : ");
+            old_cursorX = display.getCursorX();
+            old_cursorY = display.getCursorY();
+            display.fillRect(old_cursorX, old_cursorY, (6 * 5), 8, SSD1306_BLACK);      // erase previous display
+            display.setCursor(old_cursorX, old_cursorY);
+            display.print(Feedback.batVoltage);
+            display.display();
+        } else {
+            if (Serial)
+                Serial.println("Non-valid data skipped");
+        }
+
+        idx = 0;                // Reset the index (it prevents to enter in this if condition in the next cycle)
+    }
+    // Update previous states
+    incomingBytePrev = incomingByte;
+}
 
 // ########################## Nunchuk_control ##########################
 
@@ -154,8 +255,7 @@ void loop()
   unsigned long timeNow = millis();
   // Blink the LED
   digitalWrite(LED_BUILTIN, (timeNow % 2000) < 1000);
-  // if (iTimeSend > timeNow)  return;
-  iTimeSend = timeNow + TIME_SEND;
+  Receive();
   Nunchuk_control();
   // Serial.println(timeNow);
 }