src/main/io/smartport_master.c

   1 /*
   2  * This file is part of INAV.
   3  *
   4  * INAV is free software: you can redistribute it and/or modify
   5  * it under the terms of the GNU General Public License as published by
   6  * the Free Software Foundation, either version 3 of the License, or
   7  * (at your option) any later version.
   8  *
   9  * INAV is distributed in the hope that it will be useful,
  10  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  11  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  12  * GNU General Public License for more details.
  13  *
  14  * You should have received a copy of the GNU General Public License
  15  * along with INAV.  If not, see <http://www.gnu.org/licenses/>.
  16  */
  17
  18 #include <stdbool.h>
  19 #include <stdint.h>
  20 #include <string.h>
  21
  22 #include "platform.h"
  23
  24 #if defined(USE_SMARTPORT_MASTER)
  25
  26 #include "build/debug.h"
  27
  28 #include "common/utils.h"
  29
  30 #include "config/parameter_group.h"
  31 #include "config/parameter_group_ids.h"
  32
  33 #include "drivers/serial.h"
  34 #include "drivers/time.h"
  35
  36 #include "fc/settings.h"
  37
  38 #include "io/serial.h"
  39 #include "io/smartport_master.h"
  40
  41 #include "rx/frsky_crc.h"
  42
  43 enum {
  44     PRIM_DISCARD_FRAME = 0x00,
  45     PRIM_DATA_FRAME = 0x10,
  46     PRIM_WORKING_STATE = 0x20,
  47     PRIM_IDLE_STATE = 0x21,
  48     PRIM_CONFIG_READ = 0x30,
  49     PRIM_CONFIG_WRITE = 0x31,
  50     PRIM_CONFIG_RESPONSE = 0x32,
  51     PRIM_DIAG_READ = 0X40,
  52     PRIM_DIAG_WRITE = 0X41,
  53     PRIM_ENABLE_APP_NODE = 0x70,
  54     PRIM_DISABLE_APP_NODE = 0x71,
  55 };
  56
  57 enum
  58 {
  59     DATAID_SPEED      = 0x0830,
  60     DATAID_VFAS       = 0x0210,
  61     DATAID_CURRENT    = 0x0200,
  62     DATAID_RPM        = 0x050F,
  63     DATAID_ALTITUDE   = 0x0100,
  64     DATAID_FUEL       = 0x0600,
  65     DATAID_ADC1       = 0xF102,
  66     DATAID_ADC2       = 0xF103,
  67     DATAID_LATLONG    = 0x0800,
  68     DATAID_CAP_USED   = 0x0600,
  69     DATAID_VARIO      = 0x0110,
  70     DATAID_CELLS      = 0x0300,
  71     DATAID_CELLS_LAST = 0x030F,
  72     DATAID_HEADING    = 0x0840,
  73     DATAID_FPV        = 0x0450,
  74     DATAID_PITCH      = 0x0430,
  75     DATAID_ROLL       = 0x0440,
  76     DATAID_ACCX       = 0x0700,
  77     DATAID_ACCY       = 0x0710,
  78     DATAID_ACCZ       = 0x0720,
  79     DATAID_T1         = 0x0400,
  80     DATAID_T2         = 0x0410,
  81     DATAID_HOME_DIST  = 0x0420,
  82     DATAID_GPS_ALT    = 0x0820,
  83     DATAID_ASPD       = 0x0A00,
  84     DATAID_A3         = 0x0900,
  85     DATAID_A4         = 0x0910,
  86     DATAID_VS600      = 0x0E10
  87 };
  88
  89 #define SMARTPORT_BAUD 57600
  90 #define SMARTPORT_UART_MODE MODE_RXTX
  91
  92 #define SMARTPORT_PHYID_MAX 0x1B
  93 #define SMARTPORT_PHYID_COUNT (SMARTPORT_PHYID_MAX + 1)
  94
  95 #define SMARTPORT_POLLING_INTERVAL 12 // ms
  96
  97 #define SMARTPORT_FRAME_START 0x7E
  98 #define SMARTPORT_BYTESTUFFING_MARKER 0x7D
  99 #define SMARTPORT_BYTESTUFFING_XOR_VALUE 0x20
 100
 101 #define SMARTPORT_SENSOR_DATA_TIMEOUT 500 // ms
 102
 103 #define SMARTPORT_FORWARD_REQUESTS_MAX 10
 104
 105 typedef enum {
 106     PT_ACTIVE_ID,
 107     PT_INACTIVE_ID
 108 } pollType_e;
 109
 110 typedef struct smartPortMasterFrame_s {
 111     uint8_t magic;
 112     uint8_t phyID;
 113     smartPortPayload_t payload;
 114     uint8_t crc;
 115 } PACKED smartportFrame_t;
 116
 117 typedef union {
 118     smartportFrame_t frame;
 119     uint8_t bytes[sizeof(smartportFrame_t)];
 120 } smartportFrameBuffer_u;
 121
 122 typedef struct {
 123     cellsData_t cells;
 124     timeUs_t cellsTimestamp;
 125     vs600Data_t vs600;
 126     timeUs_t vs600Timestamp;
 127     int32_t altitude;
 128     timeUs_t altitudeTimestamp;
 129     int16_t vario;
 130     timeUs_t varioTimestamp;
 131 } smartportSensorsData_t;
 132
 133 typedef struct {
 134     uint8_t phyID;
 135     smartPortPayload_t payload;
 136 } smartportForwardData_t;
 137
 138 PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0);
 139
 140 PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig,
 141     .halfDuplex = SETTING_SMARTPORT_MASTER_HALFDUPLEX_DEFAULT,
 142     .inverted = SETTING_SMARTPORT_MASTER_INVERTED_DEFAULT
 143 );
 144
 145 static serialPort_t *smartportMasterSerialPort = NULL;
 146 static serialPortConfig_t *portConfig;
 147 static int8_t currentPolledPhyID = -1;
 148 static int8_t forcedPolledPhyID = -1;
 149 static uint8_t rxBufferLen = 0;
 150
 151 static uint32_t activePhyIDs = 0;
 152 static smartPortPayload_t sensorPayloadCache[SMARTPORT_PHYID_COUNT];
 153
 154 static uint8_t forwardRequestsStart = 0;
 155 static uint8_t forwardRequestsEnd = 0;
 156 static smartportForwardData_t forwardRequests[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward requests' circular buffer
 157
 158 static uint8_t forwardResponsesCount = 0;
 159 static smartportForwardData_t forwardResponses[SMARTPORT_FORWARD_REQUESTS_MAX]; // Forward responses' buffer
 160
 161 static smartportSensorsData_t sensorsData;
 162
 163
 164 bool smartportMasterInit(void)
 165 {
 166     portConfig = findSerialPortConfig(FUNCTION_TELEMETRY_SMARTPORT_MASTER);
 167     if (!portConfig) {
 168         return false;
 169     }
 170
 171     portOptions_t portOptions = (smartportMasterConfig()->halfDuplex ? SERIAL_BIDIR : SERIAL_UNIDIR) | (smartportMasterConfig()->inverted ? SERIAL_NOT_INVERTED : SERIAL_INVERTED);
 172     smartportMasterSerialPort = openSerialPort(portConfig->identifier, FUNCTION_TELEMETRY_SMARTPORT_MASTER, NULL, NULL, SMARTPORT_BAUD, SMARTPORT_UART_MODE, portOptions);
 173
 174     memset(&sensorsData, 0, sizeof(sensorsData));
 175
 176     return true;
 177 }
 178
 179 static void phyIDSetActive(uint8_t phyID, bool active)
 180 {
 181     uint32_t mask = 1 << phyID;
 182     if (active) {
 183         activePhyIDs |= mask;
 184     } else {
 185         activePhyIDs &= ~mask;
 186     }
 187 }
 188
 189 static uint32_t phyIDAllActiveMask(void)
 190 {
 191     uint32_t mask = 0;
 192     for (uint8_t i = 0; i < SMARTPORT_PHYID_COUNT; ++i) {
 193         mask |= 1 << i;
 194     }
 195     return mask;
 196 }
 197
 198 static int8_t phyIDNext(uint8_t start, bool active, bool loop)
 199 {
 200     for (uint8_t i = start; i < ((loop ? start : 0) + SMARTPORT_PHYID_COUNT); ++i) {
 201         uint8_t phyID = i % SMARTPORT_PHYID_COUNT;
 202         uint32_t mask = 1 << phyID;
 203         uint32_t phyIDMasked = activePhyIDs & mask;
 204         if ((active && phyIDMasked) || !(active || phyIDMasked)) {
 205             return phyID;
 206         }
 207     }
 208     return -1;
 209 }
 210
 211 static bool phyIDIsActive(uint8_t id)
 212 {
 213     return !!(activePhyIDs & (1 << id));
 214 }
 215
 216 static bool phyIDNoneActive(void)
 217 {
 218     return activePhyIDs == 0;
 219 }
 220
 221 static bool phyIDAllActive(void)
 222 {
 223     static uint32_t allActiveMask = 0;
 224
 225     if (!allActiveMask) {
 226         allActiveMask = phyIDAllActiveMask();
 227     }
 228
 229     return !!((activePhyIDs & allActiveMask) == allActiveMask);
 230 }
 231
 232 static bool phyIDAnyActive(void)
 233 {
 234     return !!activePhyIDs;
 235 }
 236
 237 static void smartportMasterSendByte(uint8_t byte)
 238 {
 239     serialWrite(smartportMasterSerialPort, byte);
 240 }
 241
 242 static void smartportMasterPhyIDFillCheckBits(uint8_t *phyIDByte)
 243 {
 244     *phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 1) ^ GET_BIT(*phyIDByte, 2)) << 5;
 245     *phyIDByte |= (GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 3) ^ GET_BIT(*phyIDByte, 4)) << 6;
 246     *phyIDByte |= (GET_BIT(*phyIDByte, 0) ^ GET_BIT(*phyIDByte, 2) ^ GET_BIT(*phyIDByte, 4)) << 7;
 247 }
 248
 249 static void smartportMasterPoll(void)
 250 {
 251     static pollType_e nextPollType = PT_INACTIVE_ID;
 252     static uint8_t nextActivePhyID = 0, nextInactivePhyID = 0;
 253     uint8_t phyIDToPoll;
 254
 255     if (currentPolledPhyID != -1) {
 256         // currentPolledPhyID hasn't been reset by smartportMasterReceive so we didn't get valid data for this PhyID (inactive)
 257         phyIDSetActive(currentPolledPhyID, false);
 258     }
 259
 260     if (forcedPolledPhyID != -1) {
 261
 262         phyIDToPoll = forcedPolledPhyID;
 263         forcedPolledPhyID = -1;
 264
 265     } else {
 266
 267         if (phyIDNoneActive()) {
 268             nextPollType = PT_INACTIVE_ID;
 269         } else if (phyIDAllActive()) {
 270             nextPollType = PT_ACTIVE_ID;
 271         }
 272
 273         switch (nextPollType) {
 274
 275             case PT_ACTIVE_ID: {
 276                 int8_t activePhyIDToPoll = phyIDNext(nextActivePhyID, true, false);
 277                 if (activePhyIDToPoll == -1) {
 278                     nextActivePhyID = 0;
 279                     nextPollType = PT_INACTIVE_ID;
 280                 } else {
 281                     phyIDToPoll = activePhyIDToPoll;
 282                     if (phyIDToPoll == SMARTPORT_PHYID_MAX) {
 283                         nextActivePhyID = 0;
 284                         if (!phyIDAllActive()) {
 285                             nextPollType = PT_INACTIVE_ID;
 286                         }
 287                     } else {
 288                         nextActivePhyID = phyIDToPoll + 1;
 289                     }
 290                     break;
 291                 }
 292                 FALLTHROUGH;
 293             }
 294
 295             case PT_INACTIVE_ID: {
 296                 phyIDToPoll = phyIDNext(nextInactivePhyID, false, true);
 297                 nextInactivePhyID = (phyIDToPoll == SMARTPORT_PHYID_MAX ? 0 : phyIDToPoll + 1);
 298                 if (phyIDAnyActive()) {
 299                     nextPollType = PT_ACTIVE_ID;
 300                 }
 301                 break;
 302             }
 303
 304             default: // should not happen
 305                 return;
 306
 307         }
 308
 309     }
 310
 311     currentPolledPhyID = phyIDToPoll;
 312     smartportMasterPhyIDFillCheckBits(&phyIDToPoll);
 313
 314     // poll
 315     smartportMasterSendByte(SMARTPORT_FRAME_START);
 316     smartportMasterSendByte(phyIDToPoll);
 317
 318     rxBufferLen = 0; // discard incomplete frames received during previous poll
 319 }
 320
 321 static void smartportMasterForwardNextPayload(void)
 322 {
 323     smartportForwardData_t *request = forwardRequests + forwardRequestsStart;
 324
 325     /*forcedPolledPhyID = request->phyID; // force next poll to the request's phyID          XXX disabled, doesn't seem necessary */
 326
 327     smartportMasterPhyIDFillCheckBits(&request->phyID);
 328     smartportMasterSendByte(SMARTPORT_FRAME_START);
 329     smartportMasterSendByte(request->phyID);
 330
 331     uint16_t checksum = 0;
 332     uint8_t *payloadPtr = (uint8_t *)&request->payload;
 333     for (uint8_t i = 0; i < sizeof(request->payload); ++i) {
 334         uint8_t c = *payloadPtr++;
 335         if ((c == SMARTPORT_FRAME_START) || (c == SMARTPORT_BYTESTUFFING_MARKER)) {
 336             smartportMasterSendByte(SMARTPORT_BYTESTUFFING_MARKER);
 337             smartportMasterSendByte(c ^ SMARTPORT_BYTESTUFFING_XOR_VALUE);
 338         } else {
 339             smartportMasterSendByte(c);
 340         }
 341         checksum += c;
 342     }
 343     checksum = 0xff - ((checksum & 0xff) + (checksum >> 8));
 344     smartportMasterSendByte(checksum);
 345
 346     forwardRequestsStart = (forwardRequestsStart + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
 347 }
 348
 349 static void decodeCellsData(uint32_t sdata)
 350 {
 351     uint8_t voltageStartIndex = sdata & 0xF;
 352     uint8_t count = sdata >> 4 & 0xF;
 353     uint16_t voltage1 = (sdata >> 8 & 0xFFF) * 2;
 354     uint16_t voltage2 = (sdata >> 20 & 0xFFF) * 2;
 355     if ((voltageStartIndex <= 4) && (count <= 6)) { // sanity check
 356         cellsData_t *cd = &sensorsData.cells;
 357         cd->count = count;
 358         cd->voltage[voltageStartIndex] = voltage1;
 359         cd->voltage[voltageStartIndex+1] = voltage2;
 360     }
 361 }
 362
 363 static void decodeVS600Data(uint32_t sdata)
 364 {
 365     vs600Data_t *vs600 = &sensorsData.vs600;
 366     vs600->power = (sdata >> 8) & 0xFF;
 367     vs600->band = (sdata >> 16) & 0xFF;
 368     vs600->channel = (sdata >> 24) & 0xFF;
 369 }
 370
 371 static void decodeAltitudeData(uint32_t sdata)
 372 {
 373     sensorsData.altitude = sdata * 5; // cm
 374 }
 375
 376 static void decodeVarioData(uint32_t sdata)
 377 {
 378     sensorsData.vario = sdata * 2; // mm/s
 379 }
 380
 381 static void processSensorPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
 382 {
 383     switch (payload->valueId) {
 384         case DATAID_CELLS:
 385             decodeCellsData(payload->data);
 386             sensorsData.cellsTimestamp = currentTimeUs;
 387             break;
 388
 389         case DATAID_VS600:
 390             decodeVS600Data(payload->data);
 391             sensorsData.vs600Timestamp = currentTimeUs;
 392             break;
 393
 394         case DATAID_ALTITUDE:
 395             decodeAltitudeData(payload->data);
 396             sensorsData.altitudeTimestamp = currentTimeUs;
 397             break;
 398
 399         case DATAID_VARIO:
 400             decodeVarioData(payload->data);
 401             sensorsData.varioTimestamp = currentTimeUs;
 402             break;
 403     }
 404     sensorPayloadCache[currentPolledPhyID] = *payload;
 405 }
 406
 407 static void processPayload(smartPortPayload_t *payload, timeUs_t currentTimeUs)
 408 {
 409     switch (payload->frameId) {
 410
 411         case PRIM_DISCARD_FRAME:
 412             break;
 413
 414         case PRIM_DATA_FRAME:
 415             processSensorPayload(payload, currentTimeUs);
 416             break;
 417
 418         default:
 419             if (forwardResponsesCount < SMARTPORT_FORWARD_REQUESTS_MAX) {
 420                 smartportForwardData_t *response = forwardResponses + forwardResponsesCount;
 421                 response->phyID = currentPolledPhyID;
 422                 response->payload = *payload;
 423                 forwardResponsesCount += 1;
 424             }
 425             break;
 426     }
 427 }
 428
 429 static void smartportMasterReceive(timeUs_t currentTimeUs)
 430 {
 431     static smartportFrameBuffer_u buffer;
 432     static bool processByteStuffing = false;
 433
 434     if (!rxBufferLen) {
 435         processByteStuffing = false;
 436     }
 437
 438     while (serialRxBytesWaiting(smartportMasterSerialPort)) {
 439
 440         uint8_t c = serialRead(smartportMasterSerialPort);
 441
 442         if (currentPolledPhyID == -1) { // We did not poll a sensor or a packet has already been received and processed
 443             continue;
 444         }
 445
 446         if (processByteStuffing) {
 447             c ^= SMARTPORT_BYTESTUFFING_XOR_VALUE;
 448             processByteStuffing = false;
 449         } else if (c == SMARTPORT_BYTESTUFFING_MARKER) {
 450             processByteStuffing = true;
 451             continue;
 452         }
 453
 454         buffer.bytes[rxBufferLen] = c;
 455         rxBufferLen += 1;
 456
 457         if (rxBufferLen == sizeof(buffer)) { // frame complete
 458
 459             uint8_t calcCRC = frskyCheckSum((uint8_t *)&buffer.frame.payload, sizeof(buffer.frame.payload));
 460
 461             if (buffer.frame.crc == calcCRC) {
 462                 phyIDSetActive(currentPolledPhyID, true);
 463                 processPayload(&buffer.frame.payload, currentTimeUs);
 464             }
 465
 466             currentPolledPhyID = -1; // previously polled PhyID has answered, not expecting more data until next poll
 467             rxBufferLen = 0; // reset buffer
 468
 469         }
 470
 471     }
 472 }
 473
 474 bool smartportMasterGetSensorPayload(uint8_t phyID, smartPortPayload_t *payload)
 475 {
 476     if ((phyID > SMARTPORT_PHYID_MAX) || !phyIDIsActive(phyID)) {
 477         return false;
 478     }
 479
 480     *payload = sensorPayloadCache[phyID];
 481     return true;
 482 }
 483
 484 bool smartportMasterHasForwardResponse(uint8_t phyID)
 485 {
 486     for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
 487         smartportForwardData_t *response = forwardResponses + i;
 488         if (response->phyID == phyID) {
 489             return true;
 490         }
 491     }
 492
 493     return false;
 494 }
 495
 496 bool smartportMasterNextForwardResponse(uint8_t phyID, smartPortPayload_t *payload)
 497 {
 498     for (uint8_t i = 0; i < forwardResponsesCount; ++i) {
 499         smartportForwardData_t *response = forwardResponses + i;
 500         if (response->phyID == phyID) {
 501             *payload = response->payload;
 502             forwardResponsesCount -= 1;
 503             memmove(response, response + 1, (forwardResponsesCount - i) * sizeof(*response));
 504             return true;
 505         }
 506     }
 507
 508     return false;
 509 }
 510
 511 static uint8_t forwardRequestCount(void)
 512 {
 513     if (forwardRequestsStart > forwardRequestsEnd) {
 514         return SMARTPORT_FORWARD_REQUESTS_MAX - forwardRequestsStart + forwardRequestsEnd;
 515     } else {
 516         return forwardRequestsEnd - forwardRequestsStart;
 517     }
 518 }
 519
 520 bool smartportMasterForward(uint8_t phyID, smartPortPayload_t *payload)
 521 {
 522     if (forwardRequestCount() == SMARTPORT_FORWARD_REQUESTS_MAX) {
 523         return false;
 524     }
 525
 526     smartportForwardData_t *request = forwardRequests + forwardRequestsEnd;
 527     request->phyID = phyID;
 528     request->payload = *payload;
 529     forwardRequestsEnd = (forwardRequestsEnd + 1) % SMARTPORT_FORWARD_REQUESTS_MAX;
 530     return true;
 531 }
 532
 533 void smartportMasterHandle(timeUs_t currentTimeUs)
 534 {
 535     static timeUs_t pollTimestamp = 0;
 536
 537     if (!smartportMasterSerialPort) {
 538         return;
 539     }
 540
 541     if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) {
 542         if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one
 543             smartportMasterForwardNextPayload();
 544         } else {
 545             smartportMasterPoll();
 546         }
 547         pollTimestamp = currentTimeUs;
 548     } else {
 549         smartportMasterReceive(currentTimeUs);
 550     }
 551 }
 552
 553 cellsData_t *smartportMasterGetCellsData(void)
 554 {
 555     if (micros() - sensorsData.cellsTimestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
 556         return NULL;
 557     }
 558
 559     return &sensorsData.cells;
 560 }
 561
 562 vs600Data_t *smartportMasterGetVS600Data(void)
 563 {
 564     if (micros() - sensorsData.vs600Timestamp > SMARTPORT_SENSOR_DATA_TIMEOUT) {
 565         return NULL;
 566     }
 567
 568     return &sensorsData.vs600;
 569 }
 570
 571 bool smartportMasterPhyIDIsActive(uint8_t phyID)
 572 {
 573     return phyIDIsActive(phyID);
 574 }
 575
 576 int8_t smartportMasterStripPhyIDCheckBits(uint8_t phyID)
 577 {
 578     uint8_t smartportPhyID = phyID & 0x1F;
 579     uint8_t phyIDCheck = smartportPhyID;
 580     smartportMasterPhyIDFillCheckBits(&phyIDCheck);
 581     return phyID == phyIDCheck ? smartportPhyID : -1;
 582 }
 583
 584 #endif