src/main/drivers/pwm_output_dshot_shared.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
  22 #include <math.h>
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <string.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_DSHOT
  30
  31 #include "build/debug.h"
  32
  33 #include "drivers/dma.h"
  34 #include "drivers/dma_reqmap.h"
  35 #include "drivers/io.h"
  36 #include "drivers/nvic.h"
  37 #include "drivers/rcc.h"
  38 #include "drivers/time.h"
  39 #include "drivers/timer.h"
  40 #if defined(STM32F4)
  41 #include "stm32f4xx.h"
  42 #elif defined(STM32F3)
  43 #include "stm32f30x.h"
  44 #endif
  45
  46 #include "pwm_output.h"
  47 #include "drivers/dshot.h"
  48 #include "drivers/dshot_dpwm.h"
  49 #include "drivers/dshot_command.h"
  50 #include "drivers/motor.h"
  51
  52 #include "pwm_output_dshot_shared.h"
  53
  54 FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount = 0;
  55 #ifdef STM32F7
  56 FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  57 FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  58 #else
  59 motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  60 motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  61 #endif
  62
  63 #ifdef USE_DSHOT_TELEMETRY
  64 FAST_DATA_ZERO_INIT uint32_t inputStampUs;
  65
  66 FAST_DATA_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
  67 #endif
  68
  69 motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
  70 {
  71     return &dmaMotors[index];
  72 }
  73
  74 uint8_t getTimerIndex(TIM_TypeDef *timer)
  75 {
  76     for (int i = 0; i < dmaMotorTimerCount; i++) {
  77         if (dmaMotorTimers[i].timer == timer) {
  78             return i;
  79         }
  80     }
  81     dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
  82     return dmaMotorTimerCount - 1;
  83 }
  84
  85
  86 FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
  87 {
  88     motorDmaOutput_t *const motor = &dmaMotors[index];
  89
  90     if (!motor->configured) {
  91         return;
  92     }
  93
  94     /*If there is a command ready to go overwrite the value and send that instead*/
  95     if (dshotCommandIsProcessing()) {
  96         value = dshotCommandGetCurrent(index);
  97         if (value) {
  98             motor->protocolControl.requestTelemetry = true;
  99         }
 100     }
 101
 102     motor->protocolControl.value = value;
 103
 104     uint16_t packet = prepareDshotPacket(&motor->protocolControl);
 105     uint8_t bufferSize;
 106
 107 #ifdef USE_DSHOT_DMAR
 108     if (useBurstDshot) {
 109         bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet);
 110         motor->timer->dmaBurstLength = bufferSize * 4;
 111     } else
 112 #endif
 113     {
 114         bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet);
 115         motor->timer->timerDmaSources |= motor->timerDmaSource;
 116 #ifdef USE_FULL_LL_DRIVER
 117         xLL_EX_DMA_SetDataLength(motor->dmaRef, bufferSize);
 118         xLL_EX_DMA_EnableResource(motor->dmaRef);
 119 #else
 120         xDMA_SetCurrDataCounter(motor->dmaRef, bufferSize);
 121         xDMA_Cmd(motor->dmaRef, ENABLE);
 122 #endif
 123     }
 124 }
 125
 126
 127 #ifdef USE_DSHOT_TELEMETRY
 128
 129 void dshotEnableChannels(uint8_t motorCount);
 130
 131
 132 static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
 133 {
 134     uint32_t value = 0;
 135     uint32_t oldValue = buffer[0];
 136     int bits = 0;
 137     int len;
 138     for (uint32_t i = 1; i <= count; i++) {
 139         if (i < count) {
 140             int diff = buffer[i] - oldValue;
 141             if (bits >= 21) {
 142                 break;
 143             }
 144             len = (diff + 8) / 16;
 145         } else {
 146             len = 21 - bits;
 147         }
 148
 149         value <<= len;
 150         value |= 1 << (len - 1);
 151         oldValue = buffer[i];
 152         bits += len;
 153     }
 154     if (bits != 21) {
 155         return 0xffff;
 156     }
 157
 158     static const uint32_t decode[32] = {
 159         0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
 160         0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
 161
 162     uint32_t decodedValue = decode[value & 0x1f];
 163     decodedValue |= decode[(value >> 5) & 0x1f] << 4;
 164     decodedValue |= decode[(value >> 10) & 0x1f] << 8;
 165     decodedValue |= decode[(value >> 15) & 0x1f] << 12;
 166
 167     uint32_t csum = decodedValue;
 168     csum = csum ^ (csum >> 8); // xor bytes
 169     csum = csum ^ (csum >> 4); // xor nibbles
 170
 171     if ((csum & 0xf) != 0xf) {
 172         return 0xffff;
 173     }
 174     decodedValue >>= 4;
 175
 176     if (decodedValue == 0x0fff) {
 177         return 0;
 178     }
 179     decodedValue = (decodedValue & 0x000001ff) << ((decodedValue & 0xfffffe00) >> 9);
 180     if (!decodedValue) {
 181         return 0xffff;
 182     }
 183     uint32_t ret = (1000000 * 60 / 100 + decodedValue / 2) / decodedValue;
 184     return ret;
 185 }
 186
 187 #endif
 188
 189 #ifdef USE_DSHOT_TELEMETRY
 190 FAST_CODE_NOINLINE bool pwmStartDshotMotorUpdate(void)
 191 {
 192     if (!useDshotTelemetry) {
 193         return true;
 194     }
 195 #ifdef USE_DSHOT_TELEMETRY_STATS
 196     const timeMs_t currentTimeMs = millis();
 197 #endif
 198     const timeUs_t currentUs = micros();
 199     for (int i = 0; i < dshotPwmDevice.count; i++) {
 200         timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
 201         if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
 202             return false;
 203         }
 204         if (dmaMotors[i].isInput) {
 205 #ifdef USE_FULL_LL_DRIVER
 206             uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
 207 #else
 208             uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
 209 #endif
 210
 211 #ifdef USE_FULL_LL_DRIVER
 212             LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
 213 #else
 214             TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
 215 #endif
 216
 217             uint16_t value = 0xffff;
 218
 219             if (edges > MIN_GCR_EDGES) {
 220                 dshotTelemetryState.readCount++;
 221                 value = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges);
 222
 223 #ifdef USE_DSHOT_TELEMETRY_STATS
 224                 bool validTelemetryPacket = false;
 225 #endif
 226                 if (value != 0xffff) {
 227                     dshotTelemetryState.motorState[i].telemetryValue = value;
 228                     dshotTelemetryState.motorState[i].telemetryActive = true;
 229                     if (i < 4) {
 230                         DEBUG_SET(DEBUG_DSHOT_RPM_TELEMETRY, i, value);
 231                     }
 232 #ifdef USE_DSHOT_TELEMETRY_STATS
 233                     validTelemetryPacket = true;
 234 #endif
 235                 } else {
 236                     dshotTelemetryState.invalidPacketCount++;
 237                     if (i == 0) {
 238                         memcpy(dshotTelemetryState.inputBuffer, dmaMotors[i].dmaBuffer, sizeof(dshotTelemetryState.inputBuffer));
 239                     }
 240                 }
 241 #ifdef USE_DSHOT_TELEMETRY_STATS
 242                 updateDshotTelemetryQuality(&dshotTelemetryQuality[i], validTelemetryPacket, currentTimeMs);
 243 #endif
 244             }
 245         }
 246         pwmDshotSetDirectionOutput(&dmaMotors[i]);
 247     }
 248     inputStampUs = 0;
 249     dshotEnableChannels(dshotPwmDevice.count);
 250     return true;
 251 }
 252
 253 bool isDshotMotorTelemetryActive(uint8_t motorIndex)
 254 {
 255     return dshotTelemetryState.motorState[motorIndex].telemetryActive;
 256 }
 257
 258 bool isDshotTelemetryActive(void)
 259 {
 260     const unsigned motorCount = motorDeviceCount();
 261     if (motorCount) {
 262         for (unsigned i = 0; i < motorCount; i++) {
 263             if (!isDshotMotorTelemetryActive(i)) {
 264                 return false;
 265             }
 266         }
 267         return true;
 268     }
 269     return false;
 270 }
 271
 272 #ifdef USE_DSHOT_TELEMETRY_STATS
 273 int16_t getDshotTelemetryMotorInvalidPercent(uint8_t motorIndex)
 274 {
 275     int16_t invalidPercent = 0;
 276
 277     if (dshotTelemetryState.motorState[motorIndex].telemetryActive) {
 278         const uint32_t totalCount = dshotTelemetryQuality[motorIndex].packetCountSum;
 279         const uint32_t invalidCount = dshotTelemetryQuality[motorIndex].invalidCountSum;
 280         if (totalCount > 0) {
 281             invalidPercent = lrintf(invalidCount * 10000.0f / totalCount);
 282         }
 283     } else {
 284         invalidPercent = 10000;  // 100.00%
 285     }
 286     return invalidPercent;
 287 }
 288 #endif // USE_DSHOT_TELEMETRY_STATS
 289 #endif // USE_DSHOT_TELEMETRY
 290 #endif // USE_DSHOT