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 #endif
  74 }
  75
  76 unsigned motorDeviceCount(void)
  77 {
  78     return motorDevice->count;
  79 }
  80
  81 motorVTable_t motorGetVTable(void)
  82 {
  83     return motorDevice->vTable;
  84 }
  85
  86 // This is not motor generic anymore; should be moved to analog pwm module
  87 static void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow) {
  88     if (featureIsEnabled(FEATURE_3D)) {
  89         float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2;
  90         *disarm = flight3DConfig()->neutral3d;
  91         *outputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
  92         *outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
  93         *deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
  94         *deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
  95     } else {
  96         *disarm = motorConfig->mincommand;
  97         *outputLow = motorConfig->minthrottle;
  98         *outputHigh = motorConfig->maxthrottle - ((motorConfig->maxthrottle - motorConfig->minthrottle) * (1 - outputLimit));
  99     }
 100 }
 101
 102 bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isProtocolDshot)
 103 {
 104     bool enabled = false;
 105     bool isDshot = false;
 106
 107     switch (motorDevConfig->motorPwmProtocol) {
 108     case PWM_TYPE_STANDARD:
 109     case PWM_TYPE_ONESHOT125:
 110     case PWM_TYPE_ONESHOT42:
 111     case PWM_TYPE_MULTISHOT:
 112     case PWM_TYPE_BRUSHED:
 113         enabled = true;
 114
 115         break;
 116
 117 #ifdef USE_DSHOT
 118     case PWM_TYPE_DSHOT150:
 119     case PWM_TYPE_DSHOT300:
 120     case PWM_TYPE_DSHOT600:
 121     case PWM_TYPE_PROSHOT1000:
 122         enabled = true;
 123         isDshot = true;
 124
 125         break;
 126 #endif
 127     default:
 128
 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()
 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     memset(motors, 0, sizeof(motors));
 264
 265     bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm;
 266
 267     if (isMotorProtocolEnabled()) {
 268         if (!isMotorProtocolDshot()) {
 269             motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 270         }
 271 #ifdef USE_DSHOT
 272         else {
 273 #ifdef USE_DSHOT_BITBANG
 274             if (isDshotBitbangActive(motorDevConfig)) {
 275                 motorDevice = dshotBitbangDevInit(motorDevConfig, motorCount);
 276             } else
 277 #endif
 278             {
 279                 motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 280             }
 281         }
 282 #endif
 283     }
 284
 285     if (motorDevice) {
 286         motorDevice->count = motorCount;
 287         motorDevice->initialized = true;
 288         motorDevice->motorEnableTimeMs = 0;
 289         motorDevice->enabled = false;
 290     } else {
 291         motorNullDevice.vTable = motorNullVTable;
 292         motorDevice = &motorNullDevice;
 293     }
 294 }
 295
 296 void motorDisable(void)
 297 {
 298     motorDevice->vTable.disable();
 299     motorDevice->enabled = false;
 300     motorDevice->motorEnableTimeMs = 0;
 301 }
 302
 303 void motorEnable(void)
 304 {
 305     if (motorDevice->initialized && motorDevice->vTable.enable()) {
 306         motorDevice->enabled = true;
 307         motorDevice->motorEnableTimeMs = millis();
 308     }
 309 }
 310
 311 bool motorIsEnabled(void)
 312 {
 313     return motorDevice->enabled;
 314 }
 315
 316 bool motorIsMotorEnabled(uint8_t index)
 317 {
 318     return motorDevice->vTable.isMotorEnabled(index);
 319 }
 320
 321 #ifdef USE_DSHOT
 322 timeMs_t motorGetMotorEnableTimeMs(void)
 323 {
 324     return motorDevice->motorEnableTimeMs;
 325 }
 326 #endif
 327
 328 #ifdef USE_DSHOT_BITBANG
 329 bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
 330 {
 331 #ifdef STM32F4
 332     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 333         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 334 #else
 335     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 336         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 337 #endif
 338 }
 339 #endif
 340
 341 float getDigitalIdleOffset(const motorConfig_t *motorConfig)
 342 {
 343     return CONVERT_PARAMETER_TO_PERCENT(motorConfig->digitalIdleOffsetValue * 0.01f);
 344 }
 345 #endif // USE_MOTOR