mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-19 06:15:16 +03:00
Merge branch 'master' of https://github.com/cleanflight/cleanflight
This commit is contained in:
Nicholas Sherlock
commit
dfd8e5b2b0
33 changed files with 601 additions and 603 deletions
|
|
@ -42,6 +42,9 @@
|
|||
#include "config/runtime_config.h"
|
||||
#include "config/config.h"
|
||||
|
||||
#define GIMBAL_SERVO_PITCH 0
|
||||
#define GIMBAL_SERVO_ROLL 1
|
||||
|
||||
#define AUX_FORWARD_CHANNEL_TO_SERVO_COUNT 4
|
||||
|
||||
uint8_t motorCount = 0;
|
||||
|
|
@ -62,7 +65,7 @@ static rxConfig_t *rxConfig;
|
|||
static gimbalConfig_t *gimbalConfig;
|
||||
|
||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||
static MultiType currentMixerConfiguration;
|
||||
static mixerMode_e currentMixerMode;
|
||||
|
||||
static const motorMixer_t mixerQuadX[] = {
|
||||
{ 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R
|
||||
|
|
@ -70,8 +73,7 @@ static const motorMixer_t mixerQuadX[] = {
|
|||
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
|
||||
{ 1.0f, 1.0f, -1.0f, -1.0f }, // FRONT_L
|
||||
};
|
||||
|
||||
#ifndef CJMCU
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
static const motorMixer_t mixerTri[] = {
|
||||
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
|
||||
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
|
||||
|
|
@ -189,29 +191,29 @@ static const motorMixer_t mixerDualcopter[] = {
|
|||
const mixer_t mixers[] = {
|
||||
// Mo Se Mixtable
|
||||
{ 0, 0, NULL }, // entry 0
|
||||
{ 3, 1, mixerTri }, // MULTITYPE_TRI
|
||||
{ 4, 0, mixerQuadP }, // MULTITYPE_QUADP
|
||||
{ 4, 0, mixerQuadX }, // MULTITYPE_QUADX
|
||||
{ 2, 1, mixerBi }, // MULTITYPE_BI
|
||||
{ 0, 1, NULL }, // * MULTITYPE_GIMBAL
|
||||
{ 6, 0, mixerY6 }, // MULTITYPE_Y6
|
||||
{ 6, 0, mixerHex6P }, // MULTITYPE_HEX6
|
||||
{ 1, 1, NULL }, // * MULTITYPE_FLYING_WING
|
||||
{ 4, 0, mixerY4 }, // MULTITYPE_Y4
|
||||
{ 6, 0, mixerHex6X }, // MULTITYPE_HEX6X
|
||||
{ 8, 0, mixerOctoX8 }, // MULTITYPE_OCTOX8
|
||||
{ 8, 0, mixerOctoFlatP }, // MULTITYPE_OCTOFLATP
|
||||
{ 8, 0, mixerOctoFlatX }, // MULTITYPE_OCTOFLATX
|
||||
{ 1, 1, NULL }, // * MULTITYPE_AIRPLANE
|
||||
{ 0, 1, NULL }, // * MULTITYPE_HELI_120_CCPM
|
||||
{ 0, 1, NULL }, // * MULTITYPE_HELI_90_DEG
|
||||
{ 4, 0, mixerVtail4 }, // MULTITYPE_VTAIL4
|
||||
{ 6, 0, mixerHex6H }, // MULTITYPE_HEX6H
|
||||
{ 0, 1, NULL }, // * MULTITYPE_PPM_TO_SERVO
|
||||
{ 2, 1, mixerDualcopter }, // MULTITYPE_DUALCOPTER
|
||||
{ 1, 1, NULL }, // MULTITYPE_SINGLECOPTER
|
||||
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
|
||||
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
|
||||
{ 3, 1, mixerTri }, // MIXER_TRI
|
||||
{ 4, 0, mixerQuadP }, // MIXER_QUADP
|
||||
{ 4, 0, mixerQuadX }, // MIXER_QUADX
|
||||
{ 2, 1, mixerBi }, // MIXER_BI
|
||||
{ 0, 1, NULL }, // * MIXER_GIMBAL
|
||||
{ 6, 0, mixerY6 }, // MIXER_Y6
|
||||
{ 6, 0, mixerHex6P }, // MIXER_HEX6
|
||||
{ 1, 1, NULL }, // * MIXER_FLYING_WING
|
||||
{ 4, 0, mixerY4 }, // MIXER_Y4
|
||||
{ 6, 0, mixerHex6X }, // MIXER_HEX6X
|
||||
{ 8, 0, mixerOctoX8 }, // MIXER_OCTOX8
|
||||
{ 8, 0, mixerOctoFlatP }, // MIXER_OCTOFLATP
|
||||
{ 8, 0, mixerOctoFlatX }, // MIXER_OCTOFLATX
|
||||
{ 1, 1, NULL }, // * MIXER_AIRPLANE
|
||||
{ 0, 1, NULL }, // * MIXER_HELI_120_CCPM
|
||||
{ 0, 1, NULL }, // * MIXER_HELI_90_DEG
|
||||
{ 4, 0, mixerVtail4 }, // MIXER_VTAIL4
|
||||
{ 6, 0, mixerHex6H }, // MIXER_HEX6H
|
||||
{ 0, 1, NULL }, // * MIXER_PPM_TO_SERVO
|
||||
{ 2, 1, mixerDualcopter }, // MIXER_DUALCOPTER
|
||||
{ 1, 1, NULL }, // MIXER_SINGLECOPTER
|
||||
{ 4, 0, mixerAtail4 }, // MIXER_ATAIL4
|
||||
{ 0, 0, NULL }, // MIXER_CUSTOM
|
||||
};
|
||||
#endif
|
||||
|
||||
|
|
@ -258,15 +260,15 @@ int servoDirection(int nr, int lr)
|
|||
static motorMixer_t *customMixers;
|
||||
|
||||
|
||||
#ifndef CJMCU
|
||||
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||
{
|
||||
currentMixerConfiguration = mixerConfiguration;
|
||||
currentMixerMode = mixerMode;
|
||||
|
||||
customMixers = initialCustomMixers;
|
||||
|
||||
// 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 (feature(FEATURE_SERVO_TILT))
|
||||
useServo = 1;
|
||||
|
|
@ -278,7 +280,7 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
|
||||
servoCount = pwmOutputConfiguration->servoCount;
|
||||
|
||||
if (currentMixerConfiguration == MULTITYPE_CUSTOM) {
|
||||
if (currentMixerMode == MIXER_CUSTOM) {
|
||||
// load custom mixer into currentMixer
|
||||
for (i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
|
||||
// check if done
|
||||
|
|
@ -288,11 +290,11 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
motorCount++;
|
||||
}
|
||||
} else {
|
||||
motorCount = mixers[currentMixerConfiguration].motorCount;
|
||||
motorCount = mixers[currentMixerMode].motorCount;
|
||||
// copy motor-based mixers
|
||||
if (mixers[currentMixerConfiguration].motor) {
|
||||
if (mixers[currentMixerMode].motor) {
|
||||
for (i = 0; i < motorCount; i++)
|
||||
currentMixer[i] = mixers[currentMixerConfiguration].motor[i];
|
||||
currentMixer[i] = mixers[currentMixerMode].motor[i];
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -308,19 +310,37 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
}
|
||||
|
||||
// set flag that we're on something with wings
|
||||
if (currentMixerConfiguration == MULTITYPE_FLYING_WING ||
|
||||
currentMixerConfiguration == MULTITYPE_AIRPLANE)
|
||||
if (currentMixerMode == MIXER_FLYING_WING ||
|
||||
currentMixerMode == MIXER_AIRPLANE)
|
||||
ENABLE_STATE(FIXED_WING);
|
||||
else
|
||||
DISABLE_STATE(FIXED_WING);
|
||||
|
||||
mixerResetMotors();
|
||||
}
|
||||
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||
{
|
||||
int i;
|
||||
|
||||
// we're 1-based
|
||||
index++;
|
||||
// clear existing
|
||||
for (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 (i = 0; i < mixers[index].motorCount; i++)
|
||||
customMixers[i] = mixers[index].motor[i];
|
||||
}
|
||||
}
|
||||
|
||||
#else
|
||||
|
||||
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers)
|
||||
{
|
||||
currentMixerConfiguration = mixerConfiguration;
|
||||
currentMixerMode = mixerMode;
|
||||
|
||||
customMixers = initialCustomMixers;
|
||||
|
||||
|
|
@ -342,7 +362,6 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
|||
}
|
||||
#endif
|
||||
|
||||
|
||||
void mixerResetMotors(void)
|
||||
{
|
||||
int i;
|
||||
|
|
@ -351,25 +370,6 @@ void mixerResetMotors(void)
|
|||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
|
||||
}
|
||||
|
||||
#ifndef CJMCU
|
||||
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||
{
|
||||
int i;
|
||||
|
||||
// we're 1-based
|
||||
index++;
|
||||
// clear existing
|
||||
for (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 (i = 0; i < mixers[index].motorCount; i++)
|
||||
customMixers[i] = mixers[index].motor[i];
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
static void updateGimbalServos(void)
|
||||
{
|
||||
pwmWriteServo(0, servo[0]);
|
||||
|
|
@ -381,13 +381,13 @@ void writeServos(void)
|
|||
if (!useServo)
|
||||
return;
|
||||
|
||||
switch (currentMixerConfiguration) {
|
||||
case MULTITYPE_BI:
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BI:
|
||||
pwmWriteServo(0, servo[4]);
|
||||
pwmWriteServo(1, servo[5]);
|
||||
break;
|
||||
|
||||
case MULTITYPE_TRI:
|
||||
case MIXER_TRI:
|
||||
if (mixerConfig->tri_unarmed_servo) {
|
||||
// if unarmed flag set, we always move servo
|
||||
pwmWriteServo(0, servo[5]);
|
||||
|
|
@ -400,22 +400,22 @@ void writeServos(void)
|
|||
}
|
||||
break;
|
||||
|
||||
case MULTITYPE_FLYING_WING:
|
||||
case MIXER_FLYING_WING:
|
||||
pwmWriteServo(0, servo[3]);
|
||||
pwmWriteServo(1, servo[4]);
|
||||
break;
|
||||
|
||||
case MULTITYPE_GIMBAL:
|
||||
case MIXER_GIMBAL:
|
||||
updateGimbalServos();
|
||||
break;
|
||||
|
||||
case MULTITYPE_DUALCOPTER:
|
||||
case MIXER_DUALCOPTER:
|
||||
pwmWriteServo(0, servo[4]);
|
||||
pwmWriteServo(1, servo[5]);
|
||||
break;
|
||||
|
||||
case MULTITYPE_AIRPLANE:
|
||||
case MULTITYPE_SINGLECOPTER:
|
||||
case MIXER_AIRPLANE:
|
||||
case MIXER_SINGLECOPTER:
|
||||
pwmWriteServo(0, servo[3]);
|
||||
pwmWriteServo(1, servo[4]);
|
||||
pwmWriteServo(2, servo[5]);
|
||||
|
|
@ -454,6 +454,7 @@ void writeAllMotors(int16_t mc)
|
|||
writeMotors();
|
||||
}
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
static void airplaneMixer(void)
|
||||
{
|
||||
int16_t flapperons[2] = { 0, 0 };
|
||||
|
|
@ -499,6 +500,7 @@ static void airplaneMixer(void)
|
|||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void mixTable(void)
|
||||
{
|
||||
|
|
@ -513,29 +515,34 @@ void mixTable(void)
|
|||
// motors for non-servo mixes
|
||||
if (motorCount > 1)
|
||||
for (i = 0; i < motorCount; i++)
|
||||
motor[i] = rcCommand[THROTTLE] * currentMixer[i].throttle + axisPID[PITCH] * currentMixer[i].pitch + axisPID[ROLL] * currentMixer[i].roll + -mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
|
||||
motor[i] =
|
||||
rcCommand[THROTTLE] * currentMixer[i].throttle +
|
||||
axisPID[PITCH] * currentMixer[i].pitch +
|
||||
axisPID[ROLL] * currentMixer[i].roll +
|
||||
-mixerConfig->yaw_direction * axisPID[YAW] * currentMixer[i].yaw;
|
||||
|
||||
#ifndef USE_QUAD_MIXER_ONLY
|
||||
// airplane / servo mixes
|
||||
switch (currentMixerConfiguration) {
|
||||
case MULTITYPE_BI:
|
||||
switch (currentMixerMode) {
|
||||
case MIXER_BI:
|
||||
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
|
||||
break;
|
||||
|
||||
case MULTITYPE_TRI:
|
||||
case MIXER_TRI:
|
||||
servo[5] = (servoDirection(5, 1) * axisPID[YAW]) + determineServoMiddleOrForwardFromChannel(5); // REAR
|
||||
break;
|
||||
|
||||
case MULTITYPE_GIMBAL:
|
||||
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);
|
||||
case MIXER_GIMBAL:
|
||||
servo[GIMBAL_SERVO_PITCH] = (((int32_t)servoConf[GIMBAL_SERVO_PITCH].rate * inclination.values.pitchDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_PITCH);
|
||||
servo[GIMBAL_SERVO_ROLL] = (((int32_t)servoConf[GIMBAL_SERVO_ROLL].rate * inclination.values.rollDeciDegrees) / 50) + determineServoMiddleOrForwardFromChannel(GIMBAL_SERVO_ROLL);
|
||||
break;
|
||||
|
||||
case MULTITYPE_AIRPLANE:
|
||||
case MIXER_AIRPLANE:
|
||||
airplaneMixer();
|
||||
break;
|
||||
|
||||
case MULTITYPE_FLYING_WING:
|
||||
case MIXER_FLYING_WING:
|
||||
if (!ARMING_FLAG(ARMED))
|
||||
servo[7] = escAndServoConfig->mincommand;
|
||||
else
|
||||
|
|
@ -554,14 +561,14 @@ void mixTable(void)
|
|||
servo[4] += determineServoMiddleOrForwardFromChannel(4);
|
||||
break;
|
||||
|
||||
case MULTITYPE_DUALCOPTER:
|
||||
case MIXER_DUALCOPTER:
|
||||
for (i = 4; i < 6; i++) {
|
||||
servo[i] = axisPID[5 - i] * servoDirection(i, 1); // mix and setup direction
|
||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||
}
|
||||
break;
|
||||
|
||||
case MULTITYPE_SINGLECOPTER:
|
||||
case MIXER_SINGLECOPTER:
|
||||
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] += determineServoMiddleOrForwardFromChannel(i);
|
||||
|
|
@ -572,6 +579,7 @@ void mixTable(void)
|
|||
default:
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
|
||||
// do camstab
|
||||
if (feature(FEATURE_SERVO_TILT)) {
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue