src/main/drivers/dshot_dpwm.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  * Author: jflyper
  21  */
  22
  23 #include <stdbool.h>
  24 #include <stdint.h>
  25 #include <math.h>
  26 #include <string.h>
  27
  28 #include "platform.h"
  29
  30 #ifdef USE_DSHOT
  31
  32 #include "drivers/pwm_output.h"
  33 #include "drivers/dshot.h"
  34 #include "drivers/dshot_dpwm.h"
  35 #include "drivers/motor.h"
  36
  37 #include "pg/motor.h"
  38
  39 // XXX TODO: Share a single region among dshotDmaBuffer and dshotBurstDmaBuffer
  40
  41 DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotDmaBuffer[MAX_SUPPORTED_MOTORS][DSHOT_DMA_BUFFER_ALLOC_SIZE];
  42
  43 #ifdef USE_DSHOT_DMAR
  44 DSHOT_DMA_BUFFER_ATTRIBUTE DSHOT_DMA_BUFFER_UNIT dshotBurstDmaBuffer[MAX_DMA_TIMERS][DSHOT_DMA_BUFFER_SIZE * 4];
  45 #endif
  46
  47 #ifdef USE_DSHOT_DMAR
  48 FAST_DATA_ZERO_INIT bool useBurstDshot = false;
  49 #endif
  50 #ifdef USE_DSHOT_TELEMETRY
  51 FAST_DATA_ZERO_INIT bool useDshotTelemetry = false;
  52 #endif
  53
  54 FAST_DATA_ZERO_INIT loadDmaBufferFn *loadDmaBuffer;
  55
  56 FAST_CODE_NOINLINE uint8_t loadDmaBufferDshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
  57 {
  58     int i;
  59     for (i = 0; i < 16; i++) {
  60         dmaBuffer[i * stride] = (packet & 0x8000) ? MOTOR_BIT_1 : MOTOR_BIT_0;  // MSB first
  61         packet <<= 1;
  62     }
  63     dmaBuffer[i++ * stride] = 0;
  64     dmaBuffer[i++ * stride] = 0;
  65
  66     return DSHOT_DMA_BUFFER_SIZE;
  67 }
  68
  69 FAST_CODE_NOINLINE uint8_t loadDmaBufferProshot(uint32_t *dmaBuffer, int stride, uint16_t packet)
  70 {
  71     int i;
  72     for (i = 0; i < 4; i++) {
  73         dmaBuffer[i * stride] = PROSHOT_BASE_SYMBOL + ((packet & 0xF000) >> 12) * PROSHOT_BIT_WIDTH;  // Most significant nibble first
  74         packet <<= 4;   // Shift 4 bits
  75     }
  76     dmaBuffer[i++ * stride] = 0;
  77     dmaBuffer[i++ * stride] = 0;
  78
  79     return PROSHOT_DMA_BUFFER_SIZE;
  80 }
  81
  82 uint32_t getDshotHz(motorPwmProtocolTypes_e pwmProtocolType)
  83 {
  84     switch (pwmProtocolType) {
  85     case(PWM_TYPE_PROSHOT1000):
  86         return MOTOR_PROSHOT1000_HZ;
  87     case(PWM_TYPE_DSHOT600):
  88         return MOTOR_DSHOT600_HZ;
  89     case(PWM_TYPE_DSHOT300):
  90         return MOTOR_DSHOT300_HZ;
  91     default:
  92     case(PWM_TYPE_DSHOT150):
  93         return MOTOR_DSHOT150_HZ;
  94     }
  95 }
  96
  97 static void dshotPwmShutdown(void)
  98 {
  99     // DShot signal is only generated if write to motors happen,
 100     // and that is prevented by enabled checking at write.
 101     // Hence there's no special processing required here.
 102     return;
 103 }
 104
 105 static void dshotPwmDisableMotors(void)
 106 {
 107     // No special processing required
 108     return;
 109 }
 110
 111 static bool dshotPwmEnableMotors(void)
 112 {
 113     for (int i = 0; i < dshotPwmDevice.count; i++) {
 114         motorDmaOutput_t *motor = getMotorDmaOutput(i);
 115         const IO_t motorIO = IOGetByTag(motor->timerHardware->tag);
 116         IOConfigGPIOAF(motorIO, motor->iocfg, motor->timerHardware->alternateFunction);
 117     }
 118
 119     // No special processing required
 120     return true;
 121 }
 122
 123 static bool dshotPwmIsMotorEnabled(uint8_t index)
 124 {
 125     return motors[index].enabled;
 126 }
 127
 128 static FAST_CODE void dshotWriteInt(uint8_t index, uint16_t value)
 129 {
 130     pwmWriteDshotInt(index, value);
 131 }
 132
 133 static FAST_CODE void dshotWrite(uint8_t index, float value)
 134 {
 135     pwmWriteDshotInt(index, lrintf(value));
 136 }
 137
 138 static motorVTable_t dshotPwmVTable = {
 139     .postInit = motorPostInitNull,
 140     .enable = dshotPwmEnableMotors,
 141     .disable = dshotPwmDisableMotors,
 142     .isMotorEnabled = dshotPwmIsMotorEnabled,
 143     .updateStart = motorUpdateStartNull, // May be updated after copying
 144     .write = dshotWrite,
 145     .writeInt = dshotWriteInt,
 146     .updateComplete = pwmCompleteDshotMotorUpdate,
 147     .convertExternalToMotor = dshotConvertFromExternal,
 148     .convertMotorToExternal = dshotConvertToExternal,
 149     .shutdown = dshotPwmShutdown,
 150 };
 151
 152 FAST_DATA_ZERO_INIT motorDevice_t dshotPwmDevice;
 153
 154 motorDevice_t *dshotPwmDevInit(const motorDevConfig_t *motorConfig, uint16_t idlePulse, uint8_t motorCount, bool useUnsyncedPwm)
 155 {
 156     UNUSED(idlePulse);
 157     UNUSED(useUnsyncedPwm);
 158
 159     dshotPwmDevice.vTable = dshotPwmVTable;
 160
 161 #ifdef USE_DSHOT_TELEMETRY
 162     useDshotTelemetry = motorConfig->useDshotTelemetry;
 163     dshotPwmDevice.vTable.updateStart = pwmStartDshotMotorUpdate;
 164 #endif
 165
 166     switch (motorConfig->motorPwmProtocol) {
 167     case PWM_TYPE_PROSHOT1000:
 168         loadDmaBuffer = loadDmaBufferProshot;
 169         break;
 170     case PWM_TYPE_DSHOT600:
 171     case PWM_TYPE_DSHOT300:
 172     case PWM_TYPE_DSHOT150:
 173         loadDmaBuffer = loadDmaBufferDshot;
 174 #ifdef USE_DSHOT_DMAR
 175         useBurstDshot = motorConfig->useBurstDshot == DSHOT_DMAR_ON ||
 176             (motorConfig->useBurstDshot == DSHOT_DMAR_AUTO && !motorConfig->useDshotTelemetry);
 177 #endif
 178         break;
 179     }
 180
 181     for (int motorIndex = 0; motorIndex < MAX_SUPPORTED_MOTORS && motorIndex < motorCount; motorIndex++) {
 182         const unsigned reorderedMotorIndex = motorConfig->motorOutputReordering[motorIndex];
 183         const ioTag_t tag = motorConfig->ioTags[reorderedMotorIndex];
 184         const timerHardware_t *timerHardware = timerAllocate(tag, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 185
 186         if (timerHardware != NULL) {
 187             motors[motorIndex].io = IOGetByTag(tag);
 188             IOInit(motors[motorIndex].io, OWNER_MOTOR, RESOURCE_INDEX(reorderedMotorIndex));
 189
 190             if (pwmDshotMotorHardwareConfig(timerHardware,
 191                 motorIndex,
 192                 reorderedMotorIndex,
 193                 motorConfig->motorPwmProtocol,
 194                 motorConfig->motorPwmInversion ? timerHardware->output ^ TIMER_OUTPUT_INVERTED : timerHardware->output)) {
 195                 motors[motorIndex].enabled = true;
 196
 197                 continue;
 198             }
 199         }
 200
 201         /* not enough motors initialised for the mixer or a break in the motors */
 202         dshotPwmDevice.vTable.write = motorWriteNull;
 203         dshotPwmDevice.vTable.updateComplete = motorUpdateCompleteNull;
 204
 205         /* TODO: block arming and add reason system cannot arm */
 206         return NULL;
 207     }
 208
 209     return &dshotPwmDevice;
 210 }
 211
 212 #endif // USE_DSHOT