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