hoverboard-firmware-hack/hoverserial.ino at master · Nie-Oh/hoverboard-firmware-hack · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
//  Arduino Pro Mini 3.3V example code
//  for   https://github.com/p-h-a-i-l/hoverboard-firmware-hack
//  visit https://pionierland.de/hoverhack/ to compile your firmware online :-)

//#define DEBUG_RX
#include <SoftwareSerial.h>
SoftwareSerial oSerial(2,3); // RX, TX

typedef struct{
   int16_t steer;
   int16_t speed;
   uint32_t crc;
} Serialcommand;
Serialcommand oCmd;

typedef struct{
   int16_t iSpeedL; // 100* km/h
   int16_t iSpeedR; // 100* km/h
   uint16_t iHallSkippedL;
   uint16_t iHallSkippedR;
   uint16_t iTemp;  // °C
   uint16_t iVolt;  // 100* V
   int16_t iAmpL;  // 100* A
   int16_t iAmpR;  // 100* A
   uint32_t crc;
} SerialFeedback;
SerialFeedback oFeedback;

void setup()
{
  Serial.begin(115200);
  Serial.println("Hoverhack Test v1.0");

  oSerial.begin(9600);
  pinMode(LED_BUILTIN, OUTPUT);
}

uint32_t crc32_for_byte(uint32_t r)
{
  for(int j = 0; j < 8; ++j)
    r = (r & 1? 0: (uint32_t)0xEDB88320L) ^ r >> 1;
  return r ^ (uint32_t)0xFF000000L;
}

void crc32(const void *data, size_t n_bytes, uint32_t* crc) {
  static uint32_t table[0x100];
  if(!*table)
    for(size_t i = 0; i < 0x100; ++i)
      table[i] = crc32_for_byte(i);
  for(size_t i = 0; i < n_bytes; ++i)
    *crc = table[(uint8_t)*crc ^ ((uint8_t*)data)[i]] ^ *crc >> 8;
}


void Send(int16_t iSpeed)
{
  oCmd.steer = 0;
  oCmd.speed = iSpeed;

  uint32_t crc = 0;
  crc32((const void *)&oCmd, sizeof(Serialcommand)-4,   &crc);
  oCmd.crc = crc;

  oSerial.write((uint8_t *) &oCmd, sizeof(oCmd));
}

int iFailedRec = 0;
boolean Receive()
{
  //while (oSerial.available()) {Serial.print(" ");Serial.print(oSerial.read(),HEX);}return false;

  if (oSerial.available()<  sizeof(SerialFeedback))
    return false;

  SerialFeedback oNew;
  byte* p = (byte*)&oNew;
  for (unsigned int i=0; i < sizeof(SerialFeedback); i++)
    *p++ = oSerial.read();;

  uint32_t crc = 0;
  crc32((const void *)&oNew, sizeof(SerialFeedback)-4,   &crc);

#ifdef DEBUG_RX
  char sBuff[10];
  p = (byte*)&oNew;
  for (unsigned int i=0; i < sizeof(SerialFeedback); i++)
  {
    sprintf(sBuff," %02x",p[i]);
    Serial.print(sBuff);
  }
  Serial.print(" ?= ");Serial.println(crc,HEX);
#endif

  if (oNew.crc == crc)
  {
    memcpy(&oFeedback,&oNew,sizeof(SerialFeedback));
    return true;
  }

#ifdef DEBUG_RX
  while (oSerial.available()) {Serial.print(" ");Serial.print(oSerial.read(),HEX);}Serial.println("\t:-(");
#else
  while (oSerial.available()) oSerial.read();   // empty garbage
  Serial.print("X");
  iFailedRec++;
#endif
  return false;
}

#define TIME_SEND 200
int iTest = 1000;
unsigned long iTimeSend = 0;

void loop(void)
{
  unsigned long iNow = millis();
  if (Receive())
  {
    if (iFailedRec)
      Serial.println();
    iFailedRec = 0;
    Serial.print("speedL: ");Serial.print(-0.01*(float)oFeedback.iSpeedL);
    Serial.print("\tspeedR: ");Serial.print(-0.01*(float)oFeedback.iSpeedR);
    Serial.print("\tskippedL: ");Serial.print(oFeedback.iHallSkippedL);
    Serial.print("\tskippedR: ");Serial.print(oFeedback.iHallSkippedR);
    Serial.print("\t°C: ");Serial.print(oFeedback.iTemp);
    Serial.print("\tU: ");Serial.print(0.01 * (float)oFeedback.iVolt);
    Serial.print("\tlA: ");Serial.print(0.01 * (float)oFeedback.iAmpL);
    Serial.print("\trA: ");Serial.println(0.01 * (float)oFeedback.iAmpR);
  }

  if (iTimeSend > iNow) return;

  iTimeSend = iNow + TIME_SEND;
  Send(abs(iTest)-1000);

  iTest+= 20;
  if (iTest>2000) iTest=-2000;

  digitalWrite(LED_BUILTIN, (iNow%2000)<1000);
}