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/dshot_bitbang.h"
  37 #include "drivers/dshot_dpwm.h"
  38 #include "drivers/pwm_output.h" // for PWM_TYPE_* and others
  39 #include "drivers/time.h"
  40
  41 #include "fc/rc_controls.h" // for flight3DConfig_t
  42
  43 #include "sensors/battery.h"
  44
  45 #include "motor.h"
  46
  47 static FAST_DATA_ZERO_INIT motorDevice_t *motorDevice;
  48
  49 static bool motorProtocolEnabled = false;
  50 static bool motorProtocolDshot = false;
  51
  52 void motorShutdown(void)
  53 {
  54     uint32_t shutdownDelayUs = 1500;
  55     motorDevice->vTable.shutdown();
  56     motorDevice->enabled = false;
  57     motorDevice->motorEnableTimeMs = 0;
  58     motorDevice->initialized = false;
  59
  60     switch (motorConfig()->dev.motorPwmProtocol) {
  61     case PWM_TYPE_STANDARD:
  62     case PWM_TYPE_ONESHOT125:
  63     case PWM_TYPE_ONESHOT42:
  64     case PWM_TYPE_MULTISHOT:
  65         // Delay 500ms will disarm esc which can prevent motor spin while reboot
  66         shutdownDelayUs += 500 * 1000;
  67         break;
  68     default:
  69         break;
  70     }
  71
  72     delayMicroseconds(shutdownDelayUs);
  73 }
  74
  75 void motorWriteAll(float *values)
  76 {
  77 #ifdef USE_PWM_OUTPUT
  78     if (motorDevice->enabled) {
  79 #ifdef USE_DSHOT_BITBANG
  80         if (isDshotBitbangActive(&motorConfig()->dev)) {
  81             // Initialise the output buffers
  82             if (motorDevice->vTable.updateInit) {
  83                 motorDevice->vTable.updateInit();
  84             }
  85
  86             // Update the motor data
  87             for (int i = 0; i < motorDevice->count; i++) {
  88                 motorDevice->vTable.write(i, values[i]);
  89             }
  90
  91             // Don't attempt to write commands to the motors if telemetry is still being received
  92             if (motorDevice->vTable.telemetryWait) {
  93                 (void)motorDevice->vTable.telemetryWait();
  94             }
  95
  96             // Trigger the transmission of the motor data
  97             motorDevice->vTable.updateComplete();
  98
  99             // Perform the decode of the last data received
 100             // New data will be received once the send of motor data, triggered above, completes
 101 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
 102             motorDevice->vTable.decodeTelemetry();
 103 #endif
 104         } else
 105 #endif
 106         {
 107             // Perform the decode of the last data received
 108             // New data will be received once the send of motor data, triggered above, completes
 109 #if defined(USE_DSHOT) && defined(USE_DSHOT_TELEMETRY)
 110             motorDevice->vTable.decodeTelemetry();
 111 #endif
 112
 113             // Update the motor data
 114             for (int i = 0; i < motorDevice->count; i++) {
 115                 motorDevice->vTable.write(i, values[i]);
 116             }
 117
 118             // Trigger the transmission of the motor data
 119             motorDevice->vTable.updateComplete();
 120
 121         }
 122     }
 123 #else
 124     UNUSED(values);
 125 #endif
 126 }
 127
 128 unsigned motorDeviceCount(void)
 129 {
 130     return motorDevice->count;
 131 }
 132
 133 motorVTable_t *motorGetVTable(void)
 134 {
 135     return &motorDevice->vTable;
 136 }
 137
 138 // This is not motor generic anymore; should be moved to analog pwm module
 139 static void analogInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
 140 {
 141     if (featureIsEnabled(FEATURE_3D)) {
 142         float outputLimitOffset = (flight3DConfig()->limit3d_high - flight3DConfig()->limit3d_low) * (1 - outputLimit) / 2;
 143         *disarm = flight3DConfig()->neutral3d;
 144         *outputLow = flight3DConfig()->limit3d_low + outputLimitOffset;
 145         *outputHigh = flight3DConfig()->limit3d_high - outputLimitOffset;
 146         *deadbandMotor3dHigh = flight3DConfig()->deadband3d_high;
 147         *deadbandMotor3dLow = flight3DConfig()->deadband3d_low;
 148     } else {
 149         *disarm = motorConfig->mincommand;
 150         const float minThrottle = motorConfig->mincommand + motorConfig->motorIdle * 0.1f;
 151         *outputLow = minThrottle;
 152         *outputHigh = motorConfig->maxthrottle - ((motorConfig->maxthrottle - minThrottle) * (1 - outputLimit));
 153     }
 154 }
 155
 156 bool checkMotorProtocolEnabled(const motorDevConfig_t *motorDevConfig, bool *isProtocolDshot)
 157 {
 158     bool enabled = false;
 159     bool isDshot = false;
 160
 161     switch (motorDevConfig->motorPwmProtocol) {
 162     case PWM_TYPE_STANDARD:
 163     case PWM_TYPE_ONESHOT125:
 164     case PWM_TYPE_ONESHOT42:
 165     case PWM_TYPE_MULTISHOT:
 166     case PWM_TYPE_BRUSHED:
 167         enabled = true;
 168         break;
 169
 170 #ifdef USE_DSHOT
 171     case PWM_TYPE_DSHOT150:
 172     case PWM_TYPE_DSHOT300:
 173     case PWM_TYPE_DSHOT600:
 174     case PWM_TYPE_PROSHOT1000:
 175         enabled = true;
 176         isDshot = true;
 177         break;
 178 #endif
 179     default:
 180         break;
 181     }
 182
 183     if (isProtocolDshot) {
 184         *isProtocolDshot = isDshot;
 185     }
 186
 187     return enabled;
 188 }
 189
 190 static void checkMotorProtocol(const motorDevConfig_t *motorDevConfig)
 191 {
 192     motorProtocolEnabled = checkMotorProtocolEnabled(motorDevConfig, &motorProtocolDshot);
 193 }
 194
 195 // End point initialization is called from mixerInit before motorDevInit; can't use vtable...
 196 void motorInitEndpoints(const motorConfig_t *motorConfig, float outputLimit, float *outputLow, float *outputHigh, float *disarm, float *deadbandMotor3dHigh, float *deadbandMotor3dLow)
 197 {
 198     checkMotorProtocol(&motorConfig->dev);
 199
 200     if (isMotorProtocolEnabled()) {
 201         if (!isMotorProtocolDshot()) {
 202             analogInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
 203         }
 204 #ifdef USE_DSHOT
 205         else {
 206             dshotInitEndpoints(motorConfig, outputLimit, outputLow, outputHigh, disarm, deadbandMotor3dHigh, deadbandMotor3dLow);
 207         }
 208 #endif
 209     }
 210 }
 211
 212 float motorConvertFromExternal(uint16_t externalValue)
 213 {
 214     return motorDevice->vTable.convertExternalToMotor(externalValue);
 215 }
 216
 217 uint16_t motorConvertToExternal(float motorValue)
 218 {
 219     return motorDevice->vTable.convertMotorToExternal(motorValue);
 220 }
 221
 222 void motorPostInit(void)
 223 {
 224     motorDevice->vTable.postInit();
 225 }
 226
 227 void motorPostInitNull(void)
 228 {
 229 }
 230
 231 static bool motorEnableNull(void)
 232 {
 233     return false;
 234 }
 235
 236 static void motorDisableNull(void)
 237 {
 238 }
 239
 240 static bool motorIsEnabledNull(uint8_t index)
 241 {
 242     UNUSED(index);
 243
 244     return false;
 245 }
 246
 247 bool motorDecodeTelemetryNull(void)
 248 {
 249     return true;
 250 }
 251
 252 void motorWriteNull(uint8_t index, float value)
 253 {
 254     UNUSED(index);
 255     UNUSED(value);
 256 }
 257
 258 static void motorWriteIntNull(uint8_t index, uint16_t value)
 259 {
 260     UNUSED(index);
 261     UNUSED(value);
 262 }
 263
 264 void motorUpdateCompleteNull(void)
 265 {
 266 }
 267
 268 static void motorShutdownNull(void)
 269 {
 270 }
 271
 272 static float motorConvertFromExternalNull(uint16_t value)
 273 {
 274     UNUSED(value);
 275     return 0.0f ;
 276 }
 277
 278 static uint16_t motorConvertToExternalNull(float value)
 279 {
 280     UNUSED(value);
 281     return 0;
 282 }
 283
 284 static const motorVTable_t motorNullVTable = {
 285     .postInit = motorPostInitNull,
 286     .enable = motorEnableNull,
 287     .disable = motorDisableNull,
 288     .isMotorEnabled = motorIsEnabledNull,
 289     .decodeTelemetry = motorDecodeTelemetryNull,
 290     .write = motorWriteNull,
 291     .writeInt = motorWriteIntNull,
 292     .updateComplete = motorUpdateCompleteNull,
 293     .convertExternalToMotor = motorConvertFromExternalNull,
 294     .convertMotorToExternal = motorConvertToExternalNull,
 295     .shutdown = motorShutdownNull,
 296 };
 297
 298 static motorDevice_t motorNullDevice = {
 299     .initialized = false,
 300     .enabled = false,
 301 };
 302
 303 bool isMotorProtocolEnabled(void)
 304 {
 305     return motorProtocolEnabled;
 306 }
 307
 308 bool isMotorProtocolDshot(void)
 309 {
 310     return motorProtocolDshot;
 311 }
 312
 313 bool isMotorProtocolBidirDshot(void)
 314 {
 315     return isMotorProtocolDshot() && useDshotTelemetry;
 316 }
 317
 318 void motorDevInit(const motorDevConfig_t *motorDevConfig, uint16_t idlePulse, uint8_t motorCount)
 319 {
 320     memset(motors, 0, sizeof(motors));
 321
 322     bool useUnsyncedPwm = motorDevConfig->useUnsyncedPwm;
 323
 324     if (isMotorProtocolEnabled()) {
 325         if (!isMotorProtocolDshot()) {
 326             motorDevice = motorPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 327         }
 328 #ifdef USE_DSHOT
 329         else {
 330 #ifdef USE_DSHOT_BITBANG
 331             if (isDshotBitbangActive(motorDevConfig)) {
 332                 motorDevice = dshotBitbangDevInit(motorDevConfig, motorCount);
 333             } else
 334 #endif
 335             {
 336                 motorDevice = dshotPwmDevInit(motorDevConfig, idlePulse, motorCount, useUnsyncedPwm);
 337             }
 338         }
 339 #endif
 340     }
 341
 342     if (motorDevice) {
 343         motorDevice->count = motorCount;
 344         motorDevice->initialized = true;
 345         motorDevice->motorEnableTimeMs = 0;
 346         motorDevice->enabled = false;
 347     } else {
 348         motorNullDevice.vTable = motorNullVTable;
 349         motorDevice = &motorNullDevice;
 350     }
 351 }
 352
 353 void motorDisable(void)
 354 {
 355     motorDevice->vTable.disable();
 356     motorDevice->enabled = false;
 357     motorDevice->motorEnableTimeMs = 0;
 358 }
 359
 360 void motorEnable(void)
 361 {
 362     if (motorDevice->initialized && motorDevice->vTable.enable()) {
 363         motorDevice->enabled = true;
 364         motorDevice->motorEnableTimeMs = millis();
 365     }
 366 }
 367
 368 float motorEstimateMaxRpm(void)
 369 {
 370     // Empirical testing found this relationship between estimated max RPM without props attached
 371     // (unloaded) and measured max RPM with props attached (loaded), independent from prop size
 372     float unloadedMaxRpm = 0.01f * getBatteryVoltage() * motorConfig()->kv;
 373     float loadDerating = -5.44e-6f * unloadedMaxRpm + 0.944f;
 374
 375     return unloadedMaxRpm * loadDerating;
 376 }
 377
 378 bool motorIsEnabled(void)
 379 {
 380     return motorDevice->enabled;
 381 }
 382
 383 bool motorIsMotorEnabled(uint8_t index)
 384 {
 385     return motorDevice->vTable.isMotorEnabled(index);
 386 }
 387
 388 #ifdef USE_DSHOT
 389 timeMs_t motorGetMotorEnableTimeMs(void)
 390 {
 391     return motorDevice->motorEnableTimeMs;
 392 }
 393 #endif
 394
 395 #ifdef USE_DSHOT_BITBANG
 396 bool isDshotBitbangActive(const motorDevConfig_t *motorDevConfig)
 397 {
 398 #if defined(STM32F4) || defined(APM32F4)
 399     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 400         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->useDshotTelemetry && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 401 #else
 402     return motorDevConfig->useDshotBitbang == DSHOT_BITBANG_ON ||
 403         (motorDevConfig->useDshotBitbang == DSHOT_BITBANG_AUTO && motorDevConfig->motorPwmProtocol != PWM_TYPE_PROSHOT1000);
 404 #endif
 405 }
 406 #endif
 407 #endif // USE_MOTOR