src/lib/Telemetry/telemetry.cpp

   1 #include <cstdint>
   2 #include <cstring>
   3 #include "telemetry.h"
   4
   5 #if defined(UNIT_TEST)
   6 #include <iostream>
   7 using namespace std;
   8 #endif
   9
  10 #if CRSF_RX_MODULE
  11
  12 #include "CRSF.h"
  13
  14 Telemetry::Telemetry()
  15 {
  16     ResetState();
  17 }
  18
  19 bool Telemetry::ShouldCallBootloader()
  20 {
  21     bool bootloader = callBootloader;
  22     callBootloader = false;
  23     return bootloader;
  24 }
  25
  26 bool Telemetry::ShouldCallEnterBind()
  27 {
  28     bool enterBind = callEnterBind;
  29     callEnterBind = false;
  30     return enterBind;
  31 }
  32
  33 bool Telemetry::ShouldCallUpdateModelMatch()
  34 {
  35     bool updateModelMatch = callUpdateModelMatch;
  36     callUpdateModelMatch = false;
  37     return updateModelMatch;
  38 }
  39
  40 bool Telemetry::ShouldSendDeviceFrame()
  41 {
  42     bool deviceFrame = sendDeviceFrame;
  43     sendDeviceFrame = false;
  44     return deviceFrame;
  45 }
  46
  47
  48 PAYLOAD_DATA(GPS, BATTERY_SENSOR, ATTITUDE, DEVICE_INFO, FLIGHT_MODE, VARIO);
  49
  50 bool Telemetry::GetNextPayload(uint8_t* nextPayloadSize, uint8_t **payloadData)
  51 {
  52     uint8_t checks = 0;
  53     uint8_t oldPayloadIndex = currentPayloadIndex;
  54     uint8_t realLength = 0;
  55
  56     if (payloadTypes[currentPayloadIndex].locked)
  57     {
  58         payloadTypes[currentPayloadIndex].locked = false;
  59         payloadTypes[currentPayloadIndex].updated = false;
  60     }
  61
  62     do
  63     {
  64         currentPayloadIndex = (currentPayloadIndex + 1) % payloadTypesCount;
  65         checks++;
  66     } while(!payloadTypes[currentPayloadIndex].updated && checks < payloadTypesCount);
  67
  68     if (payloadTypes[currentPayloadIndex].updated)
  69     {
  70         payloadTypes[currentPayloadIndex].locked = true;
  71
  72         realLength = CRSF_FRAME_SIZE(payloadTypes[currentPayloadIndex].data[CRSF_TELEMETRY_LENGTH_INDEX]);
  73         // search for non zero data from the end
  74         while (realLength > 0 && payloadTypes[currentPayloadIndex].data[realLength - 1] == 0)
  75         {
  76             realLength--;
  77         }
  78
  79         if (realLength > 0)
  80         {
  81             // store real length in frame
  82             payloadTypes[currentPayloadIndex].data[CRSF_TELEMETRY_LENGTH_INDEX] = realLength - CRSF_FRAME_NOT_COUNTED_BYTES;
  83             *nextPayloadSize = realLength;
  84             *payloadData = payloadTypes[currentPayloadIndex].data;
  85             return true;
  86         }
  87     }
  88
  89     currentPayloadIndex = oldPayloadIndex;
  90     *nextPayloadSize = 0;
  91     *payloadData = 0;
  92     return false;
  93 }
  94
  95 uint8_t Telemetry::UpdatedPayloadCount()
  96 {
  97     uint8_t count = 0;
  98     for (int8_t i = 0; i < payloadTypesCount; i++)
  99     {
 100         if (payloadTypes[i].updated)
 101         {
 102             count++;
 103         }
 104     }
 105
 106     return count;
 107 }
 108
 109 uint8_t Telemetry::ReceivedPackagesCount()
 110 {
 111     return receivedPackages;
 112 }
 113
 114 void Telemetry::ResetState()
 115 {
 116     telemetry_state = TELEMETRY_IDLE;
 117     currentTelemetryByte = 0;
 118     currentPayloadIndex = 0;
 119     receivedPackages = 0;
 120
 121     uint8_t offset = 0;
 122
 123     for (int8_t i = 0; i < payloadTypesCount; i++)
 124     {
 125         payloadTypes[i].locked = false;
 126         payloadTypes[i].updated = false;
 127         payloadTypes[i].data = PayloadData + offset;
 128         offset += payloadTypes[i].size;
 129
 130         #if defined(UNIT_TEST)
 131         if (offset > sizeof(PayloadData)) {
 132             cout << "data not large enough\n";
 133         }
 134         #endif
 135     }
 136 }
 137
 138 bool Telemetry::RXhandleUARTin(uint8_t data)
 139 {
 140     switch(telemetry_state) {
 141         case TELEMETRY_IDLE:
 142             if (data == CRSF_ADDRESS_CRSF_RECEIVER || data == CRSF_SYNC_BYTE)
 143             {
 144                 currentTelemetryByte = 0;
 145                 telemetry_state = RECEIVING_LENGTH;
 146                 CRSFinBuffer[0] = data;
 147             }
 148             else {
 149                 return false;
 150             }
 151
 152             break;
 153         case RECEIVING_LENGTH:
 154             if (data >= CRSF_MAX_PACKET_LEN)
 155             {
 156                 telemetry_state = TELEMETRY_IDLE;
 157                 return false;
 158             }
 159             else
 160             {
 161                 telemetry_state = RECEIVING_DATA;
 162                 CRSFinBuffer[CRSF_TELEMETRY_LENGTH_INDEX] = data;
 163             }
 164
 165             break;
 166         case RECEIVING_DATA:
 167             CRSFinBuffer[currentTelemetryByte + CRSF_FRAME_NOT_COUNTED_BYTES] = data;
 168             currentTelemetryByte++;
 169             if (CRSFinBuffer[CRSF_TELEMETRY_LENGTH_INDEX] == currentTelemetryByte)
 170             {
 171                 // exclude first bytes (sync byte + length), skip last byte (submitted crc)
 172                 uint8_t crc = crsf_crc.calc(CRSFinBuffer + CRSF_FRAME_NOT_COUNTED_BYTES, CRSFinBuffer[CRSF_TELEMETRY_LENGTH_INDEX] - CRSF_TELEMETRY_CRC_LENGTH);
 173                 telemetry_state = TELEMETRY_IDLE;
 174
 175                 if (data == crc)
 176                 {
 177                     AppendTelemetryPackage(CRSFinBuffer);
 178                     receivedPackages++;
 179                     return true;
 180                 }
 181                 #if defined(UNIT_TEST)
 182                 if (data != crc)
 183                 {
 184                     cout << "invalid " << (int)crc  << '\n';
 185                 }
 186                 #endif
 187
 188                 return false;
 189             }
 190
 191             break;
 192     }
 193
 194     return true;
 195 }
 196
 197 bool Telemetry::AppendTelemetryPackage(uint8_t *package)
 198 {
 199     const crsf_header_t *header = (crsf_header_t *) package;
 200
 201     if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'b' && package[4] == 'l')
 202     {
 203         callBootloader = true;
 204         return true;
 205     }
 206     if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'b' && package[4] == 'd')
 207     {
 208         callEnterBind = true;
 209         return true;
 210     }
 211     if (header->type == CRSF_FRAMETYPE_COMMAND && package[3] == 'm' && package[4] == 'm')
 212     {
 213         callUpdateModelMatch = true;
 214         modelMatchId = package[5];
 215         return true;
 216     }
 217     if (header->type == CRSF_FRAMETYPE_DEVICE_PING && package[CRSF_TELEMETRY_TYPE_INDEX + 1] == CRSF_ADDRESS_CRSF_RECEIVER)
 218     {
 219         sendDeviceFrame = true;
 220         return true;
 221     }
 222
 223     uint8_t targetIndex = 0;
 224     bool targetFound = false;
 225
 226
 227     if (header->type >= CRSF_FRAMETYPE_DEVICE_PING)
 228     {
 229         const crsf_ext_header_t *extHeader = (crsf_ext_header_t *) package;
 230
 231         if (header->type == CRSF_FRAMETYPE_ARDUPILOT_RESP)
 232         {
 233             // reserve last slot for adrupilot custom frame with the sub type status text: this is needed to make sure the important status messages are not lost
 234             if (package[CRSF_TELEMETRY_TYPE_INDEX + 1] == CRSF_AP_CUSTOM_TELEM_STATUS_TEXT)
 235             {
 236                 targetIndex = payloadTypesCount - 1;
 237             }
 238             else
 239             {
 240                 targetIndex = payloadTypesCount - 2;
 241             }
 242             targetFound = true;
 243         }
 244         else if (extHeader->orig_addr == CRSF_ADDRESS_FLIGHT_CONTROLLER)
 245         {
 246             targetIndex = payloadTypesCount - 2;
 247             targetFound = true;
 248
 249             // larger msp resonses are sent in two chunks so special handling is needed so both get sent
 250             if (header->type == CRSF_FRAMETYPE_MSP_RESP)
 251             {
 252                 // there is already another response stored
 253                 if (payloadTypes[targetIndex].updated)
 254                 {
 255                     // use other slot
 256                     targetIndex = payloadTypesCount - 1;
 257                 }
 258
 259                 // if both slots are taked do not overwrite other data since the first chunk would be lost
 260                 if (payloadTypes[targetIndex].updated)
 261                 {
 262                     targetFound = false;
 263                 }
 264             }
 265         }
 266         else
 267         {
 268             targetIndex = payloadTypesCount - 1;
 269             targetFound = true;
 270         }
 271     }
 272     else
 273     {
 274         for (int8_t i = 0; i < payloadTypesCount - 2; i++)
 275         {
 276             if (header->type == payloadTypes[i].type)
 277             {
 278                 if (!payloadTypes[i].locked && CRSF_FRAME_SIZE(package[CRSF_TELEMETRY_LENGTH_INDEX]) <= payloadTypes[i].size)
 279                 {
 280                     targetIndex = i;
 281                     targetFound = true;
 282                 }
 283                 #if defined(UNIT_TEST)
 284                 else if (CRSF_FRAME_SIZE(package[CRSF_TELEMETRY_LENGTH_INDEX]) > payloadTypes[i].size)
 285                 {
 286                     cout << "buffer not large enough for type " << (int)payloadTypes[i].type  << " with size " << (int)payloadTypes[i].size << " would need " << CRSF_FRAME_SIZE(package[CRSF_TELEMETRY_LENGTH_INDEX]) << '\n';
 287                 }
 288                 #endif
 289                 break;
 290             }
 291         }
 292     }
 293
 294     if (targetFound)
 295     {
 296         memcpy(payloadTypes[targetIndex].data, package, CRSF_FRAME_SIZE(package[CRSF_TELEMETRY_LENGTH_INDEX]));
 297         payloadTypes[targetIndex].updated = true;
 298     }
 299
 300     return targetFound;
 301 }
 302 #endif