src/lib/Handset/CRSF.cpp

   1 #include "CRSF.h"
   2
   3 #include "common.h"
   4 #include "FIFO.h"
   5
   6 elrsLinkStatistics_t CRSF::LinkStatistics;
   7 GENERIC_CRC8 crsf_crc(CRSF_CRC_POLY);
   8
   9 uint8_t CRSF::MspData[ELRS_MSP_BUFFER] = {0};
  10 uint8_t CRSF::MspDataLength = 0;
  11
  12 static const auto MSP_SERIAL_OUT_FIFO_SIZE = 256U;
  13 static FIFO<MSP_SERIAL_OUT_FIFO_SIZE> MspWriteFIFO;
  14
  15
  16 /***
  17  * @brief: Convert `version` (string) to a integer version representation
  18  * e.g. "2.2.15 ISM24G" => 0x0002020f
  19  * Assumes all version fields are < 256, the number portion
  20  * MUST be followed by a space to correctly be parsed
  21  ***/
  22 uint32_t CRSF::VersionStrToU32(const char *verStr)
  23 {
  24     uint32_t retVal = 0;
  25 #if !defined(FORCE_NO_DEVICE_VERSION)
  26     uint8_t accumulator = 0;
  27     char c;
  28     bool trailing_data = false;
  29     while ((c = *verStr))
  30     {
  31         ++verStr;
  32         // A decimal indicates moving to a new version field
  33         if (c == '.')
  34         {
  35             retVal = (retVal << 8) | accumulator;
  36             accumulator = 0;
  37             trailing_data = false;
  38         }
  39         // Else if this is a number add it up
  40         else if (c >= '0' && c <= '9')
  41         {
  42             accumulator = (accumulator * 10) + (c - '0');
  43             trailing_data = true;
  44         }
  45         // Anything except [0-9.] ends the parsing
  46         else
  47         {
  48             break;
  49         }
  50     }
  51     if (trailing_data)
  52     {
  53         retVal = (retVal << 8) | accumulator;
  54     }
  55     // If the version ID is < 1.0.0, we could not parse it,
  56     // just use the OTA version as the major version number
  57     if (retVal < 0x010000)
  58     {
  59         retVal = OTA_VERSION_ID << 16;
  60     }
  61 #endif
  62     return retVal;
  63 }
  64
  65 void CRSF::GetDeviceInformation(uint8_t *frame, uint8_t fieldCount)
  66 {
  67     const uint8_t size = strlen(device_name)+1;
  68     auto *device = (deviceInformationPacket_t *)(frame + sizeof(crsf_ext_header_t) + size);
  69     // Packet starts with device name
  70     memcpy(frame + sizeof(crsf_ext_header_t), device_name, size);
  71     // Followed by the device
  72     device->serialNo = htobe32(0x454C5253); // ['E', 'L', 'R', 'S'], seen [0x00, 0x0a, 0xe7, 0xc6] // "Serial 177-714694" (value is 714694)
  73     device->hardwareVer = 0; // unused currently by us, seen [ 0x00, 0x0b, 0x10, 0x01 ] // "Hardware: V 1.01" / "Bootloader: V 3.06"
  74     device->softwareVer = htobe32(VersionStrToU32(version)); // seen [ 0x00, 0x00, 0x05, 0x0f ] // "Firmware: V 5.15"
  75     device->fieldCnt = fieldCount;
  76     device->parameterVersion = 0;
  77 }
  78
  79 void CRSF::SetMspV2Request(uint8_t *frame, uint16_t function, uint8_t *payload, uint8_t payloadLength)
  80 {
  81     auto *packet = (uint8_t *)(frame + sizeof(crsf_ext_header_t));
  82     packet[0] = 0x50;          // no error, version 2, beginning of the frame, first frame (0)
  83     packet[1] = 0;             // flags
  84     packet[2] = function & 0xFF;
  85     packet[3] = (function >> 8) & 0xFF;
  86     packet[4] = payloadLength & 0xFF;
  87     packet[5] = (payloadLength >> 8) & 0xFF;
  88     memcpy(packet + 6, payload, payloadLength);
  89     packet[6 + payloadLength] = CalcCRCMsp(packet + 1, payloadLength + 5); // crc = flags + function + length + payload
  90 }
  91
  92 void CRSF::SetHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e destAddr)
  93 {
  94     auto *header = (crsf_header_t *)frame;
  95     header->device_addr = destAddr;
  96     header->frame_size = frameSize;
  97     header->type = frameType;
  98
  99     uint8_t crc = crsf_crc.calc(&frame[CRSF_FRAME_NOT_COUNTED_BYTES], frameSize - 1, 0);
 100     frame[frameSize + CRSF_FRAME_NOT_COUNTED_BYTES - 1] = crc;
 101 }
 102
 103 void CRSF::SetExtendedHeaderAndCrc(uint8_t *frame, crsf_frame_type_e frameType, uint8_t frameSize, crsf_addr_e senderAddr, crsf_addr_e destAddr)
 104 {
 105     auto *header = (crsf_ext_header_t *)frame;
 106     header->dest_addr = destAddr;
 107     header->orig_addr = senderAddr;
 108     SetHeaderAndCrc(frame, frameType, frameSize, destAddr);
 109 }
 110
 111
 112 void CRSF::GetMspMessage(uint8_t **data, uint8_t *len)
 113 {
 114     *len = MspDataLength;
 115     *data = (MspDataLength > 0) ? MspData : nullptr;
 116 }
 117
 118 void CRSF::ResetMspQueue()
 119 {
 120     MspWriteFIFO.flush();
 121     MspDataLength = 0;
 122     memset(MspData, 0, ELRS_MSP_BUFFER);
 123 }
 124
 125 void CRSF::UnlockMspMessage()
 126 {
 127     // current msp message is sent so restore next buffered write
 128     if (MspWriteFIFO.size() > 0)
 129     {
 130         MspWriteFIFO.lock();
 131         MspDataLength = MspWriteFIFO.pop();
 132         MspWriteFIFO.popBytes(MspData, MspDataLength);
 133         MspWriteFIFO.unlock();
 134     }
 135     else
 136     {
 137         // no msp message is ready to send currently
 138         MspDataLength = 0;
 139         memset(MspData, 0, ELRS_MSP_BUFFER);
 140     }
 141 }
 142
 143 void ICACHE_RAM_ATTR CRSF::AddMspMessage(mspPacket_t* packet, uint8_t destination)
 144 {
 145     if (packet->payloadSize > ENCAPSULATED_MSP_MAX_PAYLOAD_SIZE)
 146     {
 147         return;
 148     }
 149
 150     const uint8_t totalBufferLen = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES;
 151     uint8_t outBuffer[ENCAPSULATED_MSP_MAX_FRAME_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC + CRSF_FRAME_NOT_COUNTED_BYTES];
 152
 153     // CRSF extended frame header
 154     outBuffer[0] = CRSF_ADDRESS_BROADCAST;                                      // address
 155     outBuffer[1] = packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC; // length
 156     outBuffer[2] = CRSF_FRAMETYPE_MSP_WRITE;                                    // packet type
 157     outBuffer[3] = destination;                                                 // destination
 158     outBuffer[4] = CRSF_ADDRESS_RADIO_TRANSMITTER;                              // origin
 159
 160     // Encapsulated MSP payload
 161     outBuffer[5] = 0x30;                // header
 162     outBuffer[6] = packet->payloadSize; // mspPayloadSize
 163     outBuffer[7] = packet->function;    // packet->cmd
 164     for (uint16_t i = 0; i < packet->payloadSize; ++i)
 165     {
 166         // copy packet payload into outBuffer
 167         outBuffer[8 + i] = packet->payload[i];
 168     }
 169     // Encapsulated MSP crc
 170     outBuffer[totalBufferLen - 2] = CalcCRCMsp(&outBuffer[6], packet->payloadSize + 2);
 171
 172     // CRSF frame crc
 173     outBuffer[totalBufferLen - 1] = crsf_crc.calc(&outBuffer[2], packet->payloadSize + ENCAPSULATED_MSP_HEADER_CRC_LEN + CRSF_FRAME_LENGTH_EXT_TYPE_CRC - 1);
 174     AddMspMessage(totalBufferLen, outBuffer);
 175 }
 176
 177 void ICACHE_RAM_ATTR CRSF::AddMspMessage(const uint8_t length, uint8_t* data)
 178 {
 179     if (length > ELRS_MSP_BUFFER)
 180     {
 181         return;
 182     }
 183
 184     // store next msp message
 185     if (MspDataLength == 0)
 186     {
 187         for (uint8_t i = 0; i < length; i++)
 188         {
 189             MspData[i] = data[i];
 190         }
 191         MspDataLength = length;
 192     }
 193     // store all write requests since an update does send multiple writes
 194     else
 195     {
 196         MspWriteFIFO.lock();
 197         if (MspWriteFIFO.ensure(length + 1))
 198         {
 199             MspWriteFIFO.push(length);
 200             MspWriteFIFO.pushBytes((const uint8_t *)data, length);
 201         }
 202         MspWriteFIFO.unlock();
 203     }
 204 }
 205
 206 #if defined(CRSF_RX_MODULE)
 207
 208 bool CRSF::HasUpdatedUplinkPower = false;
 209
 210 /***
 211  * @brief: Call this when new uplinkPower from the TX is availble from OTA instead of setting directly
 212  */
 213 void CRSF::updateUplinkPower(uint8_t uplinkPower)
 214 {
 215     if (uplinkPower != LinkStatistics.uplink_TX_Power)
 216     {
 217         LinkStatistics.uplink_TX_Power = uplinkPower;
 218         HasUpdatedUplinkPower = true;
 219     }
 220 }
 221
 222 /***
 223  * @brief: Returns true if HasUpdatedUplinkPower and clears the flag
 224  */
 225 bool CRSF::clearUpdatedUplinkPower()
 226 {
 227     bool retVal = HasUpdatedUplinkPower;
 228     HasUpdatedUplinkPower = false;
 229     return retVal;
 230 }
 231
 232 #endif // CRSF_RX_MODULE