src/main/drivers/motor.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 <string.h>
  26
  27 #include "platform.h"
  28
  29 #ifdef USE_MOTOR
  30
  31 #include "common/maths.h"
  32
  33 #include "config/feature.h"
  34
  35 #include "drivers/dshot.h" // for DSHOT_ constants in initEscEndpoints; may be gone in the future
  36 #include "drivers/pwm_output.h" // for PWM_TYPE_* and others
  37 #include "drivers/time.h"
  38 #include "drivers/dshot_bitbang.h"
  39 #include "drivers/dshot_dpwm.h"
  40
  41 #include "fc/rc_controls.h" // for flight3DConfig_t
  42
  43 #include "motor.h"
  44
  45 static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice;
  46
  47 static bool motorProtocolEnabled = false;
  48 static bool motorProtocolDshot = false;
  49
  50 void motorShutdown(void)
  51 {
  52     motorDevice->vTable.shutdown();
  53     motorDevice->enabled = false;
  54     motorDevice->motorEnableTimeMs = 0;
  55     motorDevice->initialized = false;
  56     delayMicroseconds(1500);
  57 }
  58
  59 void motorWriteAll(float *values)
  60 {
  61 #ifdef USE_PWM_OUTPUT
  62     if (motorDevice->enabled) {
  63 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
  64         if (!motorDevice->vTable.updateStart()) {
  65             return;
  66         }
  67 #endif
  68         for (int i = 0; i < motorDevice->count; i++) {
  69             motorDevice->vTable.write(i, values[i]);
  70         }
  71         motorDevice->vTable.updateComplete();
  72     }
  73 #else
  74     UNUSED(values);
  75 #endif
  76 }
  77
  78 unsigned motorDeviceCount(void)
  79 {
  80     return motorDevice->count;
  81 }
  82
  83 motorVTable_t motorGetVTable(void)
  84 {
  85     return motorDevice->vTable;
  86 }
  87
  88 // This is not motor generic anymore; should be moved to analog pwm module
  89 static void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
  90 {
  91     if (featureIsEnabled(FEATURE_3D)) {
  92         float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2;
  93         *disarm = flight3DConfig()->neutral3d;
  94         *outputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
  95         *outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
  96         *deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
  97         *deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
  98     } else {
  99         *disarm = motorConfig->mincommand;
 100         *outputLow = motorConfig->minthrottle;
 101         *outputHigh = motorConfig->maxthrottle - ((motorConfig->maxthrottle - motorConfig->minthrottle) * (1 - outputLimit));
 102     }
 103 }
 104
 105 bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isProtocolDshot)
 106 {
 107     bool enabled = false;
 108     bool isDshot = false;
 109
 110     switch (motorDevConfig->motorPwmProtocol) {
 111     case PWM_TYPE_STANDARD:
 112     case PWM_TYPE_ONESHOT125:
 113     case PWM_TYPE_ONESHOT42:
 114     case PWM_TYPE_MULTISHOT:
 115     case PWM_TYPE_BRUSHED:
 116         enabled = true;
 117         break;
 118
 119 #ifdef USE_DSHOT
 120     case PWM_TYPE_DSHOT150:
 121     case PWM_TYPE_DSHOT300:
 122     case PWM_TYPE_DSHOT600:
 123     case PWM_TYPE_PROSHOT1000:
 124         enabled = true;
 125         isDshot = true;
 126         break;
 127 #endif
 128     default:
 129         break;
 130     }
 131
 132     if (isProtocolDshot) {
 133         *isProtocolDshot = isDshot;
 134     }
 135
 136     return enabled;
 137 }
 138
 139 static void checkMotorProtocol(const motorDevConfig_t *motorDevConfig)
 140 {
 141     motorProtocolEnabled = checkMotorProtocolEnabled(motorDevConfig, &motorProtocolDshot);
 142 }
 143
 144 // End point initialization is called from mixerInit before motorDevInit; can't use vtable...
 145 void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
 146 {
 147     checkMotorProtocol(&motorConfig->dev);
 148
 149     if (isMotorProtocolEnabled()) {
 150         if (!isMotorProtocolDshot()) {
 151             analogInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
 152         }
 153 #ifdef USE_DSHOT
 154         else {
 155             dshotInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
 156         }
 157 #endif
 158     }
 159 }
 160
 161 float motorConvertFromExternal(uint16_t externalValue)
 162 {
 163     return motorDevice->vTable.convertExternalToMotor(externalValue);
 164 }
 165
 166 uint16_t motorConvertToExternal(float motorValue)
 167 {
 168     return motorDevice->vTable.convertMotorToExternal(motorValue);
 169 }
 170
 171 void motorPostInit(void)
 172 {
 173     motorDevice->vTable.postInit();
 174 }
 175
 176 void motorPostInitNull(void)
 177 {
 178 }
 179
 180 static bool motorEnableNull(void)
 181 {
 182     return false;
 183 }
 184
 185 static void motorDisableNull(void)
 186 {
 187 }
 188
 189 static bool motorIsEnabledNull(uint8_t index)
 190 {
 191     UNUSED(index);
 192
 193     return false;
 194 }
 195
 196 bool motorUpdateStartNull(void)
 197 {
 198     return true;
 199 }
 200
 201 void motorWriteNull(uint8_t index, float value)
 202 {
 203     UNUSED(index);
 204     UNUSED(value);
 205 }
 206
 207 static void motorWriteIntNull(uint8_t index, uint16_t value)
 208 {
 209     UNUSED(index);
 210     UNUSED(value);
 211 }
 212
 213 void motorUpdateCompleteNull(void)
 214 {
 215 }
 216
 217 static void motorShutdownNull(void)
 218 {
 219 }
 220
 221 static float motorConvertFromExternalNull(uint16_t value)
 222 {
 223     UNUSED(value);
 224     return 0.0f ;
 225 }
 226
 227 static uint16_t motorConvertToExternalNull(float value)
 228 {
 229     UNUSED(value);
 230     return 0;
 231 }
 232
 233 static const motorVTable_t motorNullVTable = {
 234     .postInit = motorPostInitNull,
 235     .enable = motorEnableNull,
 236     .disable = motorDisableNull,
 237     .isMotorEnabled = motorIsEnabledNull,
 238     .updateStart = motorUpdateStartNull,
 239     .write = motorWriteNull,
 240     .writeInt = motorWriteIntNull,
 241     .updateComplete = motorUpdateCompleteNull,
 242     .convertExternalToMotor = motorConvertFromExternalNull,
 243     .convertMotorToExternal = motorConvertToExternalNull,
 244     .shutdown = motorShutdownNull,
 245 };
 246
 247 static motorDevice_t motorNullDevice = {
 248     .initialized = false,
 249     .enabled = false,
 250 };
 251
 252 bool isMotorProtocolEnabled(void)
 253 {
 254     return motorProtocolEnabled;
 255 }
 256
 257 bool isMotorProtocolDshot(void)
 258 {
 259     return motorProtocolDshot;
 260 }
 261
 262 void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount)
 263 {
 264     memset(motors, 0, sizeof(motors));
 265
 266     bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm;
 267
 268     if (isMotorProtocolEnabled()) {
 269         if (!isMotorProtocolDshot()) {
 270             motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 271         }
 272 #ifdef USE_DSHOT
 273         else {
 274 #ifdef USE_DSHOT_BITBANG
 275             if (isDshotBitbangActive(motorDevConfig)) {
 276                 motorDevice = dshotBitbangDevInit(motorDevConfig, motorCount);
 277             } else
 278 #endif
 279             {
 280                 motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 281             }
 282         }
 283 #endif
 284     }
 285
 286     if (motorDevice) {
 287         motorDevice->count = motorCount;
 288         motorDevice->initialized = true;
 289         motorDevice->motorEnableTimeMs = 0;
 290         motorDevice->enabled = false;
 291     } else {
 292         motorNullDevice.vTable = motorNullVTable;
 293         motorDevice = &motorNullDevice;
 294     }
 295 }
 296
 297 void motorDisable(void)
 298 {
 299     motorDevice->vTable.disable();
 300     motorDevice->enabled = false;
 301     motorDevice->motorEnableTimeMs = 0;
 302 }
 303
 304 void motorEnable(void)
 305 {
 306     if (motorDevice->initialized && motorDevice->vTable.enable()) {
 307         motorDevice->enabled = true;
 308         motorDevice->motorEnableTimeMs = millis();
 309     }
 310 }
 311
 312 bool motorIsEnabled(void)
 313 {
 314     return motorDevice->enabled;
 315 }
 316
 317 bool motorIsMotorEnabled(uint8_t index)
 318 {
 319     return motorDevice->vTable.isMotorEnabled(index);
 320 }
 321
 322 #ifdef USE_DSHOT
 323 timeMs_t motorGetMotorEnableTimeMs(void)
 324 {
 325     return motorDevice->motorEnableTimeMs;
 326 }
 327 #endif
 328
 329 #ifdef USE_DSHOT_BITBANG
 330 bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
 331 {
 332 #ifdef STM32F4
 333     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 334         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 335 #else
 336     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 337         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 338 #endif
 339 }
 340 #endif
 341
 342 float getDigitalIdleOffset(const motorConfig_t *motorConfig)
 343 {
 344     return CONVERT_PARAMETER_TO_PERCENT(motorConfig->digitalIdleOffsetValue * 0.01f);
 345 }
 346 #endif // USE_MOTOR