src/lib/CrsfProtocol/crsf_protocol.h

   1 #pragma once
   2
   3 #include <cstdint>
   4 #include <cmath>
   5 #include "crc.h"
   6 #include "options.h"
   7
   8 #if TARGET_TX && PLATFORM_STM32
   9 #define CRSF_TX_MODULE_STM32 1
  10 #endif
  11
  12 #if TARGET_TX
  13 #define CRSF_TX_MODULE 1
  14 #elif TARGET_RX || defined(UNIT_TEST)
  15 #define CRSF_RX_MODULE 1
  16 #else
  17 #error "Invalid configuration!"
  18 #endif
  19
  20
  21 #define PACKED __attribute__((packed))
  22
  23 #ifndef ICACHE_RAM_ATTR
  24 #define ICACHE_RAM_ATTR
  25 #endif
  26
  27 #define CRSF_CRC_POLY 0xd5
  28
  29 #ifndef RCVR_UART_BAUD
  30 #define RCVR_UART_BAUD 420000
  31 #endif
  32
  33 #define CRSF_NUM_CHANNELS 16
  34 #define CRSF_CHANNEL_VALUE_MIN  172
  35 #define CRSF_CHANNEL_VALUE_1000 191
  36 #define CRSF_CHANNEL_VALUE_MID  992
  37 #define CRSF_CHANNEL_VALUE_2000 1792
  38 #define CRSF_CHANNEL_VALUE_MAX  1811
  39 #define CRSF_MAX_PACKET_LEN 64
  40
  41 #define CRSF_SYNC_BYTE 0xC8
  42
  43 #define RCframeLength 22             // length of the RC data packed bytes frame. 16 channels in 11 bits each.
  44 #define LinkStatisticsFrameLength 10 //
  45 #define OpenTXsyncFrameLength 11     //
  46 #define BattSensorFrameLength 8      //
  47 #define VTXcontrolFrameLength 12     //
  48
  49 #define CRSF_PAYLOAD_SIZE_MAX 62
  50 #define CRSF_FRAME_NOT_COUNTED_BYTES 2
  51 #define CRSF_FRAME_SIZE(payload_size) ((payload_size) + 2) // See crsf_header_t.frame_size
  52 #define CRSF_EXT_FRAME_SIZE(payload_size) (CRSF_FRAME_SIZE(payload_size) + 2)
  53 #define CRSF_FRAME_SIZE_MAX (CRSF_PAYLOAD_SIZE_MAX + CRSF_FRAME_NOT_COUNTED_BYTES)
  54 #define CRSF_FRAME_CRC_SIZE 1
  55 #define CRSF_FRAME_LENGTH_EXT_TYPE_CRC 4 // length of Extended Dest/Origin, TYPE and CRC fields combined
  56
  57 #define CRSF_TELEMETRY_LENGTH_INDEX 1
  58 #define CRSF_TELEMETRY_TYPE_INDEX 2
  59 #define CRSF_TELEMETRY_CRC_LENGTH 1
  60 #define CRSF_TELEMETRY_TOTAL_SIZE(x) (x + CRSF_FRAME_LENGTH_EXT_TYPE_CRC)
  61
  62 // Macros for big-endian (assume little endian host for now) etc
  63 #define CRSF_DEC_U16(x) ((uint16_t)__builtin_bswap16(x))
  64 #define CRSF_DEC_I16(x) ((int16_t)CRSF_DEC_U16(x))
  65 #define CRSF_DEC_U24(x) (CRSF_DEC_U32((uint32_t)x << 8))
  66 #define CRSF_DEC_U32(x) ((uint32_t)__builtin_bswap32(x))
  67 #define CRSF_DEC_I32(x) ((int32_t)CRSF_DEC_U32(x))
  68
  69 //////////////////////////////////////////////////////////////
  70
  71 #define CRSF_MSP_REQ_PAYLOAD_SIZE 8
  72 #define CRSF_MSP_RESP_PAYLOAD_SIZE 58
  73 #define CRSF_MSP_MAX_PAYLOAD_SIZE (CRSF_MSP_REQ_PAYLOAD_SIZE > CRSF_MSP_RESP_PAYLOAD_SIZE ? CRSF_MSP_REQ_PAYLOAD_SIZE : CRSF_MSP_RESP_PAYLOAD_SIZE)
  74
  75 static const unsigned int VTXtable[6][8] =
  76     {{5865, 5845, 5825, 5805, 5785, 5765, 5745, 5725},  /* Band A */
  77      {5733, 5752, 5771, 5790, 5809, 5828, 5847, 5866},  /* Band B */
  78      {5705, 5685, 5665, 5645, 5885, 5905, 5925, 5945},  /* Band E */
  79      {5740, 5760, 5780, 5800, 5820, 5840, 5860, 5880},  /* Ariwave */
  80      {5658, 5695, 5732, 5769, 5806, 5843, 5880, 5917},  /* Race */
  81      {5621, 5584, 5547, 5510, 5473, 5436, 5399, 5362}}; /* LO Race */
  82
  83 typedef enum
  84 {
  85     CRSF_FRAMETYPE_GPS = 0x02,
  86     CRSF_FRAMETYPE_VARIO = 0x07,
  87     CRSF_FRAMETYPE_BATTERY_SENSOR = 0x08,
  88     CRSF_FRAMETYPE_LINK_STATISTICS = 0x14,
  89     CRSF_FRAMETYPE_OPENTX_SYNC = 0x10,
  90     CRSF_FRAMETYPE_RADIO_ID = 0x3A,
  91     CRSF_FRAMETYPE_RC_CHANNELS_PACKED = 0x16,
  92     CRSF_FRAMETYPE_ATTITUDE = 0x1E,
  93     CRSF_FRAMETYPE_FLIGHT_MODE = 0x21,
  94     // Extended Header Frames, range: 0x28 to 0x96
  95     CRSF_FRAMETYPE_DEVICE_PING = 0x28,
  96     CRSF_FRAMETYPE_DEVICE_INFO = 0x29,
  97     CRSF_FRAMETYPE_PARAMETER_SETTINGS_ENTRY = 0x2B,
  98     CRSF_FRAMETYPE_PARAMETER_READ = 0x2C,
  99     CRSF_FRAMETYPE_PARAMETER_WRITE = 0x2D,
 100
 101     //CRSF_FRAMETYPE_ELRS_STATUS = 0x2E, ELRS good/bad packet count and status flags
 102
 103     CRSF_FRAMETYPE_COMMAND = 0x32,
 104     // KISS frames
 105     CRSF_FRAMETYPE_KISS_REQ  = 0x78,
 106     CRSF_FRAMETYPE_KISS_RESP = 0x79,
 107     // MSP commands
 108     CRSF_FRAMETYPE_MSP_REQ = 0x7A,   // response request using msp sequence as command
 109     CRSF_FRAMETYPE_MSP_RESP = 0x7B,  // reply with 58 byte chunked binary
 110     CRSF_FRAMETYPE_MSP_WRITE = 0x7C, // write with 8 byte chunked binary (OpenTX outbound telemetry buffer limit)
 111     // Ardupilot frames
 112     CRSF_FRAMETYPE_ARDUPILOT_RESP = 0x80,
 113 } crsf_frame_type_e;
 114
 115 typedef enum {
 116     SUBCOMMAND_CRSF = 0x10
 117 } crsf_command_e;
 118
 119 typedef enum {
 120     COMMAND_MODEL_SELECT_ID = 0x05
 121 } crsf_subcommand_e;
 122
 123 enum {
 124     CRSF_FRAME_TX_MSP_FRAME_SIZE = 58,
 125     CRSF_FRAME_RX_MSP_FRAME_SIZE = 8,
 126     CRSF_FRAME_ORIGIN_DEST_SIZE = 2,
 127 };
 128
 129 enum {
 130     CRSF_FRAME_GPS_PAYLOAD_SIZE = 15,
 131     CRSF_FRAME_VARIO_PAYLOAD_SIZE = 2,
 132     CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE = 8,
 133     CRSF_FRAME_ATTITUDE_PAYLOAD_SIZE = 6,
 134     CRSF_FRAME_DEVICE_INFO_PAYLOAD_SIZE = 48,
 135     CRSF_FRAME_FLIGHT_MODE_PAYLOAD_SIZE = 16,
 136     CRSF_FRAME_GENERAL_RESP_PAYLOAD_SIZE = CRSF_EXT_FRAME_SIZE(CRSF_FRAME_TX_MSP_FRAME_SIZE)
 137 };
 138
 139 typedef enum
 140 {
 141     CRSF_ADDRESS_BROADCAST = 0x00,
 142     CRSF_ADDRESS_USB = 0x10,
 143     CRSF_ADDRESS_TBS_CORE_PNP_PRO = 0x80,
 144     CRSF_ADDRESS_RESERVED1 = 0x8A,
 145     CRSF_ADDRESS_CURRENT_SENSOR = 0xC0,
 146     CRSF_ADDRESS_GPS = 0xC2,
 147     CRSF_ADDRESS_TBS_BLACKBOX = 0xC4,
 148     CRSF_ADDRESS_FLIGHT_CONTROLLER = 0xC8,
 149     CRSF_ADDRESS_RESERVED2 = 0xCA,
 150     CRSF_ADDRESS_RACE_TAG = 0xCC,
 151     CRSF_ADDRESS_RADIO_TRANSMITTER = 0xEA,
 152     CRSF_ADDRESS_CRSF_RECEIVER = 0xEC,
 153     CRSF_ADDRESS_CRSF_TRANSMITTER = 0xEE,
 154     CRSF_ADDRESS_ELRS_LUA = 0xEF
 155 } crsf_addr_e;
 156
 157 //typedef struct crsf_addr_e asas;
 158
 159 typedef enum
 160 {
 161     CRSF_UINT8 = 0,
 162     CRSF_INT8 = 1,
 163     CRSF_UINT16 = 2,
 164     CRSF_INT16 = 3,
 165     CRSF_UINT32 = 4,
 166     CRSF_INT32 = 5,
 167     CRSF_UINT64 = 6,
 168     CRSF_INT64 = 7,
 169     CRSF_FLOAT = 8,
 170     CRSF_TEXT_SELECTION = 9,
 171     CRSF_STRING = 10,
 172     CRSF_FOLDER = 11,
 173     CRSF_INFO = 12,
 174     CRSF_COMMAND = 13,
 175     CRSF_VTX = 15,
 176     CRSF_OUT_OF_RANGE = 127,
 177 } crsf_value_type_e;
 178
 179 // These flags are or'ed with the field type above to hide the field from the normal LUA view
 180 #define CRSF_FIELD_HIDDEN       0x80     // marked as hidden in all LUA responses
 181 #define CRSF_FIELD_ELRS_HIDDEN  0x40     // marked as hidden when talking to ELRS specific LUA
 182 #define CRSF_FIELD_TYPE_MASK    ~(CRSF_FIELD_HIDDEN|CRSF_FIELD_ELRS_HIDDEN)
 183
 184 /**
 185  * Define the shape of a standard header
 186  */
 187 typedef struct crsf_header_s
 188 {
 189     uint8_t device_addr; // from crsf_addr_e
 190     uint8_t frame_size;  // counts size after this byte, so it must be the payload size + 2 (type and crc)
 191     uint8_t type;        // from crsf_frame_type_e
 192 } PACKED crsf_header_t;
 193
 194 // Used by extended header frames (type in range 0x28 to 0x96)
 195 typedef struct crsf_ext_header_s
 196 {
 197     // Common header fields, see crsf_header_t
 198     uint8_t device_addr;
 199     uint8_t frame_size;
 200     uint8_t type;
 201     // Extended fields
 202     uint8_t dest_addr;
 203     uint8_t orig_addr;
 204 } PACKED crsf_ext_header_t;
 205
 206 /**
 207  * Crossfire packed channel structure, each channel is 11 bits
 208  */
 209 typedef struct crsf_channels_s
 210 {
 211     unsigned ch0 : 11;
 212     unsigned ch1 : 11;
 213     unsigned ch2 : 11;
 214     unsigned ch3 : 11;
 215     unsigned ch4 : 11;
 216     unsigned ch5 : 11;
 217     unsigned ch6 : 11;
 218     unsigned ch7 : 11;
 219     unsigned ch8 : 11;
 220     unsigned ch9 : 11;
 221     unsigned ch10 : 11;
 222     unsigned ch11 : 11;
 223     unsigned ch12 : 11;
 224     unsigned ch13 : 11;
 225     unsigned ch14 : 11;
 226     unsigned ch15 : 11;
 227 } PACKED crsf_channels_t;
 228
 229 /**
 230  * Define the shape of a standard packet
 231  * A 'standard' header followed by the packed channels
 232  */
 233 typedef struct rcPacket_s
 234 {
 235     crsf_header_t header;
 236     crsf_channels_s channels;
 237 } PACKED rcPacket_t;
 238
 239 typedef struct deviceInformationPacket_s
 240 {
 241     uint32_t serialNo;
 242     uint32_t hardwareVer;
 243     uint32_t softwareVer;
 244     uint8_t fieldCnt;          //number of field of params this device has
 245     uint8_t parameterVersion;
 246 } PACKED deviceInformationPacket_t;
 247
 248 #define DEVICE_INFORMATION_PAYLOAD_LENGTH (sizeof(deviceInformationPacket_t) + device_name_size)
 249 #define DEVICE_INFORMATION_LENGTH (sizeof(crsf_ext_header_t) + DEVICE_INFORMATION_PAYLOAD_LENGTH + CRSF_FRAME_CRC_SIZE)
 250 #define DEVICE_INFORMATION_FRAME_SIZE (DEVICE_INFORMATION_PAYLOAD_LENGTH + CRSF_FRAME_LENGTH_EXT_TYPE_CRC)
 251
 252 /**
 253  * Union to allow accessing the input buffer as different data shapes
 254  * without generating compiler warnings (and relying on undefined C++ behaviour!)
 255  * Each entry in the union provides a different view of the same memory.
 256  * This is just the defintion of the union, the declaration of the variable that
 257  * uses it is later in the file.
 258  */
 259 union inBuffer_U
 260 {
 261     uint8_t asUint8_t[CRSF_MAX_PACKET_LEN]; // max 64 bytes for CRSF packet serial buffer
 262     rcPacket_t asRCPacket_t;    // access the memory as RC data
 263                                 // add other packet types here
 264 };
 265
 266
 267 typedef struct crsf_channels_s crsf_channels_t;
 268
 269 // Used by extended header frames (type in range 0x28 to 0x96)
 270 typedef struct crsf_sensor_battery_s
 271 {
 272     unsigned voltage : 16;  // mv * 100
 273     unsigned current : 16;  // ma * 100
 274     unsigned capacity : 24; // mah
 275     unsigned remaining : 8; // %
 276 } PACKED crsf_sensor_battery_s;
 277
 278 typedef struct crsf_sensor_battery_s crsf_sensor_battery_t;
 279
 280 /*
 281  * 0x14 Link statistics
 282  * Payload:
 283  *
 284  * uint8_t Uplink RSSI Ant. 1 ( dBm * -1 )
 285  * uint8_t Uplink RSSI Ant. 2 ( dBm * -1 )
 286  * uint8_t Uplink Package success rate / Link quality ( % )
 287  * int8_t Uplink SNR ( db )
 288  * uint8_t Diversity active antenna ( enum ant. 1 = 0, ant. 2 )
 289  * uint8_t RF Mode ( enum 4fps = 0 , 50fps, 150hz)
 290  * uint8_t Uplink TX Power ( enum 0mW = 0, 10mW, 25 mW, 100 mW, 500 mW, 1000 mW, 2000mW )
 291  * uint8_t Downlink RSSI ( dBm * -1 )
 292  * uint8_t Downlink package success rate / Link quality ( % )
 293  * int8_t Downlink SNR ( db )
 294  * Uplink is the connection from the ground to the UAV and downlink the opposite direction.
 295  */
 296
 297 typedef struct crsfPayloadLinkstatistics_s
 298 {
 299     uint8_t uplink_RSSI_1;
 300     uint8_t uplink_RSSI_2;
 301     uint8_t uplink_Link_quality;
 302     int8_t uplink_SNR;
 303     uint8_t active_antenna;
 304     uint8_t rf_Mode;
 305     uint8_t uplink_TX_Power;
 306     uint8_t downlink_RSSI;
 307     uint8_t downlink_Link_quality;
 308     int8_t downlink_SNR;
 309 } crsfLinkStatistics_t;
 310
 311 typedef struct crsfPayloadLinkstatistics_s crsfLinkStatistics_t;
 312
 313 // typedef struct crsfOpenTXsyncFrame_s
 314 // {
 315 //     uint32_t adjustedRefreshRate;
 316 //     uint32_t lastUpdate;
 317 //     uint16_t refreshRate;
 318 //     int8_t refreshRate;
 319 //     uint16_t inputLag;
 320 //     uint8_t interval;
 321 //     uint8_t target;
 322 //     uint8_t downlink_RSSI;
 323 //     uint8_t downlink_Link_quality;
 324 //     int8_t downlink_SNR;
 325 // } crsfOpenTXsyncFrame_t;
 326
 327 // typedef struct crsfOpenTXsyncFrame_s crsfOpenTXsyncFrame_t;
 328
 329 /////inline and utility functions//////
 330
 331 static uint16_t ICACHE_RAM_ATTR fmap(uint16_t x, uint16_t in_min, uint16_t in_max, uint16_t out_min, uint16_t out_max)
 332 {
 333     return ((x - in_min) * (out_max - out_min) * 2 / (in_max - in_min) + out_min * 2 + 1) / 2;
 334 }
 335
 336 // Scale a full range crossfire value to 988-2012 (Taransi channel uS)
 337 static inline uint16_t ICACHE_RAM_ATTR CRSF_to_US(uint16_t val)
 338 {
 339     return fmap(val, 172, 1811, 988, 2012);
 340 }
 341
 342 // Scale down a 10-bit value to a full range crossfire value
 343 static inline uint16_t ICACHE_RAM_ATTR UINT10_to_CRSF(uint16_t val)
 344 {
 345     return fmap(val, 0, 1024, 172, 1811);
 346 }
 347
 348 // Scale up a full range crossfire value to 10-bit
 349 static inline uint16_t ICACHE_RAM_ATTR CRSF_to_UINT10(uint16_t val)
 350 {
 351     return fmap(val, 172, 1811, 0, 1023);
 352 }
 353
 354 // Convert 0-max to the CRSF values for 1000-2000
 355 static inline uint16_t ICACHE_RAM_ATTR N_to_CRSF(uint16_t val, uint16_t max)
 356 {
 357     return val * (CRSF_CHANNEL_VALUE_2000-CRSF_CHANNEL_VALUE_1000) / max + CRSF_CHANNEL_VALUE_1000;
 358 }
 359
 360 // Convert CRSF to 0-(cnt-1), constrained between 1000us and 2000us
 361 static inline uint16_t ICACHE_RAM_ATTR CRSF_to_N(uint16_t val, uint16_t cnt)
 362 {
 363     // The span is increased by one to prevent the max val from returning cnt
 364     if (val <= CRSF_CHANNEL_VALUE_1000)
 365         return 0;
 366     if (val >= CRSF_CHANNEL_VALUE_2000)
 367         return cnt - 1;
 368     return (val - CRSF_CHANNEL_VALUE_1000) * cnt / (CRSF_CHANNEL_VALUE_2000 - CRSF_CHANNEL_VALUE_1000 + 1);
 369 }
 370
 371 // 3b switches use 0-5 to represent 6 positions switches and "7" to represent middle
 372 // The calculation is a bit non-linear all the way to the endpoints due to where
 373 // Ardupilot defines its modes
 374 static inline uint16_t ICACHE_RAM_ATTR SWITCH3b_to_CRSF(uint16_t val)
 375 {
 376     switch (val)
 377     {
 378     case 0: return CRSF_CHANNEL_VALUE_1000;
 379     case 5: return CRSF_CHANNEL_VALUE_2000;
 380     case 6: // fallthrough
 381     case 7: return CRSF_CHANNEL_VALUE_MID;
 382     default: // (val - 1) * 240 + 630; aka 150us spacing, starting at 1275
 383         return val * 240 + 391;
 384     }
 385 }
 386
 387 // Returns 1 if val is greater than CRSF_CHANNEL_VALUE_MID
 388 static inline uint8_t ICACHE_RAM_ATTR CRSF_to_BIT(uint16_t val)
 389 {
 390     return (val > CRSF_CHANNEL_VALUE_MID) ? 1 : 0;
 391 }
 392
 393 // Convert a bit into either the CRSF value for 1000 or 2000
 394 static inline uint16_t ICACHE_RAM_ATTR BIT_to_CRSF(uint8_t val)
 395 {
 396     return (val) ? CRSF_CHANNEL_VALUE_2000 : CRSF_CHANNEL_VALUE_1000;
 397 }
 398
 399 static inline uint8_t ICACHE_RAM_ATTR CalcCRCMsp(uint8_t *data, int length)
 400 {
 401     uint8_t crc = 0;
 402     for (uint8_t i = 0; i < length; ++i) {
 403         crc = crc ^ *data++;
 404     }
 405     return crc;
 406 }
 407
 408 #if !defined(__linux__)
 409 static inline uint16_t htobe16(uint16_t val)
 410 {
 411 #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
 412     return val;
 413 #else
 414     return __builtin_bswap16(val);
 415 #endif
 416 }
 417
 418 static inline uint32_t htobe32(uint32_t val)
 419 {
 420 #if (__BYTE_ORDER__ == __ORDER_BIG_ENDIAN__)
 421     return val;
 422 #else
 423     return __builtin_bswap32(val);
 424 #endif
 425 }
 426 #endif