src/main/flight/mixer_init.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
  21 #include <stdbool.h>
  22 #include <stdint.h>
  23 #include <string.h>
  24
  25 #include "platform.h"
  26
  27 #include "build/build_config.h"
  28 #include "build/debug.h"
  29
  30 #include "config/config.h"
  31 #include "config/feature.h"
  32
  33 #include "drivers/dshot.h"
  34
  35 #include "fc/controlrate_profile.h"
  36 #include "fc/runtime_config.h"
  37
  38 #include "flight/mixer_tricopter.h"
  39 #include "flight/pid.h"
  40
  41 #include "rx/rx.h"
  42
  43 #include "sensors/battery.h"
  44
  45 #include "mixer_init.h"
  46
  47 PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0);
  48
  49 PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig,
  50     .mixerMode = DEFAULT_MIXER,
  51     .yaw_motors_reversed = false,
  52     .crashflip_motor_percent = 0,
  53     .crashflip_expo = 35,
  54     .mixer_type = MIXER_LEGACY,
  55 );
  56
  57 PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
  58
  59 mixerMode_e currentMixerMode;
  60
  61 static const motorMixer_t mixerQuadX[] = {
  62     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
  63     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
  64     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
  65     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
  66 };
  67 #ifndef USE_QUAD_MIXER_ONLY
  68 static const motorMixer_t mixerTricopter[] = {
  69     { 1.0f,  0.0f,  1.333333f,  0.0f },     // REAR
  70     { 1.0f, -1.0f, -0.666667f,  0.0f },     // RIGHT
  71     { 1.0f,  1.0f, -0.666667f,  0.0f },     // LEFT
  72 };
  73
  74 static const motorMixer_t mixerQuadP[] = {
  75     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
  76     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
  77     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
  78     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
  79 };
  80
  81 #if defined(USE_UNCOMMON_MIXERS)
  82 static const motorMixer_t mixerBicopter[] = {
  83     { 1.0f,  1.0f,  0.0f,  0.0f },          // LEFT
  84     { 1.0f, -1.0f,  0.0f,  0.0f },          // RIGHT
  85 };
  86 #else
  87 #define mixerBicopter NULL
  88 #endif
  89
  90 static const motorMixer_t mixerY4[] = {
  91     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR_TOP CW
  92     { 1.0f, -1.0f, -1.0f,  0.0f },          // FRONT_R CCW
  93     { 1.0f,  0.0f,  1.0f,  1.0f },          // REAR_BOTTOM CCW
  94     { 1.0f,  1.0f, -1.0f,  0.0f },          // FRONT_L CW
  95 };
  96
  97
  98 #if (MAX_SUPPORTED_MOTORS >= 6)
  99 static const motorMixer_t mixerHex6X[] = {
 100     { 1.0f, -0.5f,  0.866025f,  1.0f },     // REAR_R
 101     { 1.0f, -0.5f, -0.866025f,  1.0f },     // FRONT_R
 102     { 1.0f,  0.5f,  0.866025f, -1.0f },     // REAR_L
 103     { 1.0f,  0.5f, -0.866025f, -1.0f },     // FRONT_L
 104     { 1.0f, -1.0f,  0.0f,      -1.0f },     // RIGHT
 105     { 1.0f,  1.0f,  0.0f,       1.0f },     // LEFT
 106 };
 107
 108 #if defined(USE_UNCOMMON_MIXERS)
 109 static const motorMixer_t mixerHex6H[] = {
 110     { 1.0f, -1.0f,  1.0f, -1.0f },     // REAR_R
 111     { 1.0f, -1.0f, -1.0f,  1.0f },     // FRONT_R
 112     { 1.0f,  1.0f,  1.0f,  1.0f },     // REAR_L
 113     { 1.0f,  1.0f, -1.0f, -1.0f },     // FRONT_L
 114     { 1.0f,  0.0f,  0.0f,  0.0f },     // RIGHT
 115     { 1.0f,  0.0f,  0.0f,  0.0f },     // LEFT
 116 };
 117
 118 static const motorMixer_t mixerHex6P[] = {
 119     { 1.0f, -0.866025f,  0.5f,  1.0f },     // REAR_R
 120     { 1.0f, -0.866025f, -0.5f, -1.0f },     // FRONT_R
 121     { 1.0f,  0.866025f,  0.5f,  1.0f },     // REAR_L
 122     { 1.0f,  0.866025f, -0.5f, -1.0f },     // FRONT_L
 123     { 1.0f,  0.0f,      -1.0f,  1.0f },     // FRONT
 124     { 1.0f,  0.0f,       1.0f, -1.0f },     // REAR
 125 };
 126 static const motorMixer_t mixerY6[] = {
 127     { 1.0f,  0.0f,  1.333333f,  1.0f },     // REAR
 128     { 1.0f, -1.0f, -0.666667f, -1.0f },     // RIGHT
 129     { 1.0f,  1.0f, -0.666667f, -1.0f },     // LEFT
 130     { 1.0f,  0.0f,  1.333333f, -1.0f },     // UNDER_REAR
 131     { 1.0f, -1.0f, -0.666667f,  1.0f },     // UNDER_RIGHT
 132     { 1.0f,  1.0f, -0.666667f,  1.0f },     // UNDER_LEFT
 133 };
 134 #else
 135 #define mixerHex6H NULL
 136 #define mixerHex6P NULL
 137 #define mixerY6 NULL
 138 #endif // USE_UNCOMMON_MIXERS
 139 #else
 140 #define mixerHex6X NULL
 141 #endif // MAX_SUPPORTED_MOTORS >= 6
 142
 143 #if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8)
 144 static const motorMixer_t mixerOctoX8[] = {
 145     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 146     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 147     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 148     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 149     { 1.0f, -1.0f,  1.0f,  1.0f },          // UNDER_REAR_R
 150     { 1.0f, -1.0f, -1.0f, -1.0f },          // UNDER_FRONT_R
 151     { 1.0f,  1.0f,  1.0f, -1.0f },          // UNDER_REAR_L
 152     { 1.0f,  1.0f, -1.0f,  1.0f },          // UNDER_FRONT_L
 153 };
 154
 155 static const motorMixer_t mixerOctoX8P[] = {
 156     { 1.0f,  0.0f,  1.0f, -1.0f },          // REAR
 157     { 1.0f, -1.0f,  0.0f,  1.0f },          // RIGHT
 158     { 1.0f,  1.0f,  0.0f,  1.0f },          // LEFT
 159     { 1.0f,  0.0f, -1.0f, -1.0f },          // FRONT
 160     { 1.0f,  0.0f,  1.0f,  1.0f },          // UNDER_REAR
 161     { 1.0f, -1.0f,  0.0f, -1.0f },          // UNDER_RIGHT
 162     { 1.0f,  1.0f,  0.0f, -1.0f },          // UNDER_LEFT
 163     { 1.0f,  0.0f, -1.0f,  1.0f },          // UNDER_FRONT
 164 };
 165
 166 static const motorMixer_t mixerOctoFlatP[] = {
 167     { 1.0f,  0.707107f, -0.707107f,  1.0f },    // FRONT_L
 168     { 1.0f, -0.707107f, -0.707107f,  1.0f },    // FRONT_R
 169     { 1.0f, -0.707107f,  0.707107f,  1.0f },    // REAR_R
 170     { 1.0f,  0.707107f,  0.707107f,  1.0f },    // REAR_L
 171     { 1.0f,  0.0f, -1.0f, -1.0f },              // FRONT
 172     { 1.0f, -1.0f,  0.0f, -1.0f },              // RIGHT
 173     { 1.0f,  0.0f,  1.0f, -1.0f },              // REAR
 174     { 1.0f,  1.0f,  0.0f, -1.0f },              // LEFT
 175 };
 176
 177 static const motorMixer_t mixerOctoFlatX[] = {
 178     { 1.0f,  1.0f, -0.414178f,  1.0f },      // MIDFRONT_L
 179     { 1.0f, -0.414178f, -1.0f,  1.0f },      // FRONT_R
 180     { 1.0f, -1.0f,  0.414178f,  1.0f },      // MIDREAR_R
 181     { 1.0f,  0.414178f,  1.0f,  1.0f },      // REAR_L
 182     { 1.0f,  0.414178f, -1.0f, -1.0f },      // FRONT_L
 183     { 1.0f, -1.0f, -0.414178f, -1.0f },      // MIDFRONT_R
 184     { 1.0f, -0.414178f,  1.0f, -1.0f },      // REAR_R
 185     { 1.0f,  1.0f,  0.414178f, -1.0f },      // MIDREAR_L
 186 };
 187 #else
 188 #define mixerOctoX8 NULL
 189 #define mixerOctoX8P NULL
 190 #define mixerOctoFlatP NULL
 191 #define mixerOctoFlatX NULL
 192 #endif
 193
 194 static const motorMixer_t mixerVtail4[] = {
 195     { 1.0f,  -0.58f,  0.58f, 1.0f },        // REAR_R
 196     { 1.0f,  -0.46f, -0.39f, -0.5f },       // FRONT_R
 197     { 1.0f,  0.58f,  0.58f, -1.0f },        // REAR_L
 198     { 1.0f,  0.46f, -0.39f, 0.5f },         // FRONT_L
 199 };
 200
 201 static const motorMixer_t mixerAtail4[] = {
 202     { 1.0f, -0.58f,  0.58f, -1.0f },          // REAR_R
 203     { 1.0f, -0.46f, -0.39f,  0.5f },          // FRONT_R
 204     { 1.0f,  0.58f,  0.58f,  1.0f },          // REAR_L
 205     { 1.0f,  0.46f, -0.39f, -0.5f },          // FRONT_L
 206 };
 207
 208 #if defined(USE_UNCOMMON_MIXERS)
 209 static const motorMixer_t mixerDualcopter[] = {
 210     { 1.0f,  0.0f,  0.0f, -1.0f },          // LEFT
 211     { 1.0f,  0.0f,  0.0f,  1.0f },          // RIGHT
 212 };
 213 #else
 214 #define mixerDualcopter NULL
 215 #endif
 216
 217 static const motorMixer_t mixerSingleProp[] = {
 218     { 1.0f,  0.0f,  0.0f, 0.0f },
 219 };
 220
 221 static const motorMixer_t mixerQuadX1234[] = {
 222     { 1.0f,  1.0f, -1.0f, -1.0f },          // FRONT_L
 223     { 1.0f, -1.0f, -1.0f,  1.0f },          // FRONT_R
 224     { 1.0f, -1.0f,  1.0f, -1.0f },          // REAR_R
 225     { 1.0f,  1.0f,  1.0f,  1.0f },          // REAR_L
 226 };
 227
 228 // Keep synced with mixerMode_e
 229 // Some of these entries are bogus when servos (USE_SERVOS) are not configured,
 230 // but left untouched to keep ordinals synced with mixerMode_e (and configurator).
 231 const mixer_t mixers[] = {
 232     // motors, use servo, motor mixer
 233     { 0, false, NULL },                // entry 0
 234     { 3, true,  mixerTricopter },      // MIXER_TRI
 235     { 4, false, mixerQuadP },          // MIXER_QUADP
 236     { 4, false, mixerQuadX },          // MIXER_QUADX
 237     { 2, true,  mixerBicopter },       // MIXER_BICOPTER
 238     { 0, true,  NULL },                // * MIXER_GIMBAL
 239     { 6, false, mixerY6 },             // MIXER_Y6
 240     { 6, false, mixerHex6P },          // MIXER_HEX6
 241     { 1, true,  mixerSingleProp },     // * MIXER_FLYING_WING
 242     { 4, false, mixerY4 },             // MIXER_Y4
 243     { 6, false, mixerHex6X },          // MIXER_HEX6X
 244     { 8, false, mixerOctoX8 },         // MIXER_OCTOX8
 245     { 8, false, mixerOctoFlatP },      // MIXER_OCTOFLATP
 246     { 8, false, mixerOctoFlatX },      // MIXER_OCTOFLATX
 247     { 1, true,  mixerSingleProp },     // * MIXER_AIRPLANE
 248     { 1, true,  mixerSingleProp },     // * MIXER_HELI_120_CCPM
 249     { 0, true,  NULL },                // * MIXER_HELI_90_DEG
 250     { 4, false, mixerVtail4 },         // MIXER_VTAIL4
 251     { 6, false, mixerHex6H },          // MIXER_HEX6H
 252     { 0, true,  NULL },                // * MIXER_PPM_TO_SERVO
 253     { 2, true,  mixerDualcopter },     // MIXER_DUALCOPTER
 254     { 1, true,  NULL },                // MIXER_SINGLECOPTER
 255     { 4, false, mixerAtail4 },         // MIXER_ATAIL4
 256     { 0, false, NULL },                // MIXER_CUSTOM
 257     { 2, true,  NULL },                // MIXER_CUSTOM_AIRPLANE
 258     { 3, true,  NULL },                // MIXER_CUSTOM_TRI
 259     { 4, false, mixerQuadX1234 },      // MIXER_QUADX_1234
 260     { 8, false, mixerOctoX8P },        // MIXER_OCTOX8P
 261 };
 262 #endif // !USE_QUAD_MIXER_ONLY
 263
 264 FAST_DATA_ZERO_INIT mixerRuntime_t mixerRuntime;
 265
 266 uint8_t getMotorCount(void)
 267 {
 268     return mixerRuntime.motorCount;
 269 }
 270
 271 bool areMotorsRunning(void)
 272 {
 273     bool motorsRunning = false;
 274     if (ARMING_FLAG(ARMED)) {
 275         motorsRunning = true;
 276     } else {
 277         for (int i = 0; i < mixerRuntime.motorCount; i++) {
 278             if (motor_disarmed[i] != mixerRuntime.disarmMotorOutput) {
 279                 motorsRunning = true;
 280
 281                 break;
 282             }
 283         }
 284     }
 285
 286     return motorsRunning;
 287 }
 288
 289 #ifdef USE_SERVOS
 290 bool mixerIsTricopter(void)
 291 {
 292     return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI);
 293 }
 294 #endif
 295
 296 // All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator
 297 // DSHOT scaling is done to the actual dshot range
 298 void initEscEndpoints(void)
 299 {
 300     float motorOutputLimit = 1.0f;
 301     if (currentPidProfile->motor_output_limit < 100) {
 302         motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f;
 303     }
 304     motorInitEndpoints(motorConfig(), motorOutputLimit, &mixerRuntime.motorOutputLow, &mixerRuntime.motorOutputHigh, &mixerRuntime.disarmMotorOutput, &mixerRuntime.deadbandMotor3dHigh, &mixerRuntime.deadbandMotor3dLow);
 305 }
 306
 307 // Initialize pidProfile related mixer settings
 308
 309 void mixerInitProfile(void)
 310 {
 311 #ifdef USE_DYN_IDLE
 312     if (motorConfigMutable()->dev.useDshotTelemetry) {
 313         mixerRuntime.dynIdleMinRps = currentPidProfile->dyn_idle_min_rpm * 100.0f / 60.0f;
 314     } else {
 315         mixerRuntime.dynIdleMinRps = 0.0f;
 316     }
 317     mixerRuntime.dynIdlePGain = currentPidProfile->dyn_idle_p_gain * 0.00015f;
 318     mixerRuntime.dynIdleIGain = currentPidProfile->dyn_idle_i_gain * 0.01f * pidGetDT();
 319     mixerRuntime.dynIdleDGain = currentPidProfile->dyn_idle_d_gain * 0.0000003f * pidGetPidFrequency();
 320     mixerRuntime.dynIdleMaxIncrease = currentPidProfile->dyn_idle_max_increase * 0.001f;
 321     mixerRuntime.dynIdleStartIncrease = currentPidProfile->dyn_idle_start_increase * 0.001f;
 322     mixerRuntime.minRpsDelayK = 800 * pidGetDT() / 20.0f; //approx 20ms D delay, arbitrarily suits many motors
 323     if (!mixerRuntime.feature3dEnabled && mixerRuntime.dynIdleMinRps) {
 324         mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE; // Override value set by initEscEndpoints to allow zero motor drive
 325     }
 326 #endif
 327
 328 #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION)
 329     mixerRuntime.vbatSagCompensationFactor = 0.0f;
 330     if (currentPidProfile->vbat_sag_compensation > 0) {
 331         //TODO: Make this voltage user configurable
 332         mixerRuntime.vbatFull = CELL_VOLTAGE_FULL_CV;
 333         mixerRuntime.vbatRangeToCompensate = mixerRuntime.vbatFull - batteryConfig()->vbatwarningcellvoltage;
 334         if (mixerRuntime.vbatRangeToCompensate > 0) {
 335             mixerRuntime.vbatSagCompensationFactor = ((float)currentPidProfile->vbat_sag_compensation) / 100.0f;
 336         }
 337     }
 338 #endif
 339 }
 340
 341 #ifdef USE_LAUNCH_CONTROL
 342 // Create a custom mixer for launch control based on the current settings
 343 // but disable the front motors. We don't care about roll or yaw because they
 344 // are limited in the PID controller.
 345 void loadLaunchControlMixer(void)
 346 {
 347     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 348         mixerRuntime.launchControlMixer[i] = mixerRuntime.currentMixer[i];
 349         // limit the front motors to minimum output
 350         if (mixerRuntime.launchControlMixer[i].pitch < 0.0f) {
 351             mixerRuntime.launchControlMixer[i].pitch = 0.0f;
 352             mixerRuntime.launchControlMixer[i].throttle = 0.0f;
 353         }
 354     }
 355 }
 356 #endif
 357
 358 #ifndef USE_QUAD_MIXER_ONLY
 359
 360 static void mixerConfigureOutput(void)
 361 {
 362     mixerRuntime.motorCount = 0;
 363
 364     if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) {
 365         // load custom mixer into currentMixer
 366         for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 367             // check if done
 368             if (customMotorMixer(i)->throttle == 0.0f) {
 369                 break;
 370             }
 371             mixerRuntime.currentMixer[i] = *customMotorMixer(i);
 372             mixerRuntime.motorCount++;
 373         }
 374     } else {
 375         mixerRuntime.motorCount = mixers[currentMixerMode].motorCount;
 376         if (mixerRuntime.motorCount > MAX_SUPPORTED_MOTORS) {
 377             mixerRuntime.motorCount = MAX_SUPPORTED_MOTORS;
 378         }
 379         // copy motor-based mixers
 380         if (mixers[currentMixerMode].motor) {
 381             for (int i = 0; i < mixerRuntime.motorCount; i++)
 382                 mixerRuntime.currentMixer[i] = mixers[currentMixerMode].motor[i];
 383         }
 384     }
 385 #ifdef USE_LAUNCH_CONTROL
 386     loadLaunchControlMixer();
 387 #endif
 388     mixerResetDisarmedMotors();
 389 }
 390
 391 void mixerLoadMix(int index, motorMixer_t *customMixers)
 392 {
 393     // we're 1-based
 394     index++;
 395     // clear existing
 396     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 397         customMixers[i].throttle = 0.0f;
 398     }
 399     // do we have anything here to begin with?
 400     if (mixers[index].motor != NULL) {
 401         for (int i = 0; i < mixers[index].motorCount; i++) {
 402             customMixers[i] = mixers[index].motor[i];
 403         }
 404     }
 405 }
 406 #else
 407 static void mixerConfigureOutput(void)
 408 {
 409     mixerRuntime.motorCount = QUAD_MOTOR_COUNT;
 410     for (int i = 0; i < mixerRuntime.motorCount; i++) {
 411         mixerRuntime.currentMixer[i] = mixerQuadX[i];
 412     }
 413 #ifdef USE_LAUNCH_CONTROL
 414     loadLaunchControlMixer();
 415 #endif
 416     mixerResetDisarmedMotors();
 417 }
 418 #endif // USE_QUAD_MIXER_ONLY
 419
 420 void mixerInit(mixerMode_e mixerMode)
 421 {
 422     currentMixerMode = mixerMode;
 423
 424     mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D);
 425
 426     initEscEndpoints();
 427 #ifdef USE_SERVOS
 428     if (mixerIsTricopter()) {
 429         mixerTricopterInit();
 430     }
 431 #endif
 432
 433 #ifdef USE_DYN_IDLE
 434     mixerRuntime.idleThrottleOffset = getDigitalIdleOffset(motorConfig());
 435     mixerRuntime.dynIdleI = 0.0f;
 436     mixerRuntime.prevMinRps = 0.0f;
 437 #endif
 438
 439     mixerConfigureOutput();
 440 }
 441
 442 void mixerResetDisarmedMotors(void)
 443 {
 444     // set disarmed motor values
 445     for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
 446         motor_disarmed[i] = mixerRuntime.disarmMotorOutput;
 447     }
 448 }
 449
 450 mixerMode_e getMixerMode(void)
 451 {
 452     return currentMixerMode;
 453 }
 454
 455 bool mixerModeIsFixedWing(mixerMode_e mixerMode)
 456 {
 457     switch (mixerMode) {
 458     case MIXER_FLYING_WING:
 459     case MIXER_AIRPLANE:
 460     case MIXER_CUSTOM_AIRPLANE:
 461         return true;
 462         break;
 463     default:
 464         return false;
 465         break;
 466     }
 467 }
 468
 469 bool isFixedWing(void)
 470 {
 471     return mixerModeIsFixedWing(currentMixerMode);
 472 }
 473
 474 float getMotorOutputLow(void)
 475 {
 476     return mixerRuntime.motorOutputLow;
 477 }
 478
 479 float getMotorOutputHigh(void)
 480 {
 481     return mixerRuntime.motorOutputHigh;
 482 }