EscTelemetry/EscTelemetry.cpp at main · eubotx/EscTelemetry · 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
#include <Arduino.h>
#include "EscTelemetry.h"

EscTelemetry::EscTelemetry(HardwareSerial *pHardSerial)
{
    _pHardSerial = pHardSerial;
};

//
// crc8 calculation
//
uint8_t EscTelemetry::updateCrc8( uint8_t crc, uint8_t crc_seed )
{
  static uint8_t crc_u;

  crc_u = crc;
  crc_u ^= crc_seed;

  for ( int i = 0; i < 8; i++ ) {
    crc_u = ( crc_u & 0x80 ) ? 0x7 ^ ( crc_u << 1 ) : ( crc_u << 1 );
  }

  return crc_u;
}

uint8_t EscTelemetry::crc8( uint8_t* buf, uint8_t buflen )
{
  static uint8_t crc;

  crc = 0;

  for ( int i = 0; i < buflen; i++ ) {
    crc = updateCrc8( buf[i], crc );
  }

  return crc;
}

void EscTelemetry::extractTelemetryData(int i)
{
    switch (i)
    {
    case 0:
        temperature = telemetryBuffer[0];
        break;
    case 2:
        voltage = ( telemetryBuffer[1] << 8 ) | telemetryBuffer[2];
        break;
    case 4:
        current = ( telemetryBuffer[3] << 8 ) | telemetryBuffer[4];
        break;
    case 6:
        consumption = ( telemetryBuffer[5] << 8 ) | telemetryBuffer[6];
    case 8:
        rpm = ( telemetryBuffer[7] << 8 ) | telemetryBuffer[8];
        break;
    case 9:
        valid = ( telemetryBuffer[9] == crc8( &telemetryBuffer[9], ESC_TELEMETRY_LENGTH - 1 ) );

        if (!valid)
        {
            do {
            _pHardSerial->clear( );
            delayMicroseconds(TLM_BYTE_TIME * 2);
            } while (_pHardSerial->available( ));

            lostPackageCounter++;
            i = 0;
        }

        break;
    default:
        break;
    }
}

void EscTelemetry::updateEscTelemetry()
{

  // Read all bytes in rx buffer up to packet length
  while (_pHardSerial->available() > 0) {
    telemetryBuffer[i] = _pHardSerial->read( );   // read next Byte

    extractTelemetryData(i);

    if (i < ESC_TELEMETRY_LENGTH) i++;
    else i = 0;
  }
}

void EscTelemetry::printTelemetry()
{
      Serial.print("Temp");
      Serial.print(":");
      Serial.print(temperature);
      Serial.print(",");
      Serial.print("Volt");
      Serial.print(":");
      Serial.print(voltage);
      Serial.print(",");
      Serial.print("Amps");
      Serial.print(":");
      Serial.print(current);
      Serial.print(",");
      Serial.print("mAh");
      Serial.print(":");
      Serial.print(consumption);
      Serial.print(",");
      Serial.print("rpm");
      Serial.print(":");
      Serial.print(rpm);
      Serial.print(",");
      Serial.print("valid");
      Serial.print(":");
      Serial.println(valid);
}