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 #endif
  43
  44 #include "pwm_output.h"
  45 #include "drivers/dshot.h"
  46 #include "drivers/dshot_dpwm.h"
  47 #include "drivers/dshot_command.h"
  48 #include "drivers/motor.h"
  49
  50 #include "pwm_output_dshot_shared.h"
  51
  52 FAST_DATA_ZERO_INIT uint8_t dmaMotorTimerCount = 0;
  53 #ifdef STM32F7
  54 FAST_DATA_ZERO_INIT motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  55 FAST_DATA_ZERO_INIT motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  56 #else
  57 motorDmaTimer_t dmaMotorTimers[MAX_DMA_TIMERS];
  58 motorDmaOutput_t dmaMotors[MAX_SUPPORTED_MOTORS];
  59 #endif
  60
  61 #ifdef USE_DSHOT_TELEMETRY
  62 FAST_DATA_ZERO_INIT uint32_t inputStampUs;
  63
  64 FAST_DATA_ZERO_INIT dshotDMAHandlerCycleCounters_t dshotDMAHandlerCycleCounters;
  65 #endif
  66
  67 motorDmaOutput_t *getMotorDmaOutput(uint8_t index)
  68 {
  69     return &dmaMotors[index];
  70 }
  71
  72 uint8_t getTimerIndex(TIM_TypeDef *timer)
  73 {
  74     for (int i = 0; i < dmaMotorTimerCount; i++) {
  75         if (dmaMotorTimers[i].timer == timer) {
  76             return i;
  77         }
  78     }
  79     dmaMotorTimers[dmaMotorTimerCount++].timer = timer;
  80     return dmaMotorTimerCount - 1;
  81 }
  82
  83 /**
  84  * Prepare to send dshot data for one motor
  85  *
  86  * Formats the value into the appropriate dma buffer and enables the dma channel.
  87  * The packet won't start transmitting until later since the dma requests from the timer
  88  * are disabled when this function is called.
  89  *
  90  * @param index index of the motor that the data is to be sent to
  91  * @param value the dshot value to be sent
  92 */
  93 FAST_CODE void pwmWriteDshotInt(uint8_t index, uint16_t value)
  94 {
  95     motorDmaOutput_t *const motor = &dmaMotors[index];
  96
  97     if (!motor->configured) {
  98         return;
  99     }
 100
 101     /*If there is a command ready to go overwrite the value and send that instead*/
 102     if (dshotCommandIsProcessing()) {
 103         value = dshotCommandGetCurrent(index);
 104         if (value) {
 105             motor->protocolControl.requestTelemetry = true;
 106         }
 107     }
 108
 109     motor->protocolControl.value = value;
 110
 111     uint16_t packet = prepareDshotPacket(&motor->protocolControl);
 112     uint8_t bufferSize;
 113
 114 #ifdef USE_DSHOT_DMAR
 115     if (useBurstDshot) {
 116         bufferSize = loadDmaBuffer(&motor->timer->dmaBurstBuffer[timerLookupChannelIndex(motor->timerHardware->channel)], 4, packet);
 117         motor->timer->dmaBurstLength = bufferSize * 4;
 118     } else
 119 #endif
 120     {
 121         bufferSize = loadDmaBuffer(motor->dmaBuffer, 1, packet);
 122
 123         motor->timer->timerDmaSources |= motor->timerDmaSource;
 124
 125 #ifdef USE_FULL_LL_DRIVER
 126         xLL_EX_DMA_SetDataLength(motor->dmaRef, bufferSize);
 127         xLL_EX_DMA_EnableResource(motor->dmaRef);
 128 #else
 129         xDMA_SetCurrDataCounter(motor->dmaRef, bufferSize);
 130
 131         // XXX we can remove this ifdef if we add a new macro for the TRUE/ENABLE constants
 132         #ifdef AT32F435
 133         xDMA_Cmd(motor->dmaRef, TRUE);
 134         #else
 135         xDMA_Cmd(motor->dmaRef, ENABLE);
 136         #endif
 137
 138 #endif // USE_FULL_LL_DRIVER
 139     }
 140
 141 }
 142
 143
 144 #ifdef USE_DSHOT_TELEMETRY
 145
 146 void dshotEnableChannels(uint8_t motorCount);
 147
 148
 149 static uint32_t decodeTelemetryPacket(uint32_t buffer[], uint32_t count)
 150 {
 151     uint32_t value = 0;
 152     uint32_t oldValue = buffer[0];
 153     int bits = 0;
 154     int len;
 155     for (uint32_t i = 1; i <= count; i++) {
 156         if (i < count) {
 157             int diff = buffer[i] - oldValue;
 158             if (bits >= 21) {
 159                 break;
 160             }
 161             len = (diff + 8) / 16;
 162         } else {
 163             len = 21 - bits;
 164         }
 165
 166         value <<= len;
 167         value |= 1 << (len - 1);
 168         oldValue = buffer[i];
 169         bits += len;
 170     }
 171     if (bits != 21) {
 172         return 0xffff;
 173     }
 174
 175     static const uint32_t decode[32] = {
 176         0, 0, 0, 0, 0, 0, 0, 0, 0, 9, 10, 11, 0, 13, 14, 15,
 177         0, 0, 2, 3, 0, 5, 6, 7, 0, 0, 8, 1, 0, 4, 12, 0 };
 178
 179     uint32_t decodedValue = decode[value & 0x1f];
 180     decodedValue |= decode[(value >> 5) & 0x1f] << 4;
 181     decodedValue |= decode[(value >> 10) & 0x1f] << 8;
 182     decodedValue |= decode[(value >> 15) & 0x1f] << 12;
 183
 184     uint32_t csum = decodedValue;
 185     csum = csum ^ (csum >> 8); // xor bytes
 186     csum = csum ^ (csum >> 4); // xor nibbles
 187
 188     if ((csum & 0xf) != 0xf) {
 189         return DSHOT_TELEMETRY_INVALID;
 190     }
 191
 192     return decodedValue >> 4;
 193 }
 194
 195 #endif
 196
 197 #ifdef USE_DSHOT_TELEMETRY
 198 /**
 199  * Process dshot telemetry packets before switching the channels back to outputs
 200  *
 201 */
 202 FAST_CODE_NOINLINE bool pwmTelemetryDecode(void)
 203 {
 204     if (!useDshotTelemetry) {
 205         return true;
 206     }
 207
 208 #ifdef USE_DSHOT_TELEMETRY_STATS
 209     const timeMs_t currentTimeMs = millis();
 210 #endif
 211     const timeUs_t currentUs = micros();
 212
 213     for (int i = 0; i < dshotPwmDevice.count; i++) {
 214         timeDelta_t usSinceInput = cmpTimeUs(currentUs, inputStampUs);
 215         if (usSinceInput >= 0 && usSinceInput < dmaMotors[i].dshotTelemetryDeadtimeUs) {
 216             return false;
 217         }
 218         if (dmaMotors[i].isInput) {
 219 #ifdef USE_FULL_LL_DRIVER
 220             uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xLL_EX_DMA_GetDataLength(dmaMotors[i].dmaRef);
 221 #else
 222             uint32_t edges = GCR_TELEMETRY_INPUT_LEN - xDMA_GetCurrDataCounter(dmaMotors[i].dmaRef);
 223 #endif
 224
 225 #ifdef USE_FULL_LL_DRIVER
 226             LL_EX_TIM_DisableIT(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource);
 227 #elif defined(AT32F435)
 228             tmr_dma_request_enable(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, FALSE);
 229 #else
 230             TIM_DMACmd(dmaMotors[i].timerHardware->tim, dmaMotors[i].timerDmaSource, DISABLE);
 231 #endif
 232
 233             uint16_t rawValue;
 234
 235             if (edges > MIN_GCR_EDGES) {
 236                 dshotTelemetryState.readCount++;
 237
 238                 rawValue = decodeTelemetryPacket(dmaMotors[i].dmaBuffer, edges);
 239
 240                 if (rawValue != DSHOT_TELEMETRY_INVALID) {
 241                     // Check EDT enable or store raw value
 242                     if ((rawValue == 0x0E00) && (dshotCommandGetCurrent(i) == DSHOT_CMD_EXTENDED_TELEMETRY_ENABLE)) {
 243                         dshotTelemetryState.motorState[i].telemetryTypes = 1 << DSHOT_TELEMETRY_TYPE_STATE_EVENTS;
 244                     } else {
 245                         dshotTelemetryState.motorState[i].rawValue = rawValue;
 246                     }
 247                 } else {
 248                     dshotTelemetryState.invalidPacketCount++;
 249                     if (i == 0) {
 250                         memcpy(dshotTelemetryState.inputBuffer, dmaMotors[i].dmaBuffer, sizeof(dshotTelemetryState.inputBuffer));
 251                     }
 252                 }
 253 #ifdef USE_DSHOT_TELEMETRY_STATS
 254                 updateDshotTelemetryQuality(&dshotTelemetryQuality[i], rawValue != DSHOT_TELEMETRY_INVALID, currentTimeMs);
 255 #endif
 256             }
 257         }
 258         pwmDshotSetDirectionOutput(&dmaMotors[i]);
 259     }
 260
 261     dshotTelemetryState.rawValueState = DSHOT_RAW_VALUE_STATE_NOT_PROCESSED;
 262     inputStampUs = 0;
 263     dshotEnableChannels(dshotPwmDevice.count);
 264     return true;
 265 }
 266
 267 #endif // USE_DSHOT_TELEMETRY
 268 #endif // USE_DSHOT