mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Rename multiType to mixerMode. Rename MULTITYPE_* to MIXER_*.
'Type' is a noise word. 'Multi' is a mis-nomer - there is nothing 'multi' about a gimbal or fixed wing.
This commit is contained in:
parent
b123b4ef03
commit
ee19c1f071
10 changed files with 100 additions and 101 deletions
|
@ -309,7 +309,7 @@ static void resetConf(void)
|
||||||
setControlRateProfile(0);
|
setControlRateProfile(0);
|
||||||
|
|
||||||
masterConfig.version = EEPROM_CONF_VERSION;
|
masterConfig.version = EEPROM_CONF_VERSION;
|
||||||
masterConfig.mixerConfiguration = MULTITYPE_QUADX;
|
masterConfig.mixerMode = MIXER_QUADX;
|
||||||
featureClearAll();
|
featureClearAll();
|
||||||
#if defined(CJMCU) || defined(SPARKY)
|
#if defined(CJMCU) || defined(SPARKY)
|
||||||
featureSet(FEATURE_RX_PPM);
|
featureSet(FEATURE_RX_PPM);
|
||||||
|
|
|
@ -23,7 +23,7 @@ typedef struct master_t {
|
||||||
uint16_t size;
|
uint16_t size;
|
||||||
uint8_t magic_be; // magic number, should be 0xBE
|
uint8_t magic_be; // magic number, should be 0xBE
|
||||||
|
|
||||||
uint8_t mixerConfiguration;
|
uint8_t mixerMode;
|
||||||
uint32_t enabledFeatures;
|
uint32_t enabledFeatures;
|
||||||
uint16_t looptime; // imu loop time in us
|
uint16_t looptime; // imu loop time in us
|
||||||
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
|
uint8_t emf_avoidance; // change pll settings to avoid noise in the uhf band
|
||||||
|
|
|
@ -103,7 +103,7 @@ void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
|
||||||
fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
|
fc_acc = 0.5f / (M_PI * accz_lpf_cutoff); // calculate RC time constant used in the accZ lpf
|
||||||
}
|
}
|
||||||
|
|
||||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration)
|
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode)
|
||||||
{
|
{
|
||||||
static int16_t gyroYawSmooth = 0;
|
static int16_t gyroYawSmooth = 0;
|
||||||
|
|
||||||
|
@ -120,7 +120,7 @@ void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfigurat
|
||||||
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
|
gyroData[FD_ROLL] = gyroADC[FD_ROLL];
|
||||||
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
|
gyroData[FD_PITCH] = gyroADC[FD_PITCH];
|
||||||
|
|
||||||
if (mixerConfiguration == MULTITYPE_TRI) {
|
if (mixerMode == MIXER_TRI) {
|
||||||
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
|
gyroData[FD_YAW] = (gyroYawSmooth * 2 + gyroADC[FD_YAW]) / 3;
|
||||||
gyroYawSmooth = gyroData[FD_YAW];
|
gyroYawSmooth = gyroData[FD_YAW];
|
||||||
} else {
|
} else {
|
||||||
|
|
|
@ -33,7 +33,7 @@ typedef struct imuRuntimeConfig_s {
|
||||||
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
|
void configureImu(imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband);
|
||||||
|
|
||||||
void calculateEstimatedAltitude(uint32_t currentTime);
|
void calculateEstimatedAltitude(uint32_t currentTime);
|
||||||
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerConfiguration);
|
void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode);
|
||||||
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
void calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||||
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
void calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
||||||
|
|
|
@ -62,7 +62,7 @@ static rxConfig_t *rxConfig;
|
||||||
static gimbalConfig_t *gimbalConfig;
|
static gimbalConfig_t *gimbalConfig;
|
||||||
|
|
||||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||||
static multiType_e currentMixerConfiguration;
|
static mixerMode_e currentMixerMode;
|
||||||
|
|
||||||
static const motorMixer_t mixerQuadX[] = {
|
static const motorMixer_t mixerQuadX[] = {
|
||||||
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
||||||
|
@ -188,29 +188,29 @@ static const motorMixer_t mixerDualcopter[] = {
|
||||||
const mixer_t mixers[] = {
|
const mixer_t mixers[] = {
|
||||||
// Mo Se Mixtable
|
// Mo Se Mixtable
|
||||||
{ 0, 0, NULL }, // entry 0
|
{ 0, 0, NULL }, // entry 0
|
||||||
{ 3, 1, mixerTri }, // MULTITYPE_TRI
|
{ 3, 1, mixerTri }, // MIXER_TRI
|
||||||
{ 4, 0, mixerQuadP }, // MULTITYPE_QUADP
|
{ 4, 0, mixerQuadP }, // MIXER_QUADP
|
||||||
{ 4, 0, mixerQuadX }, // MULTITYPE_QUADX
|
{ 4, 0, mixerQuadX }, // MIXER_QUADX
|
||||||
{ 2, 1, mixerBi }, // MULTITYPE_BI
|
{ 2, 1, mixerBi }, // MIXER_BI
|
||||||
{ 0, 1, NULL }, // * MULTITYPE_GIMBAL
|
{ 0, 1, NULL }, // * MIXER_GIMBAL
|
||||||
{ 6, 0, mixerY6 }, // MULTITYPE_Y6
|
{ 6, 0, mixerY6 }, // MIXER_Y6
|
||||||
{ 6, 0, mixerHex6P }, // MULTITYPE_HEX6
|
{ 6, 0, mixerHex6P }, // MIXER_HEX6
|
||||||
{ 1, 1, NULL }, // * MULTITYPE_FLYING_WING
|
{ 1, 1, NULL }, // * MIXER_FLYING_WING
|
||||||
{ 4, 0, mixerY4 }, // MULTITYPE_Y4
|
{ 4, 0, mixerY4 }, // MIXER_Y4
|
||||||
{ 6, 0, mixerHex6X }, // MULTITYPE_HEX6X
|
{ 6, 0, mixerHex6X }, // MIXER_HEX6X
|
||||||
{ 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8
|
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8
|
||||||
{ 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP
|
{ 8, 0, mixerOctoFlatP }, // MIXER_OCTOFLATP
|
||||||
{ 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX
|
{ 8, 0, mixerOctoFlatX }, // MIXER_OCTOFLATX
|
||||||
{ 1, 1, NULL }, // * MULTITYPE_AIRPLANE
|
{ 1, 1, NULL }, // * MIXER_AIRPLANE
|
||||||
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
|
{ 0, 1, NULL }, // * MIXER_HELI_120_CCPM
|
||||||
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
|
{ 0, 1, NULL }, // * MIXER_HELI_90_DEG
|
||||||
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
|
{ 4, 0, mixerVtail4 }, // MIXER_VTAIL4
|
||||||
{ 6, 0, mixerHex6H }, // MULTITYPE_HEX6H
|
{ 6, 0, mixerHex6H }, // MIXER_HEX6H
|
||||||
{ 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO
|
{ 0, 1, NULL }, // * MIXER_PPM_TO_SERVO
|
||||||
{ 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER
|
{ 2, 1, mixerDualcopter }, // MIXER_DUALCOPTER
|
||||||
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
|
{ 1, 1, NULL }, // MIXER_SINGLECOPTER
|
||||||
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
|
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4
|
||||||
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
|
{ 0, 0, NULL }, // MIXER_CUSTOM
|
||||||
};
|
};
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -258,14 +258,14 @@ static motorMixer_t *customMixers;
|
||||||
|
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
void mixerInit(multiType_e mixerConfiguration, motorMixer_t *initialCustomMixers)
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerConfiguration = mixerConfiguration;
|
currentMixerMode = mixerMode;
|
||||||
|
|
||||||
customMixers = initialCustomMixers;
|
customMixers = initialCustomMixers;
|
||||||
|
|
||||||
// enable servos for mixes that require them. note, this shifts motor counts.
|
// enable servos for mixes that require them. note, this shifts motor counts.
|
||||||
useServo = mixers[currentMixerConfiguration].useServo;
|
useServo = mixers[currentMixerMode].useServo;
|
||||||
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
// if we want camstab/trig, that also enables servos, even if mixer doesn't
|
||||||
if (feature(FEATURE_SERVO_TILT))
|
if (feature(FEATURE_SERVO_TILT))
|
||||||
useServo = 1;
|
useServo = 1;
|
||||||
|
@ -277,7 +277,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
|
|
||||||
servoCount = pwmOutputConfiguration->servoCount;
|
servoCount = pwmOutputConfiguration->servoCount;
|
||||||
|
|
||||||
if (currentMixerConfiguration == MULTITYPE_CUSTOM) {
|
if (currentMixerMode == MIXER_CUSTOM) {
|
||||||
// load custom mixer into currentMixer
|
// load custom mixer into currentMixer
|
||||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||||
// check if done
|
// check if done
|
||||||
|
@ -287,11 +287,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
motorCount++;
|
motorCount++;
|
||||||
}
|
}
|
||||||
} else {
|
} else {
|
||||||
motorCount = mixers[currentMixerConfiguration].motorCount;
|
motorCount = mixers[currentMixerMode].motorCount;
|
||||||
// copy motor-based mixers
|
// copy motor-based mixers
|
||||||
if (mixers[currentMixerConfiguration].motor) {
|
if (mixers[currentMixerMode].motor) {
|
||||||
for (i = 0; i < motorCount; i++)
|
for (i = 0; i < motorCount; i++)
|
||||||
currentMixer[i] = mixers[currentMixerConfiguration].motor[i];
|
currentMixer[i] = mixers[currentMixerMode].motor[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -307,8 +307,8 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
}
|
}
|
||||||
|
|
||||||
// set flag that we're on something with wings
|
// set flag that we're on something with wings
|
||||||
if (currentMixerConfiguration == MULTITYPE_FLYING_WING ||
|
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||||
currentMixerConfiguration == MULTITYPE_AIRPLANE)
|
currentMixerMode == MIXER_AIRPLANE)
|
||||||
ENABLE_STATE(FIXED_WING);
|
ENABLE_STATE(FIXED_WING);
|
||||||
else
|
else
|
||||||
DISABLE_STATE(FIXED_WING);
|
DISABLE_STATE(FIXED_WING);
|
||||||
|
@ -335,9 +335,9 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||||
|
|
||||||
#else
|
#else
|
||||||
|
|
||||||
void mixerInit(multiType_e mixerConfiguration, motorMixer_t *initialCustomMixers)
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerConfiguration = mixerConfiguration;
|
currentMixerMode = mixerMode;
|
||||||
|
|
||||||
customMixers = initialCustomMixers;
|
customMixers = initialCustomMixers;
|
||||||
|
|
||||||
|
@ -359,7 +359,6 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
|
||||||
void mixerResetMotors(void)
|
void mixerResetMotors(void)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
@ -379,13 +378,13 @@ void writeServos(void)
|
||||||
if (!useServo)
|
if (!useServo)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
switch (currentMixerConfiguration) {
|
switch (currentMixerMode) {
|
||||||
case MULTITYPE_BI:
|
case MIXER_BI:
|
||||||
pwmWriteServo(0, servo[4]);
|
pwmWriteServo(0, servo[4]);
|
||||||
pwmWriteServo(1, servo[5]);
|
pwmWriteServo(1, servo[5]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_TRI:
|
case MIXER_TRI:
|
||||||
if (mixerConfig->tri_unarmed_servo) {
|
if (mixerConfig->tri_unarmed_servo) {
|
||||||
// if unarmed flag set, we always move servo
|
// if unarmed flag set, we always move servo
|
||||||
pwmWriteServo(0, servo[5]);
|
pwmWriteServo(0, servo[5]);
|
||||||
|
@ -398,22 +397,22 @@ void writeServos(void)
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_FLYING_WING:
|
case MIXER_FLYING_WING:
|
||||||
pwmWriteServo(0, servo[3]);
|
pwmWriteServo(0, servo[3]);
|
||||||
pwmWriteServo(1, servo[4]);
|
pwmWriteServo(1, servo[4]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_GIMBAL:
|
case MIXER_GIMBAL:
|
||||||
updateGimbalServos();
|
updateGimbalServos();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_DUALCOPTER:
|
case MIXER_DUALCOPTER:
|
||||||
pwmWriteServo(0, servo[4]);
|
pwmWriteServo(0, servo[4]);
|
||||||
pwmWriteServo(1, servo[5]);
|
pwmWriteServo(1, servo[5]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_AIRPLANE:
|
case MIXER_AIRPLANE:
|
||||||
case MULTITYPE_SINGLECOPTER:
|
case MIXER_SINGLECOPTER:
|
||||||
pwmWriteServo(0, servo[3]);
|
pwmWriteServo(0, servo[3]);
|
||||||
pwmWriteServo(1, servo[4]);
|
pwmWriteServo(1, servo[4]);
|
||||||
pwmWriteServo(2, servo[5]);
|
pwmWriteServo(2, servo[5]);
|
||||||
|
@ -517,26 +516,26 @@ void mixTable(void)
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
// airplane / servo mixes
|
// airplane / servo mixes
|
||||||
switch (currentMixerConfiguration) {
|
switch (currentMixerMode) {
|
||||||
case MULTITYPE_BI:
|
case MIXER_BI:
|
||||||
servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT
|
servo[4] = (servoDirection(4, 2) * axisPID[YAW]) + (servoDirection(4, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(4); // LEFT
|
||||||
servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT
|
servo[5] = (servoDirection(5, 2) * axisPID[YAW]) + (servoDirection(5, 1) * axisPID[PITCH]) + determineServoMiddleOrForwardFromChannel(5); // RIGHT
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_TRI:
|
case MIXER_TRI:
|
||||||
servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
|
servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_GIMBAL:
|
case MIXER_GIMBAL:
|
||||||
servo[0] = (((int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
|
servo[0] = (((int32_t)servoConf[0].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(0);
|
||||||
servo[1] = (((int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
|
servo[1] = (((int32_t)servoConf[1].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(1);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_AIRPLANE:
|
case MIXER_AIRPLANE:
|
||||||
airplaneMixer();
|
airplaneMixer();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_FLYING_WING:
|
case MIXER_FLYING_WING:
|
||||||
if (!ARMING_FLAG(ARMED))
|
if (!ARMING_FLAG(ARMED))
|
||||||
servo[7] = escAndServoConfig->mincommand;
|
servo[7] = escAndServoConfig->mincommand;
|
||||||
else
|
else
|
||||||
|
@ -555,14 +554,14 @@ void mixTable(void)
|
||||||
servo[4] += determineServoMiddleOrForwardFromChannel(4);
|
servo[4] += determineServoMiddleOrForwardFromChannel(4);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_DUALCOPTER:
|
case MIXER_DUALCOPTER:
|
||||||
for (i = 4; i < 6; i++) {
|
for (i = 4; i < 6; i++) {
|
||||||
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
|
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
|
||||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MULTITYPE_SINGLECOPTER:
|
case MIXER_SINGLECOPTER:
|
||||||
for (i = 3; i < 7; i++) {
|
for (i = 3; i < 7; i++) {
|
||||||
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
|
servo[i] = (axisPID[YAW] * servoDirection(i, 2)) + (axisPID[(6 - i) >> 1] * servoDirection(i, 1)); // mix and setup direction
|
||||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||||
|
|
|
@ -20,33 +20,33 @@
|
||||||
#define MAX_SUPPORTED_MOTORS 12
|
#define MAX_SUPPORTED_MOTORS 12
|
||||||
#define MAX_SUPPORTED_SERVOS 8
|
#define MAX_SUPPORTED_SERVOS 8
|
||||||
|
|
||||||
typedef enum MultiType
|
// Note: this is called MultiType/MULTITYPE_* in baseflight.
|
||||||
|
typedef enum mixerMode
|
||||||
{
|
{
|
||||||
MULTITYPE_TRI = 1,
|
MIXER_TRI = 1,
|
||||||
MULTITYPE_QUADP = 2,
|
MIXER_QUADP = 2,
|
||||||
MULTITYPE_QUADX = 3,
|
MIXER_QUADX = 3,
|
||||||
MULTITYPE_BI = 4,
|
MIXER_BI = 4,
|
||||||
MULTITYPE_GIMBAL = 5,
|
MIXER_GIMBAL = 5,
|
||||||
MULTITYPE_Y6 = 6,
|
MIXER_Y6 = 6,
|
||||||
MULTITYPE_HEX6 = 7,
|
MIXER_HEX6 = 7,
|
||||||
MULTITYPE_FLYING_WING = 8,
|
MIXER_FLYING_WING = 8,
|
||||||
MULTITYPE_Y4 = 9,
|
MIXER_Y4 = 9,
|
||||||
MULTITYPE_HEX6X = 10,
|
MIXER_HEX6X = 10,
|
||||||
MULTITYPE_OCTOX8 = 11,
|
MIXER_OCTOX8 = 11,
|
||||||
MULTITYPE_OCTOFLATP = 12,
|
MIXER_OCTOFLATP = 12,
|
||||||
MULTITYPE_OCTOFLATX = 13,
|
MIXER_OCTOFLATX = 13,
|
||||||
MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
|
MIXER_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
|
||||||
MULTITYPE_HELI_120_CCPM = 15,
|
MIXER_HELI_120_CCPM = 15,
|
||||||
MULTITYPE_HELI_90_DEG = 16,
|
MIXER_HELI_90_DEG = 16,
|
||||||
MULTITYPE_VTAIL4 = 17,
|
MIXER_VTAIL4 = 17,
|
||||||
MULTITYPE_HEX6H = 18,
|
MIXER_HEX6H = 18,
|
||||||
MULTITYPE_PPM_TO_SERVO = 19, // PPM -> servo relay
|
MIXER_PPM_TO_SERVO = 19, // PPM -> servo relay
|
||||||
MULTITYPE_DUALCOPTER = 20,
|
MIXER_DUALCOPTER = 20,
|
||||||
MULTITYPE_SINGLECOPTER = 21,
|
MIXER_SINGLECOPTER = 21,
|
||||||
MULTITYPE_ATAIL4 = 22,
|
MIXER_ATAIL4 = 22,
|
||||||
MULTITYPE_CUSTOM = 23,
|
MIXER_CUSTOM = 23
|
||||||
MULTITYPE_LAST = 24
|
} mixerMode_e;
|
||||||
} multiType_e;
|
|
||||||
|
|
||||||
// Custom mixer data per motor
|
// Custom mixer data per motor
|
||||||
typedef struct motorMixer_t {
|
typedef struct motorMixer_t {
|
||||||
|
|
|
@ -119,7 +119,7 @@ static char cliBuffer[48];
|
||||||
static uint32_t bufferIndex = 0;
|
static uint32_t bufferIndex = 0;
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
// sync this with multiType_e
|
// sync this with mixerMode_e
|
||||||
static const char * const mixerNames[] = {
|
static const char * const mixerNames[] = {
|
||||||
"TRI", "QUADP", "QUADX", "BI",
|
"TRI", "QUADP", "QUADX", "BI",
|
||||||
"GIMBAL", "Y6", "HEX6",
|
"GIMBAL", "Y6", "HEX6",
|
||||||
|
@ -783,7 +783,7 @@ static void cliDump(char *cmdline)
|
||||||
printf("\r\n# mixer\r\n");
|
printf("\r\n# mixer\r\n");
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
|
printf("mixer %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||||
|
|
||||||
if (masterConfig.customMixer[0].throttle != 0.0f) {
|
if (masterConfig.customMixer[0].throttle != 0.0f) {
|
||||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||||
|
@ -1032,7 +1032,7 @@ static void cliMixer(char *cmdline)
|
||||||
len = strlen(cmdline);
|
len = strlen(cmdline);
|
||||||
|
|
||||||
if (len == 0) {
|
if (len == 0) {
|
||||||
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
|
printf("Current mixer: %s\r\n", mixerNames[masterConfig.mixerMode - 1]);
|
||||||
return;
|
return;
|
||||||
} else if (strncasecmp(cmdline, "list", len) == 0) {
|
} else if (strncasecmp(cmdline, "list", len) == 0) {
|
||||||
cliPrint("Available mixers: ");
|
cliPrint("Available mixers: ");
|
||||||
|
@ -1051,7 +1051,7 @@ static void cliMixer(char *cmdline)
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
|
if (strncasecmp(cmdline, mixerNames[i], len) == 0) {
|
||||||
masterConfig.mixerConfiguration = i + 1;
|
masterConfig.mixerMode = i + 1;
|
||||||
printf("Mixer set to %s\r\n", mixerNames[i]);
|
printf("Mixer set to %s\r\n", mixerNames[i]);
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
|
|
@ -225,7 +225,7 @@ const char *boardIdentifier = TARGET_BOARD_IDENTIFIER;
|
||||||
//
|
//
|
||||||
|
|
||||||
// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
|
// DEPRECATED - See MSP_API_VERSION and MSP_MIXER
|
||||||
#define MSP_IDENT 100 //out message multitype + multiwii version + protocol version + capability variable
|
#define MSP_IDENT 100 //out message mixerMode + multiwii version + protocol version + capability variable
|
||||||
|
|
||||||
|
|
||||||
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
#define MSP_STATUS 101 //out message cycletime & errors_count & sensor present & box activation & current setting number
|
||||||
|
@ -616,7 +616,7 @@ void mspInit(serialConfig_t *serialConfig)
|
||||||
}
|
}
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE)
|
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE)
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
activeBoxIds[activeBoxIdCount++] = BOXPASSTHRU;
|
||||||
|
|
||||||
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
activeBoxIds[activeBoxIdCount++] = BOXBEEPERON;
|
||||||
|
@ -711,7 +711,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
case MSP_IDENT:
|
case MSP_IDENT:
|
||||||
headSerialReply(7);
|
headSerialReply(7);
|
||||||
serialize8(MW_VERSION);
|
serialize8(MW_VERSION);
|
||||||
serialize8(masterConfig.mixerConfiguration); // type of multicopter
|
serialize8(masterConfig.mixerMode);
|
||||||
serialize8(MSP_PROTOCOL_VERSION);
|
serialize8(MSP_PROTOCOL_VERSION);
|
||||||
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
|
serialize32(CAP_DYNBALANCE | (masterConfig.airplaneConfig.flaps_speed ? CAP_FLAPS : 0)); // "capability"
|
||||||
break;
|
break;
|
||||||
|
@ -1027,7 +1027,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
|
|
||||||
case MSP_MIXER:
|
case MSP_MIXER:
|
||||||
headSerialReply(1);
|
headSerialReply(1);
|
||||||
serialize8(masterConfig.mixerConfiguration);
|
serialize8(masterConfig.mixerMode);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MSP_RX_CONFIG:
|
case MSP_RX_CONFIG:
|
||||||
|
@ -1052,7 +1052,7 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
|
|
||||||
case MSP_CONFIG:
|
case MSP_CONFIG:
|
||||||
headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
|
headSerialReply(1 + 4 + 1 + 2 + 2 + 2 + 2 + 2);
|
||||||
serialize8(masterConfig.mixerConfiguration);
|
serialize8(masterConfig.mixerMode);
|
||||||
|
|
||||||
serialize32(featureMask());
|
serialize32(featureMask());
|
||||||
|
|
||||||
|
@ -1342,7 +1342,7 @@ static bool processInCommand(void)
|
||||||
|
|
||||||
#ifndef USE_QUAD_MIXER_ONLY
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
case MSP_SET_MIXER:
|
case MSP_SET_MIXER:
|
||||||
masterConfig.mixerConfiguration = read8();
|
masterConfig.mixerMode = read8();
|
||||||
break;
|
break;
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
|
@ -1367,9 +1367,9 @@ static bool processInCommand(void)
|
||||||
case MSP_SET_CONFIG:
|
case MSP_SET_CONFIG:
|
||||||
|
|
||||||
#ifdef USE_QUAD_MIXER_ONLY
|
#ifdef USE_QUAD_MIXER_ONLY
|
||||||
read8(); // multitype ignored
|
read8(); // mixerMode ignored
|
||||||
#else
|
#else
|
||||||
masterConfig.mixerConfiguration = read8(); // multitype
|
masterConfig.mixerMode = read8(); // mixerMode
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
featureClearAll();
|
featureClearAll();
|
||||||
|
|
|
@ -96,7 +96,7 @@ void initTelemetry(void);
|
||||||
void serialInit(serialConfig_t *initialSerialConfig);
|
void serialInit(serialConfig_t *initialSerialConfig);
|
||||||
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
|
failsafe_t* failsafeInit(rxConfig_t *intialRxConfig);
|
||||||
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
|
pwmOutputConfiguration_t *pwmInit(drv_pwm_config_t *init);
|
||||||
void mixerInit(multiType_e mixerConfiguration, motorMixer_t *customMixers);
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *customMixers);
|
||||||
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
||||||
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
|
void rxInit(rxConfig_t *rxConfig, failsafe_t *failsafe);
|
||||||
void beepcodeInit(failsafe_t *initialFailsafe);
|
void beepcodeInit(failsafe_t *initialFailsafe);
|
||||||
|
@ -264,7 +264,7 @@ void init(void)
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
|
|
||||||
imuInit();
|
imuInit();
|
||||||
mixerInit(masterConfig.mixerConfiguration, masterConfig.customMixer);
|
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
|
||||||
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
if (sensors(SENSOR_MAG))
|
if (sensors(SENSOR_MAG))
|
||||||
|
@ -275,7 +275,7 @@ void init(void)
|
||||||
|
|
||||||
memset(&pwm_params, 0, sizeof(pwm_params));
|
memset(&pwm_params, 0, sizeof(pwm_params));
|
||||||
// when using airplane/wing mixer, servo/motor outputs are remapped
|
// when using airplane/wing mixer, servo/motor outputs are remapped
|
||||||
if (masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE || masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING)
|
if (masterConfig.mixerMode == MIXER_AIRPLANE || masterConfig.mixerMode == MIXER_FLYING_WING)
|
||||||
pwm_params.airplane = true;
|
pwm_params.airplane = true;
|
||||||
else
|
else
|
||||||
pwm_params.airplane = false;
|
pwm_params.airplane = false;
|
||||||
|
@ -345,7 +345,7 @@ void init(void)
|
||||||
|
|
||||||
previousTime = micros();
|
previousTime = micros();
|
||||||
|
|
||||||
if (masterConfig.mixerConfiguration == MULTITYPE_GIMBAL) {
|
if (masterConfig.mixerMode == MIXER_GIMBAL) {
|
||||||
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
accSetCalibrationCycles(CALIBRATING_ACC_CYCLES);
|
||||||
}
|
}
|
||||||
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
gyroSetCalibrationCycles(CALIBRATING_GYRO_CYCLES);
|
||||||
|
|
|
@ -574,7 +574,7 @@ void processRx(void)
|
||||||
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
DISABLE_FLIGHT_MODE(PASSTHRU_MODE);
|
||||||
}
|
}
|
||||||
|
|
||||||
if (masterConfig.mixerConfiguration == MULTITYPE_FLYING_WING || masterConfig.mixerConfiguration == MULTITYPE_AIRPLANE) {
|
if (masterConfig.mixerMode == MIXER_FLYING_WING || masterConfig.mixerMode == MIXER_AIRPLANE) {
|
||||||
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
DISABLE_FLIGHT_MODE(HEADFREE_MODE);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
@ -627,7 +627,7 @@ void loop(void)
|
||||||
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
||||||
loopTime = currentTime + masterConfig.looptime;
|
loopTime = currentTime + masterConfig.looptime;
|
||||||
|
|
||||||
computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerConfiguration);
|
computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerMode);
|
||||||
|
|
||||||
// Measure loop rate just after reading the sensors
|
// Measure loop rate just after reading the sensors
|
||||||
currentTime = micros();
|
currentTime = micros();
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue