src/main/drivers/serial_escserial.c

   1 /*
   2  * This file is part of Cleanflight and Betaflight.
   3  *
   4  * Cleanflight and Betaflight are free software. You can redistribute
   5  * this software and/or modify this software under the terms of the
   6  * GNU General Public License as published by the Free Software
   7  * Foundation, either version 3 of the License, or (at your option)
   8  * any later version.
   9  *
  10  * Cleanflight and Betaflight are distributed in the hope that they
  11  * will be useful, but WITHOUT ANY WARRANTY; without even the implied
  12  * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
  13  * See the GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this software.
  17  *
  18  * If not, see <http://www.gnu.org/licenses/>.
  19  */
  20
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #if defined(USE_ESCSERIAL)
  28
  29 #include "build/build_config.h"
  30 #include "build/atomic.h"
  31
  32 #include "common/utils.h"
  33
  34 #include "pg/pg.h"
  35 #include "pg/pg_ids.h"
  36
  37 #include "drivers/io.h"
  38 #include "drivers/light_led.h"
  39 #include "drivers/nvic.h"
  40 #include "drivers/pwm_output.h"
  41 #include "drivers/serial.h"
  42 #include "drivers/serial_escserial.h"
  43 #include "drivers/time.h"
  44 #include "drivers/timer.h"
  45
  46 #include "flight/mixer.h"
  47
  48 #include "io/serial.h"
  49
  50
  51 typedef enum {
  52     BAUDRATE_NORMAL = 19200,
  53     BAUDRATE_SIMONK = 28800, // = 9600 * 3
  54     BAUDRATE_KISS   = 38400,
  55     BAUDRATE_CASTLE = 18880
  56 } escBaudRate_e;
  57
  58 #define RX_TOTAL_BITS 10
  59 #define TX_TOTAL_BITS 10
  60
  61 #define MAX_ESCSERIAL_PORTS 1
  62 static serialPort_t *escPort = NULL;
  63 static serialPort_t *passPort = NULL;
  64
  65 #define ICPOLARITY_RISING true
  66 #define ICPOLARITY_FALLING false
  67
  68 typedef struct escSerial_s {
  69     serialPort_t     port;
  70
  71     IO_t rxIO;
  72     IO_t txIO;
  73
  74     const timerHardware_t *rxTimerHardware;
  75     volatile uint8_t rxBuffer[ESCSERIAL_BUFFER_SIZE];
  76     const timerHardware_t *txTimerHardware;
  77     volatile uint8_t txBuffer[ESCSERIAL_BUFFER_SIZE];
  78
  79 #ifdef USE_HAL_DRIVER
  80     const TIM_HandleTypeDef *txTimerHandle;
  81     const TIM_HandleTypeDef *rxTimerHandle;
  82 #endif
  83
  84     uint8_t          isSearchingForStartBit;
  85     uint8_t          rxBitIndex;
  86     uint8_t          rxLastLeadingEdgeAtBitIndex;
  87     uint8_t          rxEdge;
  88
  89     uint8_t          isTransmittingData;
  90     uint8_t          isReceivingData;
  91     int8_t           bitsLeftToTransmit;
  92
  93     uint16_t         internalTxBuffer;  // includes start and stop bits
  94     uint16_t         internalRxBuffer;  // includes start and stop bits
  95
  96     uint16_t         receiveTimeout;
  97     uint16_t         transmissionErrors;
  98     uint16_t         receiveErrors;
  99
 100     uint8_t          escSerialPortIndex;
 101     uint8_t          mode;
 102     uint8_t          outputCount;
 103
 104     timerCCHandlerRec_t timerCb;
 105     timerCCHandlerRec_t edgeCb;
 106 } escSerial_t;
 107
 108 typedef struct {
 109     IO_t io;
 110     uint8_t inverted;
 111 } escOutputs_t;
 112
 113 escOutputs_t escOutputs[MAX_SUPPORTED_MOTORS];
 114
 115 extern timerHardware_t* serialTimerHardware;
 116
 117 const struct serialPortVTable escSerialVTable[];
 118
 119 escSerial_t escSerialPorts[MAX_ESCSERIAL_PORTS];
 120
 121 PG_REGISTER_WITH_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig, PG_ESCSERIAL_CONFIG, 0);
 122
 123 #ifndef ESCSERIAL_TIMER_TX_PIN
 124 #define ESCSERIAL_TIMER_TX_PIN NONE
 125 #endif
 126
 127 PG_RESET_TEMPLATE(escSerialConfig_t, escSerialConfig,
 128     .ioTag = IO_TAG(ESCSERIAL_TIMER_TX_PIN),
 129 );
 130
 131 enum {
 132     TRAILING,
 133     LEADING
 134 };
 135
 136 #define STOP_BIT_MASK (1 << 0)
 137 #define START_BIT_MASK (1 << (RX_TOTAL_BITS - 1))
 138
 139 // XXX No TIM_DeInit equivalent in HAL driver???
 140 #ifdef USE_HAL_DRIVER
 141 static void TIM_DeInit(TIM_TypeDef *tim)
 142 {
 143     UNUSED(tim);
 144 }
 145 #endif
 146
 147 static void setTxSignalEsc(escSerial_t *escSerial, uint8_t state)
 148 {
 149     if (escSerial->mode == PROTOCOL_KISSALL)
 150     {
 151         for (volatile uint8_t i = 0; i < escSerial->outputCount; i++) {
 152             uint8_t state_temp = state;
 153             if (escOutputs[i].inverted) {
 154                 state_temp ^= ENABLE;
 155             }
 156
 157             if (state_temp) {
 158                 IOHi(escOutputs[i].io);
 159             } else {
 160                 IOLo(escOutputs[i].io);
 161             }
 162         }
 163     }
 164     else
 165     {
 166         if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_INVERTED) {
 167             state ^= ENABLE;
 168         }
 169
 170         if (state) {
 171             IOHi(escSerial->txIO);
 172         } else {
 173             IOLo(escSerial->txIO);
 174         }
 175     }
 176 }
 177
 178 static void escSerialGPIOConfig(const timerHardware_t *timhw, ioConfig_t cfg)
 179 {
 180     ioTag_t tag = timhw->tag;
 181
 182     if (!tag) {
 183         return;
 184     }
 185
 186     IOInit(IOGetByTag(tag), OWNER_MOTOR, 0);
 187 #ifdef STM32F7
 188     IOConfigGPIOAF(IOGetByTag(tag), cfg, timhw->alternateFunction);
 189 #else
 190     IOConfigGPIO(IOGetByTag(tag), cfg);
 191 #endif
 192 }
 193
 194 static void escSerialInputPortConfig(const timerHardware_t *timerHardwarePtr)
 195 {
 196 #ifdef STM32F10X
 197     escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
 198 #else
 199     escSerialGPIOConfig(timerHardwarePtr, IOCFG_AF_PP_UP);
 200 #endif
 201     timerChClearCCFlag(timerHardwarePtr);
 202     timerChITConfig(timerHardwarePtr,ENABLE);
 203 }
 204
 205
 206 static bool isTimerPeriodTooLarge(uint32_t timerPeriod)
 207 {
 208     return timerPeriod > 0xFFFF;
 209 }
 210
 211 static bool isEscSerialTransmitBufferEmpty(const serialPort_t *instance)
 212 {
 213     // start listening
 214     return instance->txBufferHead == instance->txBufferTail;
 215 }
 216
 217 static void escSerialOutputPortConfig(const timerHardware_t *timerHardwarePtr)
 218 {
 219     escSerialGPIOConfig(timerHardwarePtr, IOCFG_OUT_PP);
 220     timerChITConfig(timerHardwarePtr,DISABLE);
 221 }
 222
 223 static void processTxStateBL(escSerial_t *escSerial)
 224 {
 225     uint8_t mask;
 226     if (escSerial->isReceivingData) {
 227         return;
 228     }
 229
 230     if (!escSerial->isTransmittingData) {
 231         char byteToSend;
 232         if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
 233             // canreceive
 234             return;
 235         }
 236
 237         // data to send
 238         byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
 239         if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
 240             escSerial->port.txBufferTail = 0;
 241         }
 242
 243         // build internal buffer, MSB = Stop Bit (1) + data bits (MSB to LSB) + start bit(0) LSB
 244         escSerial->internalTxBuffer = (1 << (TX_TOTAL_BITS - 1)) | (byteToSend << 1);
 245         escSerial->bitsLeftToTransmit = TX_TOTAL_BITS;
 246         escSerial->isTransmittingData = true;
 247
 248
 249         //set output
 250         if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE) {
 251             escSerialOutputPortConfig(escSerial->rxTimerHardware);
 252         }
 253         return;
 254     }
 255
 256     if (escSerial->bitsLeftToTransmit) {
 257         mask = escSerial->internalTxBuffer & 1;
 258         escSerial->internalTxBuffer >>= 1;
 259
 260         setTxSignalEsc(escSerial, mask);
 261         escSerial->bitsLeftToTransmit--;
 262         return;
 263     }
 264
 265     escSerial->isTransmittingData = false;
 266     if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
 267         if (escSerial->mode==PROTOCOL_BLHELI || escSerial->mode==PROTOCOL_CASTLE)
 268         {
 269             escSerialInputPortConfig(escSerial->rxTimerHardware);
 270         }
 271     }
 272 }
 273
 274 static void extractAndStoreRxByteBL(escSerial_t *escSerial)
 275 {
 276     if ((escSerial->port.mode & MODE_RX) == 0) {
 277         return;
 278     }
 279
 280     uint8_t haveStartBit = (escSerial->internalRxBuffer & START_BIT_MASK) == 0;
 281     uint8_t haveStopBit = (escSerial->internalRxBuffer & STOP_BIT_MASK) == 1;
 282
 283     if (!haveStartBit || !haveStopBit) {
 284         escSerial->receiveErrors++;
 285         return;
 286     }
 287
 288     uint8_t rxByte = (escSerial->internalRxBuffer >> 1) & 0xFF;
 289
 290     if (escSerial->port.rxCallback) {
 291         escSerial->port.rxCallback(rxByte, escSerial->port.rxCallbackData);
 292     } else {
 293         escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
 294         escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
 295     }
 296 }
 297
 298 static void prepareForNextRxByteBL(escSerial_t *escSerial)
 299 {
 300     // prepare for next byte
 301     escSerial->rxBitIndex = 0;
 302     escSerial->isSearchingForStartBit = true;
 303     if (escSerial->rxEdge == LEADING) {
 304         escSerial->rxEdge = TRAILING;
 305         timerChConfigIC(
 306             escSerial->rxTimerHardware,
 307             (escSerial->port.options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0
 308         );
 309     }
 310 }
 311
 312 static void applyChangedBitsBL(escSerial_t *escSerial)
 313 {
 314     if (escSerial->rxEdge == TRAILING) {
 315         uint8_t bitToSet;
 316         for (bitToSet = escSerial->rxLastLeadingEdgeAtBitIndex; bitToSet < escSerial->rxBitIndex; bitToSet++) {
 317             escSerial->internalRxBuffer |= 1 << bitToSet;
 318         }
 319     }
 320 }
 321
 322 static void processRxStateBL(escSerial_t *escSerial)
 323 {
 324     if (escSerial->isSearchingForStartBit) {
 325         return;
 326     }
 327
 328     escSerial->rxBitIndex++;
 329
 330     if (escSerial->rxBitIndex == RX_TOTAL_BITS - 1) {
 331         applyChangedBitsBL(escSerial);
 332         return;
 333     }
 334
 335     if (escSerial->rxBitIndex == RX_TOTAL_BITS) {
 336
 337         if (escSerial->rxEdge == TRAILING) {
 338             escSerial->internalRxBuffer |= STOP_BIT_MASK;
 339         }
 340
 341         extractAndStoreRxByteBL(escSerial);
 342         prepareForNextRxByteBL(escSerial);
 343     }
 344 }
 345
 346 static void onSerialTimerBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 347 {
 348     UNUSED(capture);
 349     escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
 350
 351     processTxStateBL(escSerial);
 352     processRxStateBL(escSerial);
 353 }
 354
 355 static void serialTimerTxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, uint32_t baud)
 356 {
 357     uint32_t clock = SystemCoreClock/2;
 358     uint32_t timerPeriod;
 359     TIM_DeInit(timerHardwarePtr->tim);
 360     do {
 361         timerPeriod = clock / baud;
 362         if (isTimerPeriodTooLarge(timerPeriod)) {
 363             if (clock > 1) {
 364                 clock = clock / 2;   // this is wrong - mhz stays the same ... This will double baudrate until ok (but minimum baudrate is < 1200)
 365             } else {
 366                 // TODO unable to continue, unable to determine clock and timerPeriods for the given baud
 367             }
 368
 369         }
 370     } while (isTimerPeriodTooLarge(timerPeriod));
 371
 372     timerConfigure(timerHardwarePtr, timerPeriod, clock);
 373     timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerBL);
 374     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 375 }
 376
 377 static void onSerialRxPinChangeBL(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 378 {
 379     UNUSED(capture);
 380
 381     escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
 382     bool inverted = escSerial->port.options & SERIAL_INVERTED;
 383
 384     if ((escSerial->port.mode & MODE_RX) == 0) {
 385         return;
 386     }
 387
 388     if (escSerial->isSearchingForStartBit) {
 389         // Adjust the timing so it will interrupt on the middle.
 390         // This is clobbers transmission, but it is okay because we are
 391         // always half-duplex.
 392 #ifdef USE_HAL_DRIVER
 393         __HAL_TIM_SetCounter(escSerial->txTimerHandle, __HAL_TIM_GetAutoreload(escSerial->txTimerHandle) / 2);
 394 #else
 395         TIM_SetCounter(escSerial->txTimerHardware->tim, escSerial->txTimerHardware->tim->ARR / 2);
 396 #endif
 397         if (escSerial->isTransmittingData) {
 398             escSerial->transmissionErrors++;
 399         }
 400
 401         timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
 402         escSerial->rxEdge = LEADING;
 403
 404         escSerial->rxBitIndex = 0;
 405         escSerial->rxLastLeadingEdgeAtBitIndex = 0;
 406         escSerial->internalRxBuffer = 0;
 407         escSerial->isSearchingForStartBit = false;
 408         return;
 409     }
 410
 411     if (escSerial->rxEdge == LEADING) {
 412         escSerial->rxLastLeadingEdgeAtBitIndex = escSerial->rxBitIndex;
 413     }
 414
 415     applyChangedBitsBL(escSerial);
 416
 417     if (escSerial->rxEdge == TRAILING) {
 418         escSerial->rxEdge = LEADING;
 419         timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_FALLING : ICPOLARITY_RISING, 0);
 420     } else {
 421         escSerial->rxEdge = TRAILING;
 422         timerChConfigIC(escSerial->rxTimerHardware, inverted ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
 423     }
 424 }
 425
 426 static void serialTimerRxConfigBL(const timerHardware_t *timerHardwarePtr, uint8_t reference, portOptions_e options)
 427 {
 428     // start bit is usually a FALLING signal
 429     TIM_DeInit(timerHardwarePtr->tim);
 430     timerConfigure(timerHardwarePtr, 0xFFFF, SystemCoreClock / 2);
 431     timerChConfigIC(timerHardwarePtr, (options & SERIAL_INVERTED) ? ICPOLARITY_RISING : ICPOLARITY_FALLING, 0);
 432     timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeBL);
 433     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
 434 }
 435
 436 #ifdef USE_ESCSERIAL_SIMONK
 437 static void processTxStateEsc(escSerial_t *escSerial)
 438 {
 439     uint8_t mask;
 440     static uint8_t bitq=0, transmitStart=0;
 441     if (escSerial->isReceivingData) {
 442         return;
 443     }
 444
 445     if (transmitStart==0)
 446     {
 447         setTxSignalEsc(escSerial, 1);
 448     }
 449     if (!escSerial->isTransmittingData) {
 450         char byteToSend;
 451 reload:
 452         if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
 453             // canreceive
 454             transmitStart=0;
 455             return;
 456         }
 457
 458         if (transmitStart<3)
 459         {
 460             if (transmitStart==0)
 461                 byteToSend = 0xff;
 462             if (transmitStart==1)
 463                 byteToSend = 0xff;
 464             if (transmitStart==2)
 465                 byteToSend = 0x7f;
 466             transmitStart++;
 467         }
 468         else{
 469             // data to send
 470             byteToSend = escSerial->port.txBuffer[escSerial->port.txBufferTail++];
 471             if (escSerial->port.txBufferTail >= escSerial->port.txBufferSize) {
 472                 escSerial->port.txBufferTail = 0;
 473             }
 474         }
 475
 476
 477         // build internal buffer, data bits (MSB to LSB)
 478         escSerial->internalTxBuffer = byteToSend;
 479         escSerial->bitsLeftToTransmit = 8;
 480         escSerial->isTransmittingData = true;
 481
 482         //set output
 483         escSerialOutputPortConfig(escSerial->rxTimerHardware);
 484         return;
 485     }
 486
 487     if (escSerial->bitsLeftToTransmit) {
 488         mask = escSerial->internalTxBuffer & 1;
 489         if (mask)
 490         {
 491             if (bitq==0 || bitq==1)
 492             {
 493                 setTxSignalEsc(escSerial, 1);
 494             }
 495             if (bitq==2 || bitq==3)
 496             {
 497                 setTxSignalEsc(escSerial, 0);
 498             }
 499         }
 500         else
 501         {
 502             if (bitq==0 || bitq==2)
 503             {
 504                 setTxSignalEsc(escSerial, 1);
 505             }
 506             if (bitq==1 ||bitq==3)
 507             {
 508                 setTxSignalEsc(escSerial, 0);
 509             }
 510         }
 511         bitq++;
 512         if (bitq>3)
 513         {
 514             escSerial->internalTxBuffer >>= 1;
 515             escSerial->bitsLeftToTransmit--;
 516             bitq=0;
 517             if (escSerial->bitsLeftToTransmit==0)
 518             {
 519                 goto reload;
 520             }
 521         }
 522         return;
 523     }
 524
 525     if (isEscSerialTransmitBufferEmpty((serialPort_t *)escSerial)) {
 526         escSerial->isTransmittingData = false;
 527         escSerialInputPortConfig(escSerial->rxTimerHardware);
 528     }
 529 }
 530
 531 static void onSerialTimerEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 532 {
 533     UNUSED(capture);
 534     escSerial_t *escSerial = container_of(cbRec, escSerial_t, timerCb);
 535
 536     if (escSerial->isReceivingData)
 537     {
 538         escSerial->receiveTimeout++;
 539         if (escSerial->receiveTimeout>8)
 540         {
 541             escSerial->isReceivingData=0;
 542             escSerial->receiveTimeout=0;
 543             timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_FALLING, 0);
 544         }
 545     }
 546
 547     processTxStateEsc(escSerial);
 548 }
 549
 550 static void escSerialTimerTxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
 551 {
 552     uint32_t timerPeriod = 34;
 553     TIM_DeInit(timerHardwarePtr->tim);
 554     timerConfigure(timerHardwarePtr, timerPeriod, MHZ_TO_HZ(1));
 555     timerChCCHandlerInit(&escSerialPorts[reference].timerCb, onSerialTimerEsc);
 556     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].timerCb, NULL);
 557 }
 558
 559 static void extractAndStoreRxByteEsc(escSerial_t *escSerial)
 560 {
 561     if ((escSerial->port.mode & MODE_RX) == 0) {
 562         return;
 563     }
 564
 565     uint8_t rxByte = (escSerial->internalRxBuffer) & 0xFF;
 566
 567     if (escSerial->port.rxCallback) {
 568         escSerial->port.rxCallback(rxByte, escSerial->port.rxCallbackData);
 569     } else {
 570         escSerial->port.rxBuffer[escSerial->port.rxBufferHead] = rxByte;
 571         escSerial->port.rxBufferHead = (escSerial->port.rxBufferHead + 1) % escSerial->port.rxBufferSize;
 572     }
 573 }
 574
 575 static void onSerialRxPinChangeEsc(timerCCHandlerRec_t *cbRec, captureCompare_t capture)
 576 {
 577     UNUSED(capture);
 578     static uint8_t zerofirst=0;
 579     static uint8_t bits=0;
 580     static uint16_t bytes=0;
 581
 582     escSerial_t *escSerial = container_of(cbRec, escSerial_t, edgeCb);
 583
 584     //clear timer
 585 #ifdef USE_HAL_DRIVER
 586     __HAL_TIM_SetCounter(escSerial->rxTimerHandle, 0);
 587 #else
 588     TIM_SetCounter(escSerial->rxTimerHardware->tim,0);
 589 #endif
 590
 591     if (capture > 40 && capture < 90)
 592     {
 593         zerofirst++;
 594         if (zerofirst>1)
 595         {
 596             zerofirst=0;
 597             escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
 598             bits++;
 599         }
 600     }
 601     else if (capture>90 && capture < 200)
 602     {
 603         zerofirst=0;
 604         escSerial->internalRxBuffer = escSerial->internalRxBuffer>>1;
 605         escSerial->internalRxBuffer |= 0x80;
 606         bits++;
 607     }
 608     else
 609     {
 610         if (!escSerial->isReceivingData)
 611         {
 612             //start
 613             //lets reset
 614
 615             escSerial->isReceivingData = 1;
 616             zerofirst=0;
 617             bytes=0;
 618             bits=1;
 619             escSerial->internalRxBuffer = 0x80;
 620
 621             timerChConfigIC(escSerial->rxTimerHardware, ICPOLARITY_RISING, 0);
 622         }
 623     }
 624     escSerial->receiveTimeout = 0;
 625
 626     if (bits==8)
 627     {
 628         bits=0;
 629         bytes++;
 630         if (bytes>3)
 631         {
 632             extractAndStoreRxByteEsc(escSerial);
 633         }
 634         escSerial->internalRxBuffer=0;
 635     }
 636
 637 }
 638
 639 static void escSerialTimerRxConfig(const timerHardware_t *timerHardwarePtr, uint8_t reference)
 640 {
 641     // start bit is usually a FALLING signal
 642     TIM_DeInit(timerHardwarePtr->tim);
 643     timerConfigure(timerHardwarePtr, 0xFFFF, MHZ_TO_HZ(1));
 644     timerChConfigIC(timerHardwarePtr, ICPOLARITY_FALLING, 0);
 645     timerChCCHandlerInit(&escSerialPorts[reference].edgeCb, onSerialRxPinChangeEsc);
 646     timerChConfigCallbacks(timerHardwarePtr, &escSerialPorts[reference].edgeCb, NULL);
 647 }
 648 #endif
 649
 650 static void resetBuffers(escSerial_t *escSerial)
 651 {
 652     escSerial->port.rxBufferSize = ESCSERIAL_BUFFER_SIZE;
 653     escSerial->port.rxBuffer = escSerial->rxBuffer;
 654     escSerial->port.rxBufferTail = 0;
 655     escSerial->port.rxBufferHead = 0;
 656
 657     escSerial->port.txBuffer = escSerial->txBuffer;
 658     escSerial->port.txBufferSize = ESCSERIAL_BUFFER_SIZE;
 659     escSerial->port.txBufferTail = 0;
 660     escSerial->port.txBufferHead = 0;
 661 }
 662
 663 static serialPort_t *openEscSerial(escSerialPortIndex_e portIndex, serialReceiveCallbackPtr callback, uint16_t output, uint32_t baud, portOptions_e options, uint8_t mode)
 664 {
 665     escSerial_t *escSerial = &(escSerialPorts[portIndex]);
 666
 667     if (mode != PROTOCOL_KISSALL) {
 668         escSerial->rxTimerHardware = &(timerHardware[output]);
 669         // N-Channels can't be used as RX.
 670         if (escSerial->rxTimerHardware->output & TIMER_OUTPUT_N_CHANNEL) {
 671             return NULL;
 672         }
 673 #ifdef USE_HAL_DRIVER
 674         escSerial->rxTimerHandle = timerFindTimerHandle(escSerial->rxTimerHardware->tim);
 675 #endif
 676     }
 677
 678     escSerial->mode = mode;
 679     escSerial->txTimerHardware = timerGetByTag(escSerialConfig()->ioTag);
 680
 681 #ifdef USE_HAL_DRIVER
 682     escSerial->txTimerHandle = timerFindTimerHandle(escSerial->txTimerHardware->tim);
 683 #endif
 684
 685     escSerial->port.vTable = escSerialVTable;
 686     escSerial->port.baudRate = baud;
 687     escSerial->port.mode = MODE_RXTX;
 688     escSerial->port.options = options;
 689     escSerial->port.rxCallback = callback;
 690
 691     resetBuffers(escSerial);
 692
 693     escSerial->isTransmittingData = false;
 694
 695     escSerial->isSearchingForStartBit = true;
 696     escSerial->rxBitIndex = 0;
 697
 698     escSerial->transmissionErrors = 0;
 699     escSerial->receiveErrors = 0;
 700     escSerial->receiveTimeout = 0;
 701
 702     escSerial->escSerialPortIndex = portIndex;
 703
 704     if (mode != PROTOCOL_KISSALL)
 705     {
 706         escSerial->txIO = IOGetByTag(escSerial->rxTimerHardware->tag);
 707         escSerialInputPortConfig(escSerial->rxTimerHardware);
 708         setTxSignalEsc(escSerial, ENABLE);
 709     }
 710     delay(50);
 711
 712 #ifdef USE_ESCSERIAL_SIMONK
 713     if (mode==PROTOCOL_SIMONK) {
 714         escSerialTimerTxConfig(escSerial->txTimerHardware, portIndex);
 715         escSerialTimerRxConfig(escSerial->rxTimerHardware, portIndex);
 716     }
 717     else
 718 #endif
 719     if (mode==PROTOCOL_BLHELI) {
 720         serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
 721         serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
 722     }
 723     else if (mode==PROTOCOL_KISS) {
 724         escSerialOutputPortConfig(escSerial->rxTimerHardware); // rx is the pin used
 725         serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
 726     }
 727     else if (mode==PROTOCOL_KISSALL) {
 728         escSerial->outputCount = 0;
 729         memset(&escOutputs, 0, sizeof(escOutputs));
 730         pwmOutputPort_t *pwmMotors = pwmGetMotors();
 731         for (volatile uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 732             if (pwmMotors[i].enabled) {
 733                 if (pwmMotors[i].io != IO_NONE) {
 734                     for (volatile uint8_t j = 0; j < USABLE_TIMER_CHANNEL_COUNT; j++) {
 735                         if (pwmMotors[i].io == IOGetByTag(timerHardware[j].tag))
 736                         {
 737                             escSerialOutputPortConfig(&timerHardware[j]);
 738                             if (timerHardware[j].output & TIMER_OUTPUT_INVERTED) {
 739                                 escOutputs[escSerial->outputCount].inverted = 1;
 740                             }
 741                             break;
 742                         }
 743                     }
 744                     escOutputs[escSerial->outputCount].io = pwmMotors[i].io;
 745                     escSerial->outputCount++;
 746                 }
 747             }
 748         }
 749         setTxSignalEsc(escSerial, ENABLE);
 750         serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
 751     }
 752     else if (mode == PROTOCOL_CASTLE){
 753         escSerialOutputPortConfig(escSerial->rxTimerHardware);
 754         serialTimerTxConfigBL(escSerial->txTimerHardware, portIndex, baud);
 755         serialTimerRxConfigBL(escSerial->rxTimerHardware, portIndex, options);
 756     }
 757     return &escSerial->port;
 758 }
 759
 760
 761 static void escSerialInputPortDeConfig(const timerHardware_t *timerHardwarePtr)
 762 {
 763     timerChClearCCFlag(timerHardwarePtr);
 764     timerChITConfig(timerHardwarePtr,DISABLE);
 765     escSerialGPIOConfig(timerHardwarePtr, IOCFG_IPU);
 766 }
 767
 768
 769 static void closeEscSerial(escSerialPortIndex_e portIndex, uint8_t mode)
 770 {
 771     escSerial_t *escSerial = &(escSerialPorts[portIndex]);
 772
 773     if (mode != PROTOCOL_KISSALL) {
 774         escSerialInputPortDeConfig(escSerial->rxTimerHardware);
 775         timerChConfigCallbacks(escSerial->rxTimerHardware,NULL,NULL);
 776         TIM_DeInit(escSerial->rxTimerHardware->tim);
 777     }
 778
 779     timerChConfigCallbacks(escSerial->txTimerHardware,NULL,NULL);
 780     TIM_DeInit(escSerial->txTimerHardware->tim);
 781 }
 782
 783 static uint32_t escSerialTotalBytesWaiting(const serialPort_t *instance)
 784 {
 785     if ((instance->mode & MODE_RX) == 0) {
 786         return 0;
 787     }
 788
 789     escSerial_t *s = (escSerial_t *)instance;
 790
 791     return (s->port.rxBufferHead - s->port.rxBufferTail) & (s->port.rxBufferSize - 1);
 792 }
 793
 794 static uint8_t escSerialReadByte(serialPort_t *instance)
 795 {
 796     uint8_t ch;
 797
 798     if ((instance->mode & MODE_RX) == 0) {
 799         return 0;
 800     }
 801
 802     if (escSerialTotalBytesWaiting(instance) == 0) {
 803         return 0;
 804     }
 805
 806     ch = instance->rxBuffer[instance->rxBufferTail];
 807     instance->rxBufferTail = (instance->rxBufferTail + 1) % instance->rxBufferSize;
 808     return ch;
 809 }
 810
 811 static void escSerialWriteByte(serialPort_t *s, uint8_t ch)
 812 {
 813     if ((s->mode & MODE_TX) == 0) {
 814         return;
 815     }
 816
 817     s->txBuffer[s->txBufferHead] = ch;
 818     s->txBufferHead = (s->txBufferHead + 1) % s->txBufferSize;
 819 }
 820
 821 static void escSerialSetBaudRate(serialPort_t *s, uint32_t baudRate)
 822 {
 823     UNUSED(s);
 824     UNUSED(baudRate);
 825 }
 826
 827 static void escSerialSetMode(serialPort_t *instance, portMode_e mode)
 828 {
 829     instance->mode = mode;
 830 }
 831
 832 static uint32_t escSerialTxBytesFree(const serialPort_t *instance)
 833 {
 834     if ((instance->mode & MODE_TX) == 0) {
 835         return 0;
 836     }
 837
 838     escSerial_t *s = (escSerial_t *)instance;
 839
 840     uint8_t bytesUsed = (s->port.txBufferHead - s->port.txBufferTail) & (s->port.txBufferSize - 1);
 841
 842     return (s->port.txBufferSize - 1) - bytesUsed;
 843 }
 844
 845 const struct serialPortVTable escSerialVTable[] = {
 846     {
 847         .serialWrite = escSerialWriteByte,
 848         .serialTotalRxWaiting = escSerialTotalBytesWaiting,
 849         .serialTotalTxFree = escSerialTxBytesFree,
 850         .serialRead = escSerialReadByte,
 851         .serialSetBaudRate = escSerialSetBaudRate,
 852         .isSerialTransmitBufferEmpty = isEscSerialTransmitBufferEmpty,
 853         .setMode = escSerialSetMode,
 854         .setCtrlLineStateCb = NULL,
 855         .setBaudRateCb = NULL,
 856         .writeBuf = NULL,
 857         .beginWrite = NULL,
 858         .endWrite = NULL
 859     }
 860 };
 861
 862 typedef enum {
 863     IDLE,
 864     HEADER_START,
 865     HEADER_M,
 866     HEADER_ARROW,
 867     HEADER_SIZE,
 868     HEADER_CMD,
 869     COMMAND_RECEIVED
 870 } mspState_e;
 871
 872 typedef struct mspPort_s {
 873     uint8_t offset;
 874     uint8_t dataSize;
 875     uint8_t checksum;
 876     uint8_t indRX;
 877     uint8_t inBuf[10];
 878     mspState_e c_state;
 879     uint8_t cmdMSP;
 880 } mspPort_t;
 881
 882 static mspPort_t currentPort;
 883
 884 static bool processExitCommand(uint8_t c)
 885 {
 886     if (currentPort.c_state == IDLE) {
 887         if (c == '$') {
 888             currentPort.c_state = HEADER_START;
 889         } else {
 890             return false;
 891         }
 892     } else if (currentPort.c_state == HEADER_START) {
 893         currentPort.c_state = (c == 'M') ? HEADER_M : IDLE;
 894     } else if (currentPort.c_state == HEADER_M) {
 895         currentPort.c_state = (c == '<') ? HEADER_ARROW : IDLE;
 896     } else if (currentPort.c_state == HEADER_ARROW) {
 897         if (c > 10) {
 898             currentPort.c_state = IDLE;
 899
 900         } else {
 901             currentPort.dataSize = c;
 902             currentPort.offset = 0;
 903             currentPort.checksum = 0;
 904             currentPort.indRX = 0;
 905             currentPort.checksum ^= c;
 906             currentPort.c_state = HEADER_SIZE;
 907         }
 908     } else if (currentPort.c_state == HEADER_SIZE) {
 909         currentPort.cmdMSP = c;
 910         currentPort.checksum ^= c;
 911         currentPort.c_state = HEADER_CMD;
 912     } else if (currentPort.c_state == HEADER_CMD && currentPort.offset < currentPort.dataSize) {
 913         currentPort.checksum ^= c;
 914         currentPort.inBuf[currentPort.offset++] = c;
 915     } else if (currentPort.c_state == HEADER_CMD && currentPort.offset >= currentPort.dataSize) {
 916         if (currentPort.checksum == c) {
 917             currentPort.c_state = COMMAND_RECEIVED;
 918
 919             if ((currentPort.cmdMSP == 0xF4) && (currentPort.dataSize==0))
 920             {
 921                 currentPort.c_state = IDLE;
 922                 return true;
 923             }
 924         } else {
 925             currentPort.c_state = IDLE;
 926         }
 927     }
 928     return false;
 929 }
 930
 931
 932 void escEnablePassthrough(serialPort_t *escPassthroughPort, uint16_t output, uint8_t mode)
 933 {
 934     bool exitEsc = false;
 935     uint8_t motor_output = 0;
 936     LED0_OFF;
 937     LED1_OFF;
 938     //StopPwmAllMotors();
 939     pwmDisableMotors();
 940     passPort = escPassthroughPort;
 941
 942     uint32_t escBaudrate;
 943     switch (mode) {
 944         case PROTOCOL_KISS:
 945             escBaudrate = BAUDRATE_KISS;
 946             break;
 947         case PROTOCOL_CASTLE:
 948             escBaudrate = BAUDRATE_CASTLE;
 949             break;
 950         default:
 951             escBaudrate = BAUDRATE_NORMAL;
 952             break;
 953     }
 954
 955     if ((mode == PROTOCOL_KISS) && (output == 255)) {
 956         motor_output = 255;
 957         mode = PROTOCOL_KISSALL;
 958     }
 959     else {
 960         uint8_t first_output = 0;
 961         for (unsigned i = 0; i < USABLE_TIMER_CHANNEL_COUNT; i++) {
 962             if (timerHardware[i].usageFlags & TIM_USE_MOTOR) {
 963                 first_output = i;
 964                 break;
 965             }
 966         }
 967
 968         //doesn't work with messy timertable
 969         motor_output = first_output + output;
 970         if (motor_output >= USABLE_TIMER_CHANNEL_COUNT) {
 971             return;
 972         }
 973     }
 974
 975     escPort = openEscSerial(ESCSERIAL1, NULL, motor_output, escBaudrate, 0, mode);
 976
 977     if (!escPort) {
 978         return;
 979     }
 980
 981     uint8_t ch;
 982     while (1) {
 983         if (mode!=2)
 984         {
 985             if (serialRxBytesWaiting(escPort)) {
 986                 LED0_ON;
 987                 while (serialRxBytesWaiting(escPort))
 988                 {
 989                     ch = serialRead(escPort);
 990                     serialWrite(escPassthroughPort, ch);
 991                 }
 992                 LED0_OFF;
 993             }
 994         }
 995         if (serialRxBytesWaiting(escPassthroughPort)) {
 996             LED1_ON;
 997             while (serialRxBytesWaiting(escPassthroughPort))
 998             {
 999                 ch = serialRead(escPassthroughPort);
1000                 exitEsc = processExitCommand(ch);
1001                 if (exitEsc)
1002                 {
1003                     serialWrite(escPassthroughPort, 0x24);
1004                     serialWrite(escPassthroughPort, 0x4D);
1005                     serialWrite(escPassthroughPort, 0x3E);
1006                     serialWrite(escPassthroughPort, 0x00);
1007                     serialWrite(escPassthroughPort, 0xF4);
1008                     serialWrite(escPassthroughPort, 0xF4);
1009                     closeEscSerial(ESCSERIAL1, mode);
1010                     return;
1011                 }
1012                 if (mode==PROTOCOL_BLHELI) {
1013                     serialWrite(escPassthroughPort, ch); // blheli loopback
1014                 }
1015                 serialWrite(escPort, ch);
1016             }
1017             LED1_OFF;
1018         }
1019         if (mode != PROTOCOL_CASTLE) {
1020             delay(5);
1021         }
1022     }
1023 }
1024
1025 #endif