Merge pull request #10261 from mikeller/split_mixer
This commit is contained in:
Michael Keller
GitHub
commit
efa39dc0f6
|
|
@ -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) \
|
||||
|
|
|
|||
|
|
@ -737,6 +737,8 @@ void init(void)
|
|||
|
||||
pidInit(currentPidProfile);
|
||||
|
||||
mixerInitProfile();
|
||||
|
||||
#ifdef USE_PID_AUDIO
|
||||
pidAudioInit();
|
||||
#endif
|
||||
|
|
|
|||
|
|
@ -20,471 +20,65 @@
|
|||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
#include <math.h>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -0,0 +1,459 @@
|
|||
/*
|
||||
* 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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <stdbool.h>
|
||||
#include <stdint.h>
|
||||
#include <string.h>
|
||||
|
||||
#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
|
||||
}
|
||||
|
||||
#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;
|
||||
}
|
||||
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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;
|
||||
Loading…
Reference in New Issue