From f6f936da4ec9ce95bf66068f90ea38696c50ab7c Mon Sep 17 00:00:00 2001 From: Fisch Date: Tue, 3 Dec 2019 23:59:59 +0100 Subject: [PATCH] update to emanuelferu firmware --- .gitmodules | 3 + controller/controller.ino | 127 +++++++++++++++++++++++- hoverboard-firmware-hack-foc-serial-esc | 1 + hoverboard-firmware-hack-serial-esc | 2 +- 4 files changed, 128 insertions(+), 5 deletions(-) create mode 160000 hoverboard-firmware-hack-foc-serial-esc diff --git a/.gitmodules b/.gitmodules index 270eec3..ae910cb 100644 --- a/.gitmodules +++ b/.gitmodules @@ -7,3 +7,6 @@ [submodule "nippleremote"] path = nippleremote url = git@repos.ctdo.de:interfisch/nippleremote.git +[submodule "hoverboard-firmware-hack-foc-serial-esc"] + path = hoverboard-firmware-hack-foc-serial-esc + url = https://repos.ctdo.de/interfisch/hoverboard-firmware-hack-foc-serial-esc diff --git a/controller/controller.ino b/controller/controller.ino index a271198..82609b9 100644 --- a/controller/controller.ino +++ b/controller/controller.ino @@ -9,6 +9,10 @@ //PA2 may be defective on my bluepill +#define SERIAL_CONTROL_BAUD 38400 // [-] 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 0xAAAA // [-] Start frme definition for reliable serial communication + //#define DEBUG #define PARAMETEROUTPUT uint8_t error = 0; @@ -156,11 +160,46 @@ uint8_t controlmode=0; #define MODE_RADIONRF 1 #define MODE_GAMETRAK 2 + + +// 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 speedLeft; + int16_t speedRight; + uint16_t checksum; +} SerialCommand; +SerialCommand Command; + +typedef struct{ + uint16_t start; + int16_t cmd1; + int16_t cmd2; + int16_t speedR; + int16_t speedL; + int16_t speedR_meas; + int16_t speedL_meas; + int16_t batVoltage; + int16_t boardTemp; + int16_t checksum; +} SerialFeedback; +SerialFeedback Feedback; +SerialFeedback NewFeedback; + + + + void setup() { - Serial.begin(115200); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) + Serial.begin(SERIAL_BAUD); //Debug and Program. A9=TX1, A10=RX1 (3v3 level) - Serial2.begin(19200); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) + Serial2.begin(SERIAL_CONTROL_BAUD); //control. B10=TX3, B11=RX3 (Serial2 is Usart 3) //Serial1 max be dead on my board? @@ -208,6 +247,8 @@ void setup() { void loop() { + ReceiveSerial2(); // Check for new received data + if (millis() - last_imuupdated > IMUUPDATEPERIOD) { updateIMU(); last_imuupdated = millis(); @@ -445,9 +486,14 @@ void loop() { out_checksum = 0; //checksum=0 disables motors } - Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl)); + /*Serial2.write((uint8_t *) &out_speedl, sizeof(out_speedl)); Serial2.write((uint8_t *) &out_speedr, sizeof(out_speedr)); - Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum)); + Serial2.write((uint8_t *) &out_checksum, sizeof(out_checksum));*/ + if (motorenabled) { //motors enabled + SendSerial2(out_speedl,out_speedr); + } else { //motors disabled + SendSerial2(0,0); + } lastsend_out_speedl = out_speedl; //remember last transmittet values (for stat sending) lastsend_out_speedr = out_speedr; last_send = millis(); @@ -560,3 +606,76 @@ void updateIMU() Yaw: around Z axis, CCW positive, 0 to 360 */ } + + +// ########################## SEND ########################## +void SendSerial2(int16_t uSpeedLeft, int16_t uSpeedRight) +{ + // Create command + Command.start = (uint16_t)START_FRAME; + Command.speedLeft = (int16_t)uSpeedLeft; + Command.speedRight = (int16_t)uSpeedRight; + Command.checksum = (uint16_t)(Command.start ^ Command.speedLeft ^ Command.speedRight); + + // Write to Serial + Serial2.write((uint8_t *) &Command, sizeof(Command)); +} + +// ########################## RECEIVE ########################## +void ReceiveSerial2() +{ + // Check for new data availability in the Serial buffer + if (Serial2.available()) { + incomingByte = Serial2.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 built-in 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); + } 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/hoverboard-firmware-hack-foc-serial-esc b/hoverboard-firmware-hack-foc-serial-esc new file mode 160000 index 0000000..ddeb7fb --- /dev/null +++ b/hoverboard-firmware-hack-foc-serial-esc @@ -0,0 +1 @@ +Subproject commit ddeb7fbd18b95842228bfe39b9cbcad1e655101b diff --git a/hoverboard-firmware-hack-serial-esc b/hoverboard-firmware-hack-serial-esc index 2f19aa1..9c84e97 160000 --- a/hoverboard-firmware-hack-serial-esc +++ b/hoverboard-firmware-hack-serial-esc @@ -1 +1 @@ -Subproject commit 2f19aa1de9969c9c2b2c1156ee9ea14738f9cabd +Subproject commit 9c84e97a58f6027a3b8fdfdcfeaf64985e2327ff