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