From 7991d1bb80abbbd91ed06e9f5dd2aed30f93b57c Mon Sep 17 00:00:00 2001 From: Lukas Bachschwell Date: Sat, 11 May 2024 15:32:04 +0200 Subject: [PATCH] Add hoverusart Signed-off-by: Lukas Bachschwell --- src/hoverusart.h | 161 +++++++++++++++++++++++++++++++++++++++++++++ src/valuehelpers.h | 19 ++++++ 2 files changed, 180 insertions(+) create mode 100644 src/hoverusart.h create mode 100644 src/valuehelpers.h diff --git a/src/hoverusart.h b/src/hoverusart.h new file mode 100644 index 0000000..7640805 --- /dev/null +++ b/src/hoverusart.h @@ -0,0 +1,161 @@ +// ******************************************************************* +// Arduino Nano 5V example code +// for https://github.com/EmanuelFeru/hoverboard-firmware-hack-FOC +// +// Copyright (C) 2019-2020 Emanuel FERU +// +// ******************************************************************* +// INFO: +// • This sketch uses the the Serial Software interface to communicate and send commands to the hoverboard +// • The built-in (HW) Serial interface is used for debugging and visualization. In case the debugging is not needed, +// it is recommended to use the built-in Serial interface for full speed perfomace. +// • The data packaging includes a Start Frame, checksum, and re-syncronization capability for reliable communication +// +// The code starts with zero speed and moves towards + +// +// CONFIGURATION on the hoverboard side in config.h: +// • Option 1: Serial on Right Sensor cable (short wired cable) - recommended, since the USART3 pins are 5V tolerant. +// #define CONTROL_SERIAL_USART3 +// #define FEEDBACK_SERIAL_USART3 +// // #define DEBUG_SERIAL_USART3 +// • Option 2: Serial on Left Sensor cable (long wired cable) - use only with 3.3V devices! The USART2 pins are not 5V tolerant! +// #define CONTROL_SERIAL_USART2 +// #define FEEDBACK_SERIAL_USART2 +// // #define DEBUG_SERIAL_USART2 +// ******************************************************************* + +#include "Arduino.h" + +// ########################## DEFINES ########################## +#define HOVER_SERIAL_BAUD 115200 // [-] Baud rate for HoverSerial (used to communicate with the hoverboard) +#define SERIAL_BAUD 115200 // [-] Baud rate for built-in Serial (used for the Serial Monitor) +#define START_FRAME 0xABCD // [-] Start frme definition for reliable serial communication +// #define DEBUG_RX // [-] Debug received data. Prints all bytes to serial (comment-out to disable) + +#define HoverSerial Serial2 +// GPIOS: 16:RXD 17:TXD + +// Global variables +uint8_t idx = 0; // Index for new data pointer +uint16_t bufStartFrame; // Buffer Start Frame +byte *p; // Pointer declaration for the new received data +byte incomingByte; +byte incomingBytePrev; + +typedef struct +{ + uint16_t start; + int16_t steer; + int16_t speed; + uint16_t checksum; +} SerialCommand; +SerialCommand Command; + +typedef struct +{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t batVoltage; + int16_t boardTemp; + uint16_t cmdLed; + uint16_t checksum; +} SerialFeedback; +SerialFeedback Feedback; +SerialFeedback NewFeedback; + +// ########################## SETUP ########################## +void initHoverSerial() +{ + HoverSerial.begin(HOVER_SERIAL_BAUD); +} + +// ########################## SEND ########################## +void Send(int16_t uSteer, int16_t uSpeed) +{ + if (uSteer == 0 || uSpeed == 0) + { + Serial.printf("Sending %d %d\n", uSteer, uSpeed); + } + // Create command + Command.start = (uint16_t)START_FRAME; + Command.steer = (int16_t)uSteer; + Command.speed = (int16_t)uSpeed; + Command.checksum = (uint16_t)(Command.start ^ Command.steer ^ Command.speed); + + // Write to Serial + HoverSerial.write((uint8_t *)&Command, sizeof(Command)); +} + +// ########################## RECEIVE ########################## +void Receive() +{ + // Check for new data availability in the Serial buffer + if (!HoverSerial.available()) + { + return; + } + + incomingByte = HoverSerial.read(); // Read the incoming byte + bufStartFrame = ((uint16_t)(incomingByte) << 8) | incomingBytePrev; // Construct the start frame + +// 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_meas ^ NewFeedback.speedL_meas ^ NewFeedback.batVoltage ^ NewFeedback.boardTemp ^ NewFeedback.cmdLed); + + // 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 built-in Serial + Serial.print("1: "); + Serial.print(Feedback.cmd1); + Serial.print(" 2: "); + Serial.print(Feedback.cmd2); + Serial.print(" 3: "); + Serial.print(Feedback.speedR_meas); + Serial.print(" 4: "); + Serial.print(Feedback.speedL_meas); + Serial.print(" 5: "); + Serial.print(Feedback.batVoltage); + Serial.print(" 6: "); + Serial.print(Feedback.boardTemp); + Serial.print(" 7: "); + Serial.println(Feedback.cmdLed); + } + else + { + 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; +} diff --git a/src/valuehelpers.h b/src/valuehelpers.h new file mode 100644 index 0000000..85da788 --- /dev/null +++ b/src/valuehelpers.h @@ -0,0 +1,19 @@ +#include + +// Value Helpers +int16_t make16(uint8_t u1, uint8_t u2) +{ + return ((int16_t)u1 << 8) | u2; +} + +uint8_t split16(uint16_t value, uint8_t index) +{ + switch (index) + { + case 0: + return (value >> 8); + case 1: + return value & 0xff; + } + return 0; +}