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