From 9e6b65680ef39ac8b19bd8d9764ba6b7c8bf284f Mon Sep 17 00:00:00 2001 From: mikeller Date: Sat, 10 Oct 2020 22:35:24 +1300 Subject: [PATCH 1/2] Split mixer code into initialisation and runtime to reduce flash space usage. --- make/source.mk | 7 +- src/main/flight/mixer.c | 570 ++++------------------------------- src/main/flight/mixer_init.c | 461 ++++++++++++++++++++++++++++ src/main/flight/mixer_init.h | 54 ++++ 4 files changed, 582 insertions(+), 510 deletions(-) create mode 100644 src/main/flight/mixer_init.c create mode 100644 src/main/flight/mixer_init.h diff --git a/make/source.mk b/make/source.mk index 37c078306..926a4a15a 100644 --- a/make/source.mk +++ b/make/source.mk @@ -95,6 +95,7 @@ COMMON_SRC = \ flight/imu.c \ flight/interpolated_setpoint.c \ flight/mixer.c \ + flight/mixer_init.c \ flight/mixer_tricopter.c \ flight/pid.c \ flight/pid_init.c \ @@ -275,6 +276,7 @@ SPEED_OPTIMISED_SRC := $(SPEED_OPTIMISED_SRC) \ $(DEVICE_STDPERIPH_SRC) \ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ + $(shell find $(SRC_DIR) -name '*_init.c') \ bus_bst_stm32f30x.c \ cli/cli.c \ cli/settings.c \ @@ -302,7 +304,6 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ drivers/serial_escserial.c \ drivers/serial_pinconfig.c \ drivers/serial_tcp.c \ - drivers/serial_uart_init.c \ drivers/serial_uart_pinconfig.c \ drivers/serial_usb_vcp.c \ drivers/transponder_ir_io_hal.c \ @@ -348,9 +349,7 @@ SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ io/spektrum_vtx_control.c \ osd/osd.c \ osd/osd_elements.c \ - rx/rx_bind.c \ - sensors/gyro_init.c\ - flight/pid_init.c + rx/rx_bind.c # Gyro driver files that only contain initialization and configuration code - not runtime code SIZE_OPTIMISED_SRC := $(SIZE_OPTIMISED_SRC) \ diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a0ad8b4f8..b5aa884da 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -20,471 +20,65 @@ #include #include -#include #include #include "platform.h" -#include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" #include "common/filter.h" #include "common/maths.h" +#include "config/config.h" #include "config/feature.h" -#include "pg/motor.h" -#include "pg/rx.h" - #include "drivers/dshot.h" +#include "drivers/io.h" #include "drivers/motor.h" #include "drivers/time.h" -#include "drivers/io.h" -#include "config/config.h" #include "fc/controlrate_profile.h" +#include "fc/core.h" +#include "fc/rc.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" -#include "fc/core.h" -#include "fc/rc.h" #include "flight/failsafe.h" -#include "flight/imu.h" #include "flight/gps_rescue.h" -#include "flight/mixer.h" +#include "flight/imu.h" +#include "flight/mixer_init.h" #include "flight/mixer_tricopter.h" #include "flight/pid.h" #include "flight/rpm_filter.h" +#include "pg/rx.h" + #include "rx/rx.h" #include "sensors/battery.h" #include "sensors/gyro.h" -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); +#include "mixer.h" #define DYN_LPF_THROTTLE_STEPS 100 #define DYN_LPF_THROTTLE_UPDATE_DELAY_US 5000 // minimum of 5ms between updates -PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .mixerMode = DEFAULT_MIXER, - .yaw_motors_reversed = false, - .crashflip_motor_percent = 0, - .crashflip_expo = 35 -); +#define RC_COMMAND_THROTTLE_RANGE (PWM_RANGE_MAX - PWM_RANGE_MIN) -PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); - -#define PWM_RANGE_MID 1500 - -static FAST_DATA_ZERO_INIT uint8_t motorCount; static FAST_DATA_ZERO_INIT float motorMixRange; float FAST_DATA_ZERO_INIT motor[MAX_SUPPORTED_MOTORS]; float motor_disarmed[MAX_SUPPORTED_MOTORS]; -mixerMode_e currentMixerMode; -static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; - -#ifdef USE_LAUNCH_CONTROL -static motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS]; -#endif - static FAST_DATA_ZERO_INIT int throttleAngleCorrection; -static const motorMixer_t mixerQuadX[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L -}; -#ifndef USE_QUAD_MIXER_ONLY -static const motorMixer_t mixerTricopter[] = { - { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerQuadP[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT - { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT -}; - -#if defined(USE_UNCOMMON_MIXERS) -static const motorMixer_t mixerBicopter[] = { - { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT - { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT -}; -#else -#define mixerBicopter NULL -#endif - -static const motorMixer_t mixerY4[] = { - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW - { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW - { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW - { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW -}; - - -#if (MAX_SUPPORTED_MOTORS >= 6) -static const motorMixer_t mixerHex6X[] = { - { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R - { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R - { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L - { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT -}; - -#if defined(USE_UNCOMMON_MIXERS) -static const motorMixer_t mixerHex6H[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT - { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT -}; - -static const motorMixer_t mixerHex6P[] = { - { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R - { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R - { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L - { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L - { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR -}; -static const motorMixer_t mixerY6[] = { - { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR - { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT - { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT - { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR - { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT - { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT -}; -#else -#define mixerHex6H NULL -#define mixerHex6P NULL -#define mixerY6 NULL -#endif // USE_UNCOMMON_MIXERS -#else -#define mixerHex6X NULL -#endif // MAX_SUPPORTED_MOTORS >= 6 - -#if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8) -static const motorMixer_t mixerOctoX8[] = { - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R - { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R - { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L - { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L -}; - -static const motorMixer_t mixerOctoFlatP[] = { - { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L - { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R - { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R - { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L - { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT - { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT - { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR - { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT -}; - -static const motorMixer_t mixerOctoFlatX[] = { - { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L - { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R - { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L - { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R - { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R - { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L -}; -#else -#define mixerOctoX8 NULL -#define mixerOctoFlatP NULL -#define mixerOctoFlatX NULL -#endif - -static const motorMixer_t mixerVtail4[] = { - { 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R - { 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R - { 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L - { 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L -}; - -static const motorMixer_t mixerAtail4[] = { - { 1.0f, -0.58f, 0.58f, -1.0f }, // REAR_R - { 1.0f, -0.46f, -0.39f, 0.5f }, // FRONT_R - { 1.0f, 0.58f, 0.58f, 1.0f }, // REAR_L - { 1.0f, 0.46f, -0.39f, -0.5f }, // FRONT_L -}; - -#if defined(USE_UNCOMMON_MIXERS) -static const motorMixer_t mixerDualcopter[] = { - { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT - { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT -}; -#else -#define mixerDualcopter NULL -#endif - -static const motorMixer_t mixerSingleProp[] = { - { 1.0f, 0.0f, 0.0f, 0.0f }, -}; - -static const motorMixer_t mixerQuadX1234[] = { - { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L - { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R - { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R - { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L -}; - -// Keep synced with mixerMode_e -// Some of these entries are bogus when servos (USE_SERVOS) are not configured, -// but left untouched to keep ordinals synced with mixerMode_e (and configurator). -const mixer_t mixers[] = { - // motors, use servo, motor mixer - { 0, false, NULL }, // entry 0 - { 3, true, mixerTricopter }, // MIXER_TRI - { 4, false, mixerQuadP }, // MIXER_QUADP - { 4, false, mixerQuadX }, // MIXER_QUADX - { 2, true, mixerBicopter }, // MIXER_BICOPTER - { 0, true, NULL }, // * MIXER_GIMBAL - { 6, false, mixerY6 }, // MIXER_Y6 - { 6, false, mixerHex6P }, // MIXER_HEX6 - { 1, true, mixerSingleProp }, // * MIXER_FLYING_WING - { 4, false, mixerY4 }, // MIXER_Y4 - { 6, false, mixerHex6X }, // MIXER_HEX6X - { 8, false, mixerOctoX8 }, // MIXER_OCTOX8 - { 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP - { 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX - { 1, true, mixerSingleProp }, // * MIXER_AIRPLANE - { 1, true, mixerSingleProp }, // * MIXER_HELI_120_CCPM - { 0, true, NULL }, // * MIXER_HELI_90_DEG - { 4, false, mixerVtail4 }, // MIXER_VTAIL4 - { 6, false, mixerHex6H }, // MIXER_HEX6H - { 0, true, NULL }, // * MIXER_PPM_TO_SERVO - { 2, true, mixerDualcopter }, // MIXER_DUALCOPTER - { 1, true, NULL }, // MIXER_SINGLECOPTER - { 4, false, mixerAtail4 }, // MIXER_ATAIL4 - { 0, false, NULL }, // MIXER_CUSTOM - { 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE - { 3, true, NULL }, // MIXER_CUSTOM_TRI - { 4, false, mixerQuadX1234 }, -}; -#endif // !USE_QUAD_MIXER_ONLY - -static FAST_DATA_ZERO_INIT bool feature3dEnabled; -static FAST_DATA_ZERO_INIT float motorOutputLow; -static FAST_DATA_ZERO_INIT float motorOutputHigh; - -static FAST_DATA_ZERO_INIT float disarmMotorOutput, deadbandMotor3dHigh, deadbandMotor3dLow; -static FAST_DATA_ZERO_INIT float rcCommandThrottleRange; -#ifdef USE_DYN_IDLE -static FAST_DATA_ZERO_INIT float idleMaxIncrease; -static FAST_DATA_ZERO_INIT float idleThrottleOffset; -static FAST_DATA_ZERO_INIT float idleMinMotorRps; -static FAST_DATA_ZERO_INIT float idleP; -static FAST_DATA_ZERO_INIT float oldMinRps; -#endif -#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) -static FAST_DATA_ZERO_INIT float vbatSagCompensationFactor; -static FAST_DATA_ZERO_INIT float vbatFull; -static FAST_DATA_ZERO_INIT float vbatRangeToCompensate; -#endif - -uint8_t getMotorCount(void) -{ - return motorCount; -} - float getMotorMixRange(void) { return motorMixRange; } -bool areMotorsRunning(void) -{ - bool motorsRunning = false; - if (ARMING_FLAG(ARMED)) { - motorsRunning = true; - } else { - for (int i = 0; i < motorCount; i++) { - if (motor_disarmed[i] != disarmMotorOutput) { - motorsRunning = true; - - break; - } - } - } - - return motorsRunning; -} - -#ifdef USE_SERVOS -bool mixerIsTricopter(void) -{ - return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI); -} -#endif - -// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator -// DSHOT scaling is done to the actual dshot range -void initEscEndpoints(void) -{ - float motorOutputLimit = 1.0f; - if (currentPidProfile->motor_output_limit < 100) { - motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f; - } - - motorInitEndpoints(motorConfig(), motorOutputLimit, &motorOutputLow, &motorOutputHigh, &disarmMotorOutput, &deadbandMotor3dHigh, &deadbandMotor3dLow); - if (!feature3dEnabled && currentPidProfile->idle_min_rpm) { - motorOutputLow = DSHOT_MIN_THROTTLE; - } - - rcCommandThrottleRange = PWM_RANGE_MAX - PWM_RANGE_MIN; -} - -// Initialize pidProfile related mixer settings -void mixerInitProfile(void) -{ -#ifdef USE_DYN_IDLE - idleMinMotorRps = currentPidProfile->idle_min_rpm * 100.0f / 60.0f; - idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f; - idleP = currentPidProfile->idle_p * 0.0001f; - oldMinRps = 0; -#endif - -#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) - vbatSagCompensationFactor = 0.0f; - if (currentPidProfile->vbat_sag_compensation > 0) { - //TODO: Make this voltage user configurable - vbatFull = CELL_VOLTAGE_FULL_CV; - vbatRangeToCompensate = vbatFull - batteryConfig()->vbatwarningcellvoltage; - if (vbatRangeToCompensate > 0) { - vbatSagCompensationFactor = ((float)currentPidProfile->vbat_sag_compensation) / 100.0f; - } - } -#endif -} - -void mixerInit(mixerMode_e mixerMode) -{ - currentMixerMode = mixerMode; - - feature3dEnabled = featureIsEnabled(FEATURE_3D); - - initEscEndpoints(); -#ifdef USE_SERVOS - if (mixerIsTricopter()) { - mixerTricopterInit(); - } -#endif - -#ifdef USE_DYN_IDLE - idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f; -#endif - - mixerInitProfile(); -} - -#ifdef USE_LAUNCH_CONTROL -// Create a custom mixer for launch control based on the current settings -// but disable the front motors. We don't care about roll or yaw because they -// are limited in the PID controller. -void loadLaunchControlMixer(void) -{ - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - launchControlMixer[i] = currentMixer[i]; - // limit the front motors to minimum output - if (launchControlMixer[i].pitch < 0.0f) { - launchControlMixer[i].pitch = 0.0f; - launchControlMixer[i].throttle = 0.0f; - } - } -} -#endif - -#ifndef USE_QUAD_MIXER_ONLY - -void mixerConfigureOutput(void) -{ - motorCount = 0; - - if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { - // load custom mixer into currentMixer - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - // check if done - if (customMotorMixer(i)->throttle == 0.0f) { - break; - } - currentMixer[i] = *customMotorMixer(i); - motorCount++; - } - } else { - motorCount = mixers[currentMixerMode].motorCount; - if (motorCount > MAX_SUPPORTED_MOTORS) { - motorCount = MAX_SUPPORTED_MOTORS; - } - // copy motor-based mixers - if (mixers[currentMixerMode].motor) { - for (int i = 0; i < motorCount; i++) - currentMixer[i] = mixers[currentMixerMode].motor[i]; - } - } -#ifdef USE_LAUNCH_CONTROL - loadLaunchControlMixer(); -#endif - mixerResetDisarmedMotors(); -} - -void mixerLoadMix(int index, motorMixer_t *customMixers) -{ - // we're 1-based - index++; - // clear existing - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - customMixers[i].throttle = 0.0f; - } - // do we have anything here to begin with? - if (mixers[index].motor != NULL) { - for (int i = 0; i < mixers[index].motorCount; i++) { - customMixers[i] = mixers[index].motor[i]; - } - } -} -#else -void mixerConfigureOutput(void) -{ - motorCount = QUAD_MOTOR_COUNT; - for (int i = 0; i < motorCount; i++) { - currentMixer[i] = mixerQuadX[i]; - } -#ifdef USE_LAUNCH_CONTROL - loadLaunchControlMixer(); -#endif - mixerResetDisarmedMotors(); -} -#endif // USE_QUAD_MIXER_ONLY - -void mixerResetDisarmedMotors(void) -{ - // set disarmed motor values - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - motor_disarmed[i] = disarmMotorOutput; - } -} - void writeMotors(void) { motorWriteAll(motor); @@ -493,7 +87,7 @@ void writeMotors(void) static void writeAllMotors(int16_t mc) { // Sends commands to all motors - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { motor[i] = mc; } writeMotors(); @@ -501,7 +95,7 @@ static void writeAllMotors(int16_t mc) void stopMotors(void) { - writeAllMotors(disarmMotorOutput); + writeAllMotors(mixerRuntime.disarmMotorOutput); delay(50); // give the timers and ESCs a chance to react. } @@ -521,7 +115,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) static float motorRangeMinIncrease = 0; float currentThrottleInputRange = 0; - if (feature3dEnabled) { + if (mixerRuntime.feature3dEnabled) { uint16_t rcCommand3dDeadBandLow; uint16_t rcCommand3dDeadBandHigh; @@ -546,17 +140,17 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) if (rcCommand[THROTTLE] <= rcCommand3dDeadBandLow || isFlipOverAfterCrashActive()) { // INVERTED - motorRangeMin = motorOutputLow; - motorRangeMax = deadbandMotor3dLow; + motorRangeMin = mixerRuntime.motorOutputLow; + motorRangeMax = mixerRuntime.deadbandMotor3dLow; #ifdef USE_DSHOT if (isMotorProtocolDshot()) { - motorOutputMin = motorOutputLow; - motorOutputRange = deadbandMotor3dLow - motorOutputLow; + motorOutputMin = mixerRuntime.motorOutputLow; + motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; } else #endif { - motorOutputMin = deadbandMotor3dLow; - motorOutputRange = motorOutputLow - deadbandMotor3dLow; + motorOutputMin = mixerRuntime.deadbandMotor3dLow; + motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; } if (motorOutputMixSign != -1) { @@ -569,10 +163,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) currentThrottleInputRange = rcCommandThrottleRange3dLow; } else if (rcCommand[THROTTLE] >= rcCommand3dDeadBandHigh) { // NORMAL - motorRangeMin = deadbandMotor3dHigh; - motorRangeMax = motorOutputHigh; - motorOutputMin = deadbandMotor3dHigh; - motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorRangeMin = mixerRuntime.deadbandMotor3dHigh; + motorRangeMax = mixerRuntime.motorOutputHigh; + motorOutputMin = mixerRuntime.deadbandMotor3dHigh; + motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; if (motorOutputMixSign != 1) { reversalTimeUs = currentTimeUs; } @@ -584,18 +178,18 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) !flight3DConfigMutable()->switched_mode3d) || isMotorsReversed()) { // INVERTED_TO_DEADBAND - motorRangeMin = motorOutputLow; - motorRangeMax = deadbandMotor3dLow; + motorRangeMin = mixerRuntime.motorOutputLow; + motorRangeMax = mixerRuntime.deadbandMotor3dLow; #ifdef USE_DSHOT if (isMotorProtocolDshot()) { - motorOutputMin = motorOutputLow; - motorOutputRange = deadbandMotor3dLow - motorOutputLow; + motorOutputMin = mixerRuntime.motorOutputLow; + motorOutputRange = mixerRuntime.deadbandMotor3dLow - mixerRuntime.motorOutputLow; } else #endif { - motorOutputMin = deadbandMotor3dLow; - motorOutputRange = motorOutputLow - deadbandMotor3dLow; + motorOutputMin = mixerRuntime.deadbandMotor3dLow; + motorOutputRange = mixerRuntime.motorOutputLow - mixerRuntime.deadbandMotor3dLow; } if (motorOutputMixSign != -1) { @@ -607,10 +201,10 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) currentThrottleInputRange = rcCommandThrottleRange3dLow; } else { // NORMAL_TO_DEADBAND - motorRangeMin = deadbandMotor3dHigh; - motorRangeMax = motorOutputHigh; - motorOutputMin = deadbandMotor3dHigh; - motorOutputRange = motorOutputHigh - deadbandMotor3dHigh; + motorRangeMin = mixerRuntime.deadbandMotor3dHigh; + motorRangeMax = mixerRuntime.motorOutputHigh; + motorOutputMin = mixerRuntime.deadbandMotor3dHigh; + motorOutputRange = mixerRuntime.motorOutputHigh - mixerRuntime.deadbandMotor3dHigh; if (motorOutputMixSign != 1) { reversalTimeUs = currentTimeUs; } @@ -625,15 +219,15 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) } else { throttle = rcCommand[THROTTLE] - PWM_RANGE_MIN + throttleAngleCorrection; #ifdef USE_DYN_IDLE - if (idleMinMotorRps > 0.0f) { - const float maxIncrease = isAirmodeActivated() ? idleMaxIncrease : 0.04f; + if (mixerRuntime.idleMinMotorRps > 0.0f) { + const float maxIncrease = isAirmodeActivated() ? mixerRuntime.idleMaxIncrease : 0.04f; const float minRps = rpmMinMotorFrequency(); - const float targetRpsChangeRate = (idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed; - const float error = targetRpsChangeRate - (minRps - oldMinRps) * pidGetPidFrequency(); - const float pidSum = constrainf(idleP * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit); + const float targetRpsChangeRate = (mixerRuntime.idleMinMotorRps - minRps) * currentPidProfile->idle_adjustment_speed; + const float error = targetRpsChangeRate - (minRps - mixerRuntime.oldMinRps) * pidGetPidFrequency(); + const float pidSum = constrainf(mixerRuntime.idleP * error, -currentPidProfile->idle_pid_limit, currentPidProfile->idle_pid_limit); motorRangeMinIncrease = constrainf(motorRangeMinIncrease + pidSum * pidGetDT(), 0.0f, maxIncrease); - oldMinRps = minRps; - throttle += idleThrottleOffset * rcCommandThrottleRange; + mixerRuntime.oldMinRps = minRps; + throttle += mixerRuntime.idleThrottleOffset * RC_COMMAND_THROTTLE_RANGE; DEBUG_SET(DEBUG_DYN_IDLE, 0, motorRangeMinIncrease * 1000); DEBUG_SET(DEBUG_DYN_IDLE, 1, targetRpsChangeRate); @@ -647,21 +241,21 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) #if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) float motorRangeAttenuationFactor = 0; // reduce motorRangeMax when battery is full - if (vbatSagCompensationFactor > 0.0f) { + if (mixerRuntime.vbatSagCompensationFactor > 0.0f) { const uint16_t currentCellVoltage = getBatterySagCellVoltage(); // batteryGoodness = 1 when voltage is above vbatFull, and 0 when voltage is below vbatLow - float batteryGoodness = 1.0f - constrainf((vbatFull - currentCellVoltage) / vbatRangeToCompensate, 0.0f, 1.0f); - motorRangeAttenuationFactor = (vbatRangeToCompensate / vbatFull) * batteryGoodness * vbatSagCompensationFactor; + float batteryGoodness = 1.0f - constrainf((mixerRuntime.vbatFull - currentCellVoltage) / mixerRuntime.vbatRangeToCompensate, 0.0f, 1.0f); + motorRangeAttenuationFactor = (mixerRuntime.vbatRangeToCompensate / mixerRuntime.vbatFull) * batteryGoodness * mixerRuntime.vbatSagCompensationFactor; DEBUG_SET(DEBUG_BATTERY, 2, batteryGoodness * 100); DEBUG_SET(DEBUG_BATTERY, 3, motorRangeAttenuationFactor * 1000); } - motorRangeMax = motorOutputHigh - motorRangeAttenuationFactor * (motorOutputHigh - motorOutputLow); + motorRangeMax = mixerRuntime.motorOutputHigh - motorRangeAttenuationFactor * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); #else - motorRangeMax = motorOutputHigh; + motorRangeMax = mixerRuntime.motorOutputHigh; #endif - currentThrottleInputRange = rcCommandThrottleRange; - motorRangeMin = motorOutputLow + motorRangeMinIncrease * (motorOutputHigh - motorOutputLow); + currentThrottleInputRange = RC_COMMAND_THROTTLE_RANGE; + motorRangeMin = mixerRuntime.motorOutputLow + motorRangeMinIncrease * (mixerRuntime.motorOutputHigh - mixerRuntime.motorOutputLow); motorOutputMin = motorRangeMin; motorOutputRange = motorRangeMax - motorRangeMin; motorOutputMixSign = 1; @@ -720,11 +314,11 @@ static void applyFlipOverAfterCrashModeToMotors(void) const float flipStickRange = 1.0f - crashFlipStickMinExpo; const float flipPower = MAX(0.0f, stickDeflectionExpoLength - crashFlipStickMinExpo) / flipStickRange; - for (int i = 0; i < motorCount; ++i) { + for (int i = 0; i < mixerRuntime.motorCount; ++i) { float motorOutputNormalised = - signPitch*currentMixer[i].pitch + - signRoll*currentMixer[i].roll + - signYaw*currentMixer[i].yaw; + signPitch * mixerRuntime.currentMixer[i].pitch + + signRoll * mixerRuntime.currentMixer[i].roll + + signYaw * mixerRuntime.currentMixer[i].yaw; if (motorOutputNormalised < 0) { if (mixerConfig()->crashflip_motor_percent > 0) { @@ -737,13 +331,13 @@ static void applyFlipOverAfterCrashModeToMotors(void) float motorOutput = motorOutputMin + motorOutputNormalised * motorOutputRange; // Add a little bit to the motorOutputMin so props aren't spinning when sticks are centered - motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND); + motorOutput = (motorOutput < motorOutputMin + CRASH_FLIP_DEADBAND) ? mixerRuntime.disarmMotorOutput : (motorOutput - CRASH_FLIP_DEADBAND); motor[i] = motorOutput; } } else { // Disarmed mode - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { motor[i] = motor_disarmed[i]; } } @@ -753,7 +347,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t { // Now add in the desired throttle, but keep in a range that doesn't clip adjusted // roll/pitch/yaw. This could move throttle down, but also up for those low throttle flips. - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { float motorOutput = motorOutputMixSign * motorMix[i] + throttle * activeMixer[i].throttle; #ifdef USE_THRUST_LINEARIZATION motorOutput = pidApplyThrustLinearization(motorOutput); @@ -768,10 +362,10 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t if (failsafeIsActive()) { #ifdef USE_DSHOT if (isMotorProtocolDshot()) { - motorOutput = (motorOutput < motorRangeMin) ? disarmMotorOutput : motorOutput; // Prevent getting into special reserved range + motorOutput = (motorOutput < motorRangeMin) ? mixerRuntime.disarmMotorOutput : motorOutput; // Prevent getting into special reserved range } #endif - motorOutput = constrain(motorOutput, disarmMotorOutput, motorRangeMax); + motorOutput = constrain(motorOutput, mixerRuntime.disarmMotorOutput, motorRangeMax); } else { motorOutput = constrain(motorOutput, motorRangeMin, motorRangeMax); } @@ -780,7 +374,7 @@ static void applyMixToMotors(float motorMix[MAX_SUPPORTED_MOTORS], motorMixer_t // Disarmed mode if (!ARMING_FLAG(ARMED)) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { motor[i] = motor_disarmed[i]; } } @@ -803,8 +397,8 @@ static float applyThrottleLimit(float throttle) static void applyMotorStop(void) { - for (int i = 0; i < motorCount; i++) { - motor[i] = disarmMotorOutput; + for (int i = 0; i < mixerRuntime.motorCount; i++) { + motor[i] = mixerRuntime.disarmMotorOutput; } } @@ -841,10 +435,10 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) const bool launchControlActive = isLaunchControlActive(); - motorMixer_t * activeMixer = ¤tMixer[0]; + motorMixer_t * activeMixer = &mixerRuntime.currentMixer[0]; #ifdef USE_LAUNCH_CONTROL if (launchControlActive && (currentPidProfile->launchControlMode == LAUNCH_CONTROL_MODE_PITCHONLY)) { - activeMixer = &launchControlMixer[0]; + activeMixer = &mixerRuntime.launchControlMixer[0]; } #endif @@ -896,7 +490,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) // Find roll/pitch/yaw desired output float motorMix[MAX_SUPPORTED_MOTORS]; float motorMixMax = 0, motorMixMin = 0; - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { float mix = scaledAxisPidRoll * activeMixer[i].roll + @@ -946,7 +540,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) motorMixRange = motorMixMax - motorMixMin; if (motorMixRange > 1.0f) { - for (int i = 0; i < motorCount; i++) { + for (int i = 0; i < mixerRuntime.motorCount; i++) { motorMix[i] /= motorMixRange; } // Get the maximum correction by setting offset to center when airmode enabled @@ -968,7 +562,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) if (featureIsEnabled(FEATURE_MOTOR_STOP) && ARMING_FLAG(ARMED) - && !feature3dEnabled + && !mixerRuntime.feature3dEnabled && !airmodeEnabled && !FLIGHT_MODE(GPS_RESCUE_MODE) // disable motor_stop while GPS Rescue is active && (rcData[THROTTLE] < rxConfig()->mincheck)) { @@ -989,39 +583,3 @@ float mixerGetThrottle(void) { return mixerThrottle; } - -mixerMode_e getMixerMode(void) -{ - return currentMixerMode; -} - -bool mixerModeIsFixedWing(mixerMode_e mixerMode) -{ - switch (mixerMode) { - case MIXER_FLYING_WING: - case MIXER_AIRPLANE: - case MIXER_CUSTOM_AIRPLANE: - return true; - - break; - default: - return false; - - break; - } -} - -bool isFixedWing(void) -{ - return mixerModeIsFixedWing(currentMixerMode); -} - -float getMotorOutputLow(void) -{ - return motorOutputLow; -} - -float getMotorOutputHigh(void) -{ - return motorOutputHigh; -} diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c new file mode 100644 index 000000000..842e10510 --- /dev/null +++ b/src/main/flight/mixer_init.c @@ -0,0 +1,461 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#include +#include +#include + +#include "platform.h" + +#include "build/build_config.h" +#include "build/debug.h" + +#include "config/config.h" +#include "config/feature.h" + +#include "drivers/dshot.h" + +#include "fc/controlrate_profile.h" +#include "fc/runtime_config.h" + +#include "flight/mixer_tricopter.h" +#include "flight/pid.h" + +#include "rx/rx.h" + +#include "sensors/battery.h" + +#include "mixer_init.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 0); + +PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, + .mixerMode = DEFAULT_MIXER, + .yaw_motors_reversed = false, + .crashflip_motor_percent = 0, + .crashflip_expo = 35 +); + +PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0); + +mixerMode_e currentMixerMode; + +static const motorMixer_t mixerQuadX[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L +}; +#ifndef USE_QUAD_MIXER_ONLY +static const motorMixer_t mixerTricopter[] = { + { 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, 0.0f }, // LEFT +}; + +static const motorMixer_t mixerQuadP[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT +}; + +#if defined(USE_UNCOMMON_MIXERS) +static const motorMixer_t mixerBicopter[] = { + { 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT + { 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT +}; +#else +#define mixerBicopter NULL +#endif + +static const motorMixer_t mixerY4[] = { + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW + { 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW + { 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW + { 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW +}; + + +#if (MAX_SUPPORTED_MOTORS >= 6) +static const motorMixer_t mixerHex6X[] = { + { 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R + { 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R + { 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L + { 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT +}; + +#if defined(USE_UNCOMMON_MIXERS) +static const motorMixer_t mixerHex6H[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT + { 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT +}; + +static const motorMixer_t mixerHex6P[] = { + { 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R + { 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R + { 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L + { 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L + { 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR +}; +static const motorMixer_t mixerY6[] = { + { 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR + { 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT + { 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT + { 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR + { 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT + { 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT +}; +#else +#define mixerHex6H NULL +#define mixerHex6P NULL +#define mixerY6 NULL +#endif // USE_UNCOMMON_MIXERS +#else +#define mixerHex6X NULL +#endif // MAX_SUPPORTED_MOTORS >= 6 + +#if defined(USE_UNCOMMON_MIXERS) && (MAX_SUPPORTED_MOTORS >= 8) +static const motorMixer_t mixerOctoX8[] = { + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R + { 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R + { 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L + { 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L +}; + +static const motorMixer_t mixerOctoFlatP[] = { + { 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L + { 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R + { 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R + { 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L + { 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT + { 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT + { 1.0f, 0.0f, 1.0f, -1.0f }, // REAR + { 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT +}; + +static const motorMixer_t mixerOctoFlatX[] = { + { 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L + { 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R + { 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L + { 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R + { 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R + { 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L +}; +#else +#define mixerOctoX8 NULL +#define mixerOctoFlatP NULL +#define mixerOctoFlatX NULL +#endif + +static const motorMixer_t mixerVtail4[] = { + { 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R + { 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R + { 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L + { 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L +}; + +static const motorMixer_t mixerAtail4[] = { + { 1.0f, -0.58f, 0.58f, -1.0f }, // REAR_R + { 1.0f, -0.46f, -0.39f, 0.5f }, // FRONT_R + { 1.0f, 0.58f, 0.58f, 1.0f }, // REAR_L + { 1.0f, 0.46f, -0.39f, -0.5f }, // FRONT_L +}; + +#if defined(USE_UNCOMMON_MIXERS) +static const motorMixer_t mixerDualcopter[] = { + { 1.0f, 0.0f, 0.0f, -1.0f }, // LEFT + { 1.0f, 0.0f, 0.0f, 1.0f }, // RIGHT +}; +#else +#define mixerDualcopter NULL +#endif + +static const motorMixer_t mixerSingleProp[] = { + { 1.0f, 0.0f, 0.0f, 0.0f }, +}; + +static const motorMixer_t mixerQuadX1234[] = { + { 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L + { 1.0f, -1.0f, -1.0f, 1.0f }, // FRONT_R + { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R + { 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L +}; + +// Keep synced with mixerMode_e +// Some of these entries are bogus when servos (USE_SERVOS) are not configured, +// but left untouched to keep ordinals synced with mixerMode_e (and configurator). +const mixer_t mixers[] = { + // motors, use servo, motor mixer + { 0, false, NULL }, // entry 0 + { 3, true, mixerTricopter }, // MIXER_TRI + { 4, false, mixerQuadP }, // MIXER_QUADP + { 4, false, mixerQuadX }, // MIXER_QUADX + { 2, true, mixerBicopter }, // MIXER_BICOPTER + { 0, true, NULL }, // * MIXER_GIMBAL + { 6, false, mixerY6 }, // MIXER_Y6 + { 6, false, mixerHex6P }, // MIXER_HEX6 + { 1, true, mixerSingleProp }, // * MIXER_FLYING_WING + { 4, false, mixerY4 }, // MIXER_Y4 + { 6, false, mixerHex6X }, // MIXER_HEX6X + { 8, false, mixerOctoX8 }, // MIXER_OCTOX8 + { 8, false, mixerOctoFlatP }, // MIXER_OCTOFLATP + { 8, false, mixerOctoFlatX }, // MIXER_OCTOFLATX + { 1, true, mixerSingleProp }, // * MIXER_AIRPLANE + { 1, true, mixerSingleProp }, // * MIXER_HELI_120_CCPM + { 0, true, NULL }, // * MIXER_HELI_90_DEG + { 4, false, mixerVtail4 }, // MIXER_VTAIL4 + { 6, false, mixerHex6H }, // MIXER_HEX6H + { 0, true, NULL }, // * MIXER_PPM_TO_SERVO + { 2, true, mixerDualcopter }, // MIXER_DUALCOPTER + { 1, true, NULL }, // MIXER_SINGLECOPTER + { 4, false, mixerAtail4 }, // MIXER_ATAIL4 + { 0, false, NULL }, // MIXER_CUSTOM + { 2, true, NULL }, // MIXER_CUSTOM_AIRPLANE + { 3, true, NULL }, // MIXER_CUSTOM_TRI + { 4, false, mixerQuadX1234 }, +}; +#endif // !USE_QUAD_MIXER_ONLY + +FAST_DATA_ZERO_INIT mixerRuntime_t mixerRuntime; + +uint8_t getMotorCount(void) +{ + return mixerRuntime.motorCount; +} + +bool areMotorsRunning(void) +{ + bool motorsRunning = false; + if (ARMING_FLAG(ARMED)) { + motorsRunning = true; + } else { + for (int i = 0; i < mixerRuntime.motorCount; i++) { + if (motor_disarmed[i] != mixerRuntime.disarmMotorOutput) { + motorsRunning = true; + + break; + } + } + } + + return motorsRunning; +} + +#ifdef USE_SERVOS +bool mixerIsTricopter(void) +{ + return (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI); +} +#endif + +// All PWM motor scaling is done to standard PWM range of 1000-2000 for easier tick conversion with legacy code / configurator +// DSHOT scaling is done to the actual dshot range +void initEscEndpoints(void) +{ + float motorOutputLimit = 1.0f; + if (currentPidProfile->motor_output_limit < 100) { + motorOutputLimit = currentPidProfile->motor_output_limit / 100.0f; + } + + motorInitEndpoints(motorConfig(), motorOutputLimit, &mixerRuntime.motorOutputLow, &mixerRuntime.motorOutputHigh, &mixerRuntime.disarmMotorOutput, &mixerRuntime.deadbandMotor3dHigh, &mixerRuntime.deadbandMotor3dLow); + if (!mixerRuntime.feature3dEnabled && currentPidProfile->idle_min_rpm) { + mixerRuntime.motorOutputLow = DSHOT_MIN_THROTTLE; + } +} + +// Initialize pidProfile related mixer settings +void mixerInitProfile(void) +{ +#ifdef USE_DYN_IDLE + mixerRuntime.idleMinMotorRps = currentPidProfile->idle_min_rpm * 100.0f / 60.0f; + mixerRuntime.idleMaxIncrease = currentPidProfile->idle_max_increase * 0.001f; + mixerRuntime.idleP = currentPidProfile->idle_p * 0.0001f; + mixerRuntime.oldMinRps = 0; +#endif + +#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) + mixerRuntime.vbatSagCompensationFactor = 0.0f; + if (currentPidProfile->vbat_sag_compensation > 0) { + //TODO: Make this voltage user configurable + mixerRuntime.vbatFull = CELL_VOLTAGE_FULL_CV; + mixerRuntime.vbatRangeToCompensate = mixerRuntime.vbatFull - batteryConfig()->vbatwarningcellvoltage; + if (mixerRuntime.vbatRangeToCompensate > 0) { + mixerRuntime.vbatSagCompensationFactor = ((float)currentPidProfile->vbat_sag_compensation) / 100.0f; + } + } +#endif +} + +void mixerInit(mixerMode_e mixerMode) +{ + currentMixerMode = mixerMode; + + mixerRuntime.feature3dEnabled = featureIsEnabled(FEATURE_3D); + + initEscEndpoints(); +#ifdef USE_SERVOS + if (mixerIsTricopter()) { + mixerTricopterInit(); + } +#endif + +#ifdef USE_DYN_IDLE + mixerRuntime.idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f; +#endif + + mixerInitProfile(); +} + +#ifdef USE_LAUNCH_CONTROL +// Create a custom mixer for launch control based on the current settings +// but disable the front motors. We don't care about roll or yaw because they +// are limited in the PID controller. +void loadLaunchControlMixer(void) +{ + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + mixerRuntime.launchControlMixer[i] = mixerRuntime.currentMixer[i]; + // limit the front motors to minimum output + if (mixerRuntime.launchControlMixer[i].pitch < 0.0f) { + mixerRuntime.launchControlMixer[i].pitch = 0.0f; + mixerRuntime.launchControlMixer[i].throttle = 0.0f; + } + } +} +#endif + +#ifndef USE_QUAD_MIXER_ONLY + +void mixerConfigureOutput(void) +{ + mixerRuntime.motorCount = 0; + + if (currentMixerMode == MIXER_CUSTOM || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM_AIRPLANE) { + // load custom mixer into currentMixer + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + // check if done + if (customMotorMixer(i)->throttle == 0.0f) { + break; + } + mixerRuntime.currentMixer[i] = *customMotorMixer(i); + mixerRuntime.motorCount++; + } + } else { + mixerRuntime.motorCount = mixers[currentMixerMode].motorCount; + if (mixerRuntime.motorCount > MAX_SUPPORTED_MOTORS) { + mixerRuntime.motorCount = MAX_SUPPORTED_MOTORS; + } + // copy motor-based mixers + if (mixers[currentMixerMode].motor) { + for (int i = 0; i < mixerRuntime.motorCount; i++) + mixerRuntime.currentMixer[i] = mixers[currentMixerMode].motor[i]; + } + } +#ifdef USE_LAUNCH_CONTROL + loadLaunchControlMixer(); +#endif + mixerResetDisarmedMotors(); +} + +void mixerLoadMix(int index, motorMixer_t *customMixers) +{ + // we're 1-based + index++; + // clear existing + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + customMixers[i].throttle = 0.0f; + } + // do we have anything here to begin with? + if (mixers[index].motor != NULL) { + for (int i = 0; i < mixers[index].motorCount; i++) { + customMixers[i] = mixers[index].motor[i]; + } + } +} +#else +void mixerConfigureOutput(void) +{ + mixerRuntime.motorCount = QUAD_MOTOR_COUNT; + for (int i = 0; i < mixerRuntime.motorCount; i++) { + mixerRuntime.currentMixer[i] = mixerQuadX[i]; + } +#ifdef USE_LAUNCH_CONTROL + loadLaunchControlMixer(); +#endif + mixerResetDisarmedMotors(); +} +#endif // USE_QUAD_MIXER_ONLY + +void mixerResetDisarmedMotors(void) +{ + // set disarmed motor values + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + motor_disarmed[i] = mixerRuntime.disarmMotorOutput; + } +} + +mixerMode_e getMixerMode(void) +{ + return currentMixerMode; +} + +bool mixerModeIsFixedWing(mixerMode_e mixerMode) +{ + switch (mixerMode) { + case MIXER_FLYING_WING: + case MIXER_AIRPLANE: + case MIXER_CUSTOM_AIRPLANE: + return true; + + break; + default: + return false; + + break; + } +} + +bool isFixedWing(void) +{ + return mixerModeIsFixedWing(currentMixerMode); +} + +float getMotorOutputLow(void) +{ + return mixerRuntime.motorOutputLow; +} + +float getMotorOutputHigh(void) +{ + return mixerRuntime.motorOutputHigh; +} diff --git a/src/main/flight/mixer_init.h b/src/main/flight/mixer_init.h new file mode 100644 index 000000000..dd55fb943 --- /dev/null +++ b/src/main/flight/mixer_init.h @@ -0,0 +1,54 @@ +/* + * This file is part of Cleanflight and Betaflight. + * + * Cleanflight and Betaflight are free software. You can redistribute + * this software and/or modify this software under the terms of the + * GNU General Public License as published by the Free Software + * Foundation, either version 3 of the License, or (at your option) + * any later version. + * + * Cleanflight and Betaflight are distributed in the hope that they + * will be useful, but WITHOUT ANY WARRANTY; without even the implied + * warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this software. + * + * If not, see . + */ + +#pragma once + +#include "platform.h" + +#include "flight/mixer.h" + + +typedef struct mixerRuntime_s { + uint8_t motorCount; + motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +#ifdef USE_LAUNCH_CONTROL + motorMixer_t launchControlMixer[MAX_SUPPORTED_MOTORS]; +#endif + bool feature3dEnabled; + float motorOutputLow; + float motorOutputHigh; + float disarmMotorOutput; + float deadbandMotor3dHigh; + float deadbandMotor3dLow; +#ifdef USE_DYN_IDLE + float idleMaxIncrease; + float idleThrottleOffset; + float idleMinMotorRps; + float idleP; + float oldMinRps; +#endif +#if defined(USE_BATTERY_VOLTAGE_SAG_COMPENSATION) + float vbatSagCompensationFactor; + float vbatFull; + float vbatRangeToCompensate; +#endif +} mixerRuntime_t; + +extern mixerRuntime_t mixerRuntime; From e4b3a1f2e57967714048e64434853cd67a25ff72 Mon Sep 17 00:00:00 2001 From: mikeller Date: Sun, 11 Oct 2020 13:02:32 +1300 Subject: [PATCH 2/2] Moved mixer profile initialisation to after pid initialisation. --- src/main/fc/init.c | 2 ++ src/main/flight/mixer_init.c | 2 -- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/init.c b/src/main/fc/init.c index 2b2684fd9..4d1ee94e2 100644 --- a/src/main/fc/init.c +++ b/src/main/fc/init.c @@ -737,6 +737,8 @@ void init(void) pidInit(currentPidProfile); + mixerInitProfile(); + #ifdef USE_PID_AUDIO pidAudioInit(); #endif diff --git a/src/main/flight/mixer_init.c b/src/main/flight/mixer_init.c index 842e10510..4cc5c9127 100644 --- a/src/main/flight/mixer_init.c +++ b/src/main/flight/mixer_init.c @@ -333,8 +333,6 @@ void mixerInit(mixerMode_e mixerMode) #ifdef USE_DYN_IDLE mixerRuntime.idleThrottleOffset = motorConfig()->digitalIdleOffsetValue * 0.0001f; #endif - - mixerInitProfile(); } #ifdef USE_LAUNCH_CONTROL