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