src/platform/AT32/dshot_bitbang.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 <stdint.h>
  22 #include <math.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #ifdef USE_DSHOT_BITBANG
  28
  29 #include "build/debug.h"
  30 #include "build/debug_pin.h"
  31
  32 #include "drivers/io.h"
  33 #include "drivers/io_impl.h"
  34 #include "drivers/dma.h"
  35 #include "drivers/dma_reqmap.h"
  36 #include "drivers/dshot.h"
  37 #include "drivers/dshot_bitbang.h"
  38 #include "drivers/dshot_bitbang_impl.h"
  39 #include "drivers/dshot_command.h"
  40 #include "drivers/motor.h"
  41 #include "drivers/nvic.h"
  42 #include "drivers/pwm_output.h" // XXX for pwmOutputPort_t motors[]; should go away with refactoring
  43 #include "drivers/dshot_dpwm.h" // XXX for motorDmaOutput_t *getMotorDmaOutput(uint8_t index); should go away with refactoring
  44 #include "drivers/dshot_bitbang_decode.h"
  45 #include "drivers/time.h"
  46 #include "drivers/timer.h"
  47
  48 #include "pg/motor.h"
  49
  50 // Maximum time to wait for telemetry reception to complete
  51 #define DSHOT_TELEMETRY_TIMEOUT 2000
  52
  53 FAST_DATA_ZERO_INIT bbPacer_t bbPacers[MAX_MOTOR_PACERS];  // TIM1 or TIM8
  54 FAST_DATA_ZERO_INIT int usedMotorPacers = 0;
  55
  56 FAST_DATA_ZERO_INIT bbPort_t bbPorts[MAX_SUPPORTED_MOTOR_PORTS];
  57 FAST_DATA_ZERO_INIT int usedMotorPorts;
  58
  59 FAST_DATA_ZERO_INIT bbMotor_t bbMotors[MAX_SUPPORTED_MOTORS];
  60
  61 static FAST_DATA_ZERO_INIT int motorCount;
  62 dshotBitbangStatus_e bbStatus;
  63
  64 // For MCUs that use MPU to control DMA coherency, there might be a performance hit
  65 // on manipulating input buffer content especially if it is read multiple times,
  66 // as the buffer region is attributed as not cachable.
  67 // If this is not desirable, we should use manual cache invalidation.
  68 #ifdef USE_DSHOT_CACHE_MGMT
  69 #define BB_OUTPUT_BUFFER_ATTRIBUTE DMA_RW_AXI __attribute__((aligned(32)))
  70 #define BB_INPUT_BUFFER_ATTRIBUTE  DMA_RW_AXI __attribute__((aligned(32)))
  71 #else
  72 #define BB_OUTPUT_BUFFER_ATTRIBUTE FAST_DATA_ZERO_INIT
  73 #define BB_INPUT_BUFFER_ATTRIBUTE  FAST_DATA_ZERO_INIT
  74 #endif // USE_DSHOT_CACHE_MGMT
  75
  76 BB_OUTPUT_BUFFER_ATTRIBUTE uint32_t bbOutputBuffer[MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
  77 BB_INPUT_BUFFER_ATTRIBUTE uint16_t bbInputBuffer[DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH * MAX_SUPPORTED_MOTOR_PORTS];
  78
  79 uint8_t bbPuPdMode;
  80 FAST_DATA_ZERO_INIT timeUs_t dshotFrameUs;
  81
  82 const timerHardware_t bbTimerHardware[] = {
  83     DEF_TIM(TMR8,  CH1, NONE, 0, 0, 0),
  84     DEF_TIM(TMR8,  CH2, NONE, 0, 1, 0),
  85     DEF_TIM(TMR8,  CH3, NONE, 0, 2, 0),
  86     DEF_TIM(TMR8,  CH4, NONE, 0, 3, 0),
  87     DEF_TIM(TMR1,  CH1, NONE, 0, 0, 0),
  88     DEF_TIM(TMR1,  CH2, NONE, 0, 1, 0),
  89     DEF_TIM(TMR1,  CH3, NONE, 0, 2, 0),
  90     DEF_TIM(TMR1,  CH4, NONE, 0, 3, 0),
  91 };
  92
  93 static FAST_DATA_ZERO_INIT motorDevice_t bbDevice;
  94 static FAST_DATA_ZERO_INIT timeUs_t lastSendUs;
  95
  96 static motorPwmProtocolTypes_e motorPwmProtocol;
  97
  98 // DMA GPIO output buffer formatting
  99
 100 static void bbOutputDataInit(uint32_t *buffer, uint16_t portMask, bool inverted)
 101 {
 102     uint32_t resetMask;
 103     uint32_t setMask;
 104
 105     if (inverted) {
 106         resetMask = portMask;
 107         setMask = (portMask << 16);
 108     } else {
 109         resetMask = (portMask << 16);
 110         setMask = portMask;
 111     }
 112
 113     int symbol_index;
 114
 115     for (symbol_index = 0; symbol_index < MOTOR_DSHOT_FRAME_BITS; symbol_index++) {
 116         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 0] |= setMask ; // Always set all ports
 117         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 1] = 0;          // Reset bits are port dependent
 118         buffer[symbol_index * MOTOR_DSHOT_STATE_PER_SYMBOL + 2] |= resetMask; // Always reset all ports
 119     }
 120
 121     //
 122     // output one more 'bit' that keeps the line level at idle to allow the ESC to sample the last bit
 123     //
 124     // Avoid CRC errors in the case of bi-directional d-shot.  CRC errors can occur if the output is
 125     // transitioned to an input before the signal has been sampled by the ESC as the sampled voltage
 126     // may be somewhere between logic-high and logic-low depending on how the motor output line is
 127     // driven or floating.  On some MCUs it's observed that the voltage momentarily drops low on transition
 128     // to input.
 129
 130     int hold_bit_index = MOTOR_DSHOT_FRAME_BITS * MOTOR_DSHOT_STATE_PER_SYMBOL;
 131     buffer[hold_bit_index + 0] |= resetMask; // Always reset all ports
 132     buffer[hold_bit_index + 1] = 0;          // Never any change
 133     buffer[hold_bit_index + 2] = 0;          // Never any change
 134 }
 135
 136 static void bbOutputDataSet(uint32_t *buffer, int pinNumber, uint16_t value, bool inverted)
 137 {
 138     uint32_t middleBit;
 139
 140     if (inverted) {
 141         middleBit = (1 << (pinNumber + 0));
 142     } else {
 143         middleBit = (1 << (pinNumber + 16));
 144     }
 145
 146     for (int pos = 0; pos < 16; pos++) {
 147         if (!(value & 0x8000)) {
 148             buffer[pos * 3 + 1] |= middleBit;
 149         }
 150         value <<= 1;
 151     }
 152 }
 153
 154 static void bbOutputDataClear(uint32_t *buffer)
 155 {
 156     // Middle position to no change
 157     for (int bitpos = 0; bitpos < 16; bitpos++) {
 158         buffer[bitpos * 3 + 1] = 0;
 159     }
 160 }
 161
 162 // bbPacer management
 163
 164 static bbPacer_t *bbFindMotorPacer(TIM_TypeDef *tim)
 165 {
 166     for (int i = 0; i < MAX_MOTOR_PACERS; i++) {
 167
 168         bbPacer_t *bbPacer = &bbPacers[i];
 169
 170         if (bbPacer->tim == NULL) {
 171             bbPacer->tim = tim;
 172             ++usedMotorPacers;
 173             return bbPacer;
 174         }
 175
 176         if (bbPacer->tim == tim) {
 177             return bbPacer;
 178         }
 179     }
 180
 181     return NULL;
 182 }
 183
 184 // bbPort management
 185
 186 static bbPort_t *bbFindMotorPort(int portIndex)
 187 {
 188     for (int i = 0; i < usedMotorPorts; i++) {
 189         if (bbPorts[i].portIndex == portIndex) {
 190             return &bbPorts[i];
 191         }
 192     }
 193     return NULL;
 194 }
 195
 196 static bbPort_t *bbAllocateMotorPort(int portIndex)
 197 {
 198     if (usedMotorPorts >= MAX_SUPPORTED_MOTOR_PORTS) {
 199         bbStatus = DSHOT_BITBANG_STATUS_TOO_MANY_PORTS;
 200         return NULL;
 201     }
 202
 203     bbPort_t *bbPort = &bbPorts[usedMotorPorts];
 204
 205     if (!bbPort->timhw) {
 206         // No more pacer channel available
 207         bbStatus = DSHOT_BITBANG_STATUS_NO_PACER;
 208         return NULL;
 209     }
 210
 211     bbPort->portIndex = portIndex;
 212     bbPort->owner.owner = OWNER_DSHOT_BITBANG;
 213     bbPort->owner.resourceIndex = RESOURCE_INDEX(portIndex);
 214
 215     ++usedMotorPorts;
 216
 217     return bbPort;
 218 }
 219
 220 const timerHardware_t *dshotBitbangTimerGetAllocatedByNumberAndChannel(int8_t timerNumber, uint16_t timerChannel)
 221 {
 222     for (int index = 0; index < usedMotorPorts; index++) {
 223         const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
 224         if (bitbangTimer && timerGetTIMNumber(bitbangTimer->tim) == timerNumber && bitbangTimer->channel == timerChannel && bbPorts[index].owner.owner) {
 225             return bitbangTimer;
 226         }
 227     }
 228
 229     return NULL;
 230 }
 231
 232 const resourceOwner_t *dshotBitbangTimerGetOwner(const timerHardware_t *timer)
 233 {
 234     for (int index = 0; index < usedMotorPorts; index++) {
 235         const timerHardware_t *bitbangTimer = bbPorts[index].timhw;
 236         if (bitbangTimer && bitbangTimer == timer) {
 237             return &bbPorts[index].owner;
 238         }
 239     }
 240
 241     return &freeOwner;
 242 }
 243
 244 // Return frequency of smallest change [state/sec]
 245
 246 static uint32_t getDshotBaseFrequency(motorPwmProtocolTypes_e pwmProtocolType)
 247 {
 248     switch (pwmProtocolType) {
 249     case(PWM_TYPE_DSHOT600):
 250         return MOTOR_DSHOT600_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 251     case(PWM_TYPE_DSHOT300):
 252         return MOTOR_DSHOT300_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 253     default:
 254     case(PWM_TYPE_DSHOT150):
 255         return MOTOR_DSHOT150_SYMBOL_RATE * MOTOR_DSHOT_STATE_PER_SYMBOL;
 256     }
 257 }
 258
 259 static void bbSetupDma(bbPort_t *bbPort)
 260 {
 261     const dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(bbPort->dmaResource);
 262     dmaEnable(dmaIdentifier);
 263     bbPort->dmaSource = timerDmaSource(bbPort->timhw->channel);
 264
 265     bbPacer_t *bbPacer = bbFindMotorPacer(bbPort->timhw->tim);
 266     bbPacer->dmaSources |= bbPort->dmaSource;
 267
 268     dmaSetHandler(dmaIdentifier, bbDMAIrqHandler, NVIC_BUILD_PRIORITY(2, 1), (uint32_t)bbPort);
 269
 270     dmaMuxEnable(dmaIdentifier, bbPort->dmaChannel);
 271
 272     bbDMA_ITConfig(bbPort);
 273 }
 274
 275 FAST_IRQ_HANDLER void bbDMAIrqHandler(dmaChannelDescriptor_t *descriptor)
 276 {
 277     dbgPinHi(0);
 278
 279     bbPort_t *bbPort = (bbPort_t *)descriptor->userParam;
 280
 281     bbDMA_Cmd(bbPort, FALSE);
 282
 283     bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, FALSE);
 284
 285     if (DMA_GET_FLAG_STATUS(descriptor, DMA_IT_TEIF)) {
 286         while (1) {};
 287     }
 288
 289     DMA_CLEAR_FLAG(descriptor, DMA_IT_TCIF);
 290
 291 #ifdef USE_DSHOT_TELEMETRY
 292     if (useDshotTelemetry) {
 293         if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
 294 #ifdef DEBUG_COUNT_INTERRUPT
 295             bbPort->inputIrq++;
 296 #endif
 297             // Disable DMA as telemetry reception is complete
 298             bbDMA_Cmd(bbPort, FALSE);
 299         } else {
 300 #ifdef DEBUG_COUNT_INTERRUPT
 301             bbPort->outputIrq++;
 302 #endif
 303
 304             // Switch to input
 305
 306             bbSwitchToInput(bbPort);
 307
 308             bbTIM_DMACmd(bbPort->timhw->tim, bbPort->dmaSource, TRUE);
 309         }
 310     }
 311 #endif
 312     dbgPinLo(0);
 313 }
 314
 315 // Setup bbPorts array elements so that they each have a TIM1 or TIM8 channel
 316 // in timerHardware array for BB-DShot.
 317
 318 static void bbFindPacerTimer(void)
 319 {
 320     for (int bbPortIndex = 0; bbPortIndex < MAX_SUPPORTED_MOTOR_PORTS; bbPortIndex++) {
 321         for (unsigned timerIndex = 0; timerIndex < ARRAYLEN(bbTimerHardware); timerIndex++) {
 322             const timerHardware_t *timer = &bbTimerHardware[timerIndex];
 323             int timNumber = timerGetTIMNumber(timer->tim);
 324             if ((motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM1 && timNumber != 1)
 325                 || (motorConfig()->dev.useDshotBitbangedTimer == DSHOT_BITBANGED_TIMER_TIM8 && timNumber != 8)) {
 326                 continue;
 327             }
 328             bool timerConflict = false;
 329             for (int channel = 0; channel < CC_CHANNELS_PER_TIMER; channel++) {
 330                 const timerHardware_t *timer = timerGetAllocatedByNumberAndChannel(timNumber, CC_CHANNEL_FROM_INDEX(channel));
 331                 const resourceOwner_e timerOwner = timerGetOwner(timer)->owner;
 332                 if (timerOwner != OWNER_FREE && timerOwner != OWNER_DSHOT_BITBANG) {
 333                     timerConflict = true;
 334                     break;
 335                 }
 336             }
 337
 338             for (int index = 0; index < bbPortIndex; index++) {
 339                 const timerHardware_t* t = bbPorts[index].timhw;
 340                 if (timerGetTIMNumber(t->tim) == timNumber && timer->channel == t->channel) {
 341                     timerConflict = true;
 342                     break;
 343                 }
 344             }
 345
 346             if (timerConflict) {
 347                 continue;
 348             }
 349
 350 #ifdef USE_DMA_SPEC
 351             dmaoptValue_t dmaopt = dmaGetOptionByTimer(timer);
 352             const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timer->tim, timer->channel, dmaopt);
 353             dmaResource_t *dma = dmaChannelSpec->ref;
 354 #else
 355             dmaResource_t *dma = timer->dmaRef;
 356 #endif
 357             dmaIdentifier_e dmaIdentifier = dmaGetIdentifier(dma);
 358             if (dmaGetOwner(dmaIdentifier)->owner == OWNER_FREE) {
 359                 bbPorts[bbPortIndex].timhw = timer;
 360
 361                 break;
 362             }
 363         }
 364     }
 365 }
 366
 367 static void bbTimebaseSetup(bbPort_t *bbPort, motorPwmProtocolTypes_e dshotProtocolType)
 368 {
 369     uint32_t timerclock = timerClock(bbPort->timhw->tim);
 370
 371     uint32_t outputFreq = getDshotBaseFrequency(dshotProtocolType);
 372     dshotFrameUs = 1000000 * 17 * 3 / outputFreq;
 373     bbPort->outputARR = timerclock / outputFreq - 1;
 374
 375     // XXX Explain this formula
 376     uint32_t inputFreq = outputFreq * 5 * 2 * DSHOT_BITBANG_TELEMETRY_OVER_SAMPLE / 24;
 377     bbPort->inputARR = timerclock / inputFreq - 1;
 378 }
 379
 380 //
 381 // bb only use pin info associated with timerHardware entry designated as TIM_USE_MOTOR;
 382 // it does not use the timer channel associated with the pin.
 383 //
 384
 385 static bool bbMotorConfig(IO_t io, uint8_t motorIndex, motorPwmProtocolTypes_e pwmProtocolType, uint8_t output)
 386 {
 387     int pinIndex = IO_GPIOPinIdx(io);
 388     int portIndex = IO_GPIOPortIdx(io);
 389
 390     bbPort_t *bbPort = bbFindMotorPort(portIndex);
 391
 392     if (!bbPort) {
 393
 394         // New port group
 395
 396         bbPort = bbAllocateMotorPort(portIndex);
 397
 398         if (bbPort) {
 399             const timerHardware_t *timhw = bbPort->timhw;
 400
 401 #ifdef USE_DMA_SPEC
 402             const dmaChannelSpec_t *dmaChannelSpec = dmaGetChannelSpecByTimerValue(timhw->tim, timhw->channel, dmaGetOptionByTimer(timhw));
 403             bbPort->dmaResource = dmaChannelSpec->ref;
 404             bbPort->dmaChannel = dmaChannelSpec->dmaMuxId;
 405 #else
 406             bbPort->dmaResource = timhw->dmaRef;
 407             bbPort->dmaChannel = timhw->dmaChannel;
 408 #endif
 409         }
 410
 411         if (!bbPort || !dmaAllocate(dmaGetIdentifier(bbPort->dmaResource), bbPort->owner.owner, bbPort->owner.resourceIndex)) {
 412             bbDevice.vTable.write = motorWriteNull;
 413             bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
 414             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 415
 416             return false;
 417         }
 418
 419         bbPort->gpio = IO_GPIO(io);
 420
 421         bbPort->portOutputCount = MOTOR_DSHOT_BUF_LENGTH;
 422         bbPort->portOutputBuffer = &bbOutputBuffer[(bbPort - bbPorts) * MOTOR_DSHOT_BUF_CACHE_ALIGN_LENGTH];
 423
 424         bbPort->portInputCount = DSHOT_BB_PORT_IP_BUF_LENGTH;
 425         bbPort->portInputBuffer = &bbInputBuffer[(bbPort - bbPorts) * DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_LENGTH];
 426
 427         bbTimebaseSetup(bbPort, pwmProtocolType);
 428         bbTIM_TimeBaseInit(bbPort, bbPort->outputARR);
 429         bbTimerChannelInit(bbPort);
 430
 431         bbSetupDma(bbPort);
 432         bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_OUTPUT);
 433         bbDMAPreconfigure(bbPort, DSHOT_BITBANG_DIRECTION_INPUT);
 434
 435         bbDMA_ITConfig(bbPort);
 436     }
 437
 438     bbMotors[motorIndex].pinIndex = pinIndex;
 439     bbMotors[motorIndex].io = io;
 440     bbMotors[motorIndex].output = output;
 441     bbMotors[motorIndex].bbPort = bbPort;
 442
 443     IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 444
 445     // Setup GPIO_MODER and GPIO_ODR register manipulation values
 446
 447     bbGpioSetup(&bbMotors[motorIndex]);
 448
 449 #ifdef USE_DSHOT_TELEMETRY
 450     if (useDshotTelemetry) {
 451         bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_INVERTED);
 452     } else
 453 #endif
 454     {
 455         bbOutputDataInit(bbPort->portOutputBuffer, (1 << pinIndex), DSHOT_BITBANG_NONINVERTED);
 456     }
 457
 458     bbSwitchToOutput(bbPort);
 459
 460     bbMotors[motorIndex].configured = true;
 461
 462     return true;
 463 }
 464
 465 static bool bbTelemetryWait(void)
 466 {
 467     // Wait for telemetry reception to complete
 468     bool telemetryPending;
 469     bool telemetryWait = false;
 470     const timeUs_t startTimeUs = micros();
 471
 472     do {
 473         telemetryPending = false;
 474         for (int i = 0; i < usedMotorPorts; i++) {
 475             telemetryPending |= bbPorts[i].telemetryPending;
 476         }
 477
 478         telemetryWait |= telemetryPending;
 479
 480         if (cmpTimeUs(micros(), startTimeUs) > DSHOT_TELEMETRY_TIMEOUT) {
 481             break;
 482         }
 483     } while (telemetryPending);
 484
 485     if (telemetryWait) {
 486         DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 2, debug[2] + 1);
 487     }
 488
 489     return telemetryWait;
 490 }
 491
 492 static void bbUpdateInit(void)
 493 {
 494     for (int i = 0; i < usedMotorPorts; i++) {
 495         bbOutputDataClear(bbPorts[i].portOutputBuffer);
 496     }
 497 }
 498
 499 static bool bbDecodeTelemetry(void)
 500 {
 501 #ifdef USE_DSHOT_TELEMETRY
 502     if (useDshotTelemetry) {
 503 #ifdef USE_DSHOT_TELEMETRY_STATS
 504         const timeMs_t currentTimeMs = millis();
 505 #endif
 506
 507 #ifdef USE_DSHOT_CACHE_MGMT
 508         for (int i = 0; i < usedMotorPorts; i++) {
 509             bbPort_t *bbPort = &bbPorts[i];
 510             SCB_InvalidateDCache_by_Addr((uint32_t *)bbPort->portInputBuffer, DSHOT_BB_PORT_IP_BUF_CACHE_ALIGN_BYTES);
 511         }
 512 #endif
 513         for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 514
 515             uint32_t rawValue = decode_bb_bitband(
 516                 bbMotors[motorIndex].bbPort->portInputBuffer,
 517                 bbMotors[motorIndex].bbPort->portInputCount,
 518                 bbMotors[motorIndex].pinIndex);
 519
 520             if (rawValue == DSHOT_TELEMETRY_NOEDGE) {
 521                 DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 1, debug[1] + 1);
 522                 continue;
 523             }
 524             DEBUG_SET(DEBUG_DSHOT_TELEMETRY_COUNTS, 0, debug[0] + 1);
 525             dshotTelemetryState.readCount++;
 526
 527             if (rawValue != DSHOT_TELEMETRY_INVALID) {
 528                 // Check EDT enable or store raw value
 529                 if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(motorIndex) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
 530                     dshotTelemetryState.motorState[motorIndex].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
 531                 } else {
 532                     dshotTelemetryState.motorState[motorIndex].rawValue = rawValue;
 533                 }
 534             } else {
 535                 dshotTelemetryState.invalidPacketCount++;
 536             }
 537 #ifdef USE_DSHOT_TELEMETRY_STATS
 538             updateDshotTelemetryQuality(&dshotTelemetryQuality[motorIndex], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
 539 #endif
 540         }
 541
 542         dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
 543     }
 544 #endif
 545
 546     return true;
 547 }
 548
 549 static void bbWriteInt(uint8_t motorIndex, uint16_t value)
 550 {
 551     bbMotor_t *const bbmotor = &bbMotors[motorIndex];
 552
 553     if (!bbmotor->configured) {
 554         return;
 555     }
 556
 557     // fetch requestTelemetry from motors. Needs to be refactored.
 558     motorDmaOutput_t * const motor = getMotorDmaOutput(motorIndex);
 559     bbmotor->protocolControl.requestTelemetry = motor->protocolControl.requestTelemetry;
 560     motor->protocolControl.requestTelemetry = false;
 561
 562     // If there is a command ready to go overwrite the value and send that instead
 563     if (dshotCommandIsProcessing()) {
 564         value = dshotCommandGetCurrent(motorIndex);
 565         if (value) {
 566             bbmotor->protocolControl.requestTelemetry = true;
 567         }
 568     }
 569
 570     bbmotor->protocolControl.value = value;
 571
 572     uint16_t packet = prepareDshotPacket(&bbmotor->protocolControl);
 573
 574     bbPort_t *bbPort = bbmotor->bbPort;
 575
 576 #ifdef USE_DSHOT_TELEMETRY
 577     if (useDshotTelemetry) {
 578         bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_INVERTED);
 579     } else
 580 #endif
 581     {
 582         bbOutputDataSet(bbPort->portOutputBuffer, bbmotor->pinIndex, packet, DSHOT_BITBANG_NONINVERTED);
 583     }
 584 }
 585
 586 static void bbWrite(uint8_t motorIndex, float value)
 587 {
 588     bbWriteInt(motorIndex, lrintf(value));
 589 }
 590
 591 static void bbUpdateComplete(void)
 592 {
 593     // If there is a dshot command loaded up, time it correctly with motor update
 594
 595     if (!dshotCommandQueueEmpty()) {
 596         if (!dshotCommandOutputIsEnabled(bbDevice.count)) {
 597             return;
 598         }
 599     }
 600
 601 #ifdef USE_DSHOT_CACHE_MGMT
 602     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 603         // Only clean each buffer once. If all motors are on a common port they'll share a buffer.
 604         bool clean = false;
 605         for (int i = 0; i < motorIndex; i++) {
 606             if (bbMotors[motorIndex].bbPort->portOutputBuffer == bbMotors[i].bbPort->portOutputBuffer) {
 607                 clean = true;
 608             }
 609         }
 610         if (!clean) {
 611             SCB_CleanDCache_by_Addr(bbMotors[motorIndex].bbPort->portOutputBuffer, MOTOR_DSHOT_BUF_CACHE_ALIGN_BYTES);
 612         }
 613     }
 614 #endif
 615
 616     for (int i = 0; i < usedMotorPorts; i++) {
 617         bbPort_t *bbPort = &bbPorts[i];
 618
 619 #ifdef USE_DSHOT_TELEMETRY
 620         if (useDshotTelemetry) {
 621             if (bbPort->direction == DSHOT_BITBANG_DIRECTION_INPUT) {
 622                 bbPort->inputActive = false;
 623                 bbSwitchToOutput(bbPort);
 624             }
 625         } else
 626 #endif
 627         {
 628             bbSwitchToOutput(bbPort);
 629         }
 630
 631         bbDMA_Cmd(bbPort, TRUE);
 632     }
 633
 634     lastSendUs = micros();
 635     for (int i = 0; i < usedMotorPacers; i++) {
 636         bbPacer_t *bbPacer = &bbPacers[i];
 637         bbTIM_DMACmd(bbPacer->tim, bbPacer->dmaSources, TRUE);
 638     }
 639 }
 640
 641 static bool bbEnableMotors(void)
 642 {
 643     for (int i = 0; i < motorCount; i++) {
 644         if (bbMotors[i].configured) {
 645             IOConfigGPIO(bbMotors[i].io, bbMotors[i].iocfg);
 646         }
 647     }
 648     return true;
 649 }
 650
 651 static void bbDisableMotors(void)
 652 {
 653     return;
 654 }
 655
 656 static void bbShutdown(void)
 657 {
 658     return;
 659 }
 660
 661 static bool bbIsMotorEnabled(uint8_t index)
 662 {
 663     return bbMotors[index].enabled;
 664 }
 665
 666 static void bbPostInit(void)
 667 {
 668     bbFindPacerTimer();
 669
 670     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 671
 672         if (!bbMotorConfig(bbMotors[motorIndex].io, motorIndex, motorPwmProtocol, bbMotors[motorIndex].output)) {
 673             return;
 674         }
 675
 676         bbMotors[motorIndex].enabled = true;
 677
 678         // Fill in motors structure for 4way access (XXX Should be refactored)
 679
 680         motors[motorIndex].enabled = true;
 681     }
 682 }
 683
 684 static motorVTable_t bbVTable = {
 685     .postInit = bbPostInit,
 686     .enable = bbEnableMotors,
 687     .disable = bbDisableMotors,
 688     .isMotorEnabled = bbIsMotorEnabled,
 689     .telemetryWait = bbTelemetryWait,
 690     .decodeTelemetry = bbDecodeTelemetry,
 691     .updateInit = bbUpdateInit,
 692     .write = bbWrite,
 693     .writeInt = bbWriteInt,
 694     .updateComplete = bbUpdateComplete,
 695     .convertExternalToMotor = dshotConvertFromExternal,
 696     .convertMotorToExternal = dshotConvertToExternal,
 697     .shutdown = bbShutdown,
 698 };
 699
 700 dshotBitbangStatus_e dshotBitbangGetStatus(void)
 701 {
 702     return bbStatus;
 703 }
 704
 705 motorDevice_t *dshotBitbangDevInit(const motorDevConfig_t *motorConfig, uint8_t count)
 706 {
 707     dbgPinLo(0);
 708     dbgPinLo(1);
 709
 710     motorPwmProtocol = motorConfig->motorPwmProtocol;
 711     bbDevice.vTable = bbVTable;
 712     motorCount = count;
 713     bbStatus = DSHOT_BITBANG_STATUS_OK;
 714
 715 #ifdef USE_DSHOT_TELEMETRY
 716     useDshotTelemetry = motorConfig->useDshotTelemetry;
 717 #endif
 718
 719     memset(bbOutputBuffer, 0, sizeof(bbOutputBuffer));
 720
 721     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 722         const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
 723         const timerHardware_t *timerHardware = timerGetConfiguredByTag(motorConfig->ioTags[reorderedMotorIndex]);
 724         const IO_t io = IOGetByTag(motorConfig->ioTags[reorderedMotorIndex]);
 725
 726         uint8_t output = motorConfig->motorPwmInversion ?  timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output;
 727         bbPuPdMode = (output & TIMER_OUTPUT_INVERTED) ? BB_GPIO_PULLDOWN : BB_GPIO_PULLUP;
 728
 729 #ifdef USE_DSHOT_TELEMETRY
 730         if (useDshotTelemetry) {
 731             output ^= TIMER_OUTPUT_INVERTED;
 732         }
 733 #endif
 734
 735         if (!IOIsFreeOrPreinit(io)) {
 736             /* not enough motors initialised for the mixer or a break in the motors */
 737             bbDevice.vTable.write = motorWriteNull;
 738             bbDevice.vTable.decodeTelemetry = motorDecodeTelemetryNull;
 739             bbDevice.vTable.updateComplete = motorUpdateCompleteNull;
 740             bbStatus = DSHOT_BITBANG_STATUS_MOTOR_PIN_CONFLICT;
 741             return NULL;
 742         }
 743
 744         int pinIndex = IO_GPIOPinIdx(io);
 745
 746         bbMotors[motorIndex].pinIndex = pinIndex;
 747         bbMotors[motorIndex].io = io;
 748         bbMotors[motorIndex].output = output;
 749         bbMotors[motorIndex].iocfg = IO_CONFIG(GPIO_MODE_OUTPUT, GPIO_DRIVE_STRENGTH_STRONGER, GPIO_OUTPUT_PUSH_PULL, bbPuPdMode);
 750
 751         IOInit(io, OWNER_MOTOR, RESOURCE_INDEX(motorIndex));
 752         IOConfigGPIO(io, bbMotors[motorIndex].iocfg);
 753         if (output & TIMER_OUTPUT_INVERTED) {
 754             IOLo(io);
 755         } else {
 756             IOHi(io);
 757         }
 758
 759         // Fill in motors structure for 4way access (XXX Should be refactored)
 760         motors[motorIndex].io = bbMotors[motorIndex].io;
 761     }
 762
 763     return &bbDevice;
 764 }
 765
 766 #endif // USE_DSHOT_BB