mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-15 20:35:33 +03:00
Cleanup disabling of mixers for CJMCU, this might be usefulfor the
AlienWii32 target too. Deleted old out of date comments. Various other minor cleanups.
This commit is contained in:
parent
db14bd80cb
commit
b123b4ef03
6 changed files with 69 additions and 54 deletions
|
@ -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 currentMixerConfiguration;
|
static multiType_e currentMixerConfiguration;
|
||||||
|
|
||||||
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
|
||||||
|
@ -70,8 +70,7 @@ static const motorMixer_t mixerQuadX[] = {
|
||||||
{ 1.0f, 1.0f, 1.0f, 1.0f }, // REAR_L
|
{ 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 }, // FRONT_L
|
||||||
};
|
};
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
#ifndef CJMCU
|
|
||||||
static const motorMixer_t mixerTri[] = {
|
static const motorMixer_t mixerTri[] = {
|
||||||
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
|
{ 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 }, // RIGHT
|
||||||
|
@ -258,8 +257,8 @@ int servoDirection(int nr, int lr)
|
||||||
static motorMixer_t *customMixers;
|
static motorMixer_t *customMixers;
|
||||||
|
|
||||||
|
|
||||||
#ifndef CJMCU
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
void mixerInit(multiType_e mixerConfiguration, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerConfiguration = mixerConfiguration;
|
currentMixerConfiguration = mixerConfiguration;
|
||||||
|
|
||||||
|
@ -316,9 +315,27 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
|
|
||||||
mixerResetMotors();
|
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
|
#else
|
||||||
|
|
||||||
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
void mixerInit(multiType_e mixerConfiguration, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerConfiguration = mixerConfiguration;
|
currentMixerConfiguration = mixerConfiguration;
|
||||||
|
|
||||||
|
@ -351,25 +368,6 @@ void mixerResetMotors(void)
|
||||||
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
|
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)
|
static void updateGimbalServos(void)
|
||||||
{
|
{
|
||||||
pwmWriteServo(0, servo[0]);
|
pwmWriteServo(0, servo[0]);
|
||||||
|
@ -454,6 +452,7 @@ void writeAllMotors(int16_t mc)
|
||||||
writeMotors();
|
writeMotors();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
static void airplaneMixer(void)
|
static void airplaneMixer(void)
|
||||||
{
|
{
|
||||||
int16_t flapperons[2] = { 0, 0 };
|
int16_t flapperons[2] = { 0, 0 };
|
||||||
|
@ -499,6 +498,7 @@ static void airplaneMixer(void)
|
||||||
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
servo[i] += determineServoMiddleOrForwardFromChannel(i);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
void mixTable(void)
|
void mixTable(void)
|
||||||
{
|
{
|
||||||
|
@ -515,6 +515,7 @@ void mixTable(void)
|
||||||
for (i = 0; i < motorCount; i++)
|
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
|
// airplane / servo mixes
|
||||||
switch (currentMixerConfiguration) {
|
switch (currentMixerConfiguration) {
|
||||||
case MULTITYPE_BI:
|
case MULTITYPE_BI:
|
||||||
|
@ -572,6 +573,7 @@ void mixTable(void)
|
||||||
default:
|
default:
|
||||||
break;
|
break;
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
// do camstab
|
// do camstab
|
||||||
if (feature(FEATURE_SERVO_TILT)) {
|
if (feature(FEATURE_SERVO_TILT)) {
|
||||||
|
|
|
@ -20,7 +20,6 @@
|
||||||
#define MAX_SUPPORTED_MOTORS 12
|
#define MAX_SUPPORTED_MOTORS 12
|
||||||
#define MAX_SUPPORTED_SERVOS 8
|
#define MAX_SUPPORTED_SERVOS 8
|
||||||
|
|
||||||
// Syncronized with GUI. Only exception is mixer > 11, which is always returned as 11 during serialization.
|
|
||||||
typedef enum MultiType
|
typedef enum MultiType
|
||||||
{
|
{
|
||||||
MULTITYPE_TRI = 1,
|
MULTITYPE_TRI = 1,
|
||||||
|
@ -33,9 +32,9 @@ typedef enum MultiType
|
||||||
MULTITYPE_FLYING_WING = 8,
|
MULTITYPE_FLYING_WING = 8,
|
||||||
MULTITYPE_Y4 = 9,
|
MULTITYPE_Y4 = 9,
|
||||||
MULTITYPE_HEX6X = 10,
|
MULTITYPE_HEX6X = 10,
|
||||||
MULTITYPE_OCTOX8 = 11, // Java GUI is same for the next 3 configs
|
MULTITYPE_OCTOX8 = 11,
|
||||||
MULTITYPE_OCTOFLATP = 12, // MultiWinGui shows this differently
|
MULTITYPE_OCTOFLATP = 12,
|
||||||
MULTITYPE_OCTOFLATX = 13, // MultiWinGui shows this differently
|
MULTITYPE_OCTOFLATX = 13,
|
||||||
MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
|
MULTITYPE_AIRPLANE = 14, // airplane / singlecopter / dualcopter (not yet properly supported)
|
||||||
MULTITYPE_HELI_120_CCPM = 15,
|
MULTITYPE_HELI_120_CCPM = 15,
|
||||||
MULTITYPE_HELI_90_DEG = 16,
|
MULTITYPE_HELI_90_DEG = 16,
|
||||||
|
@ -47,7 +46,7 @@ typedef enum MultiType
|
||||||
MULTITYPE_ATAIL4 = 22,
|
MULTITYPE_ATAIL4 = 22,
|
||||||
MULTITYPE_CUSTOM = 23,
|
MULTITYPE_CUSTOM = 23,
|
||||||
MULTITYPE_LAST = 24
|
MULTITYPE_LAST = 24
|
||||||
} MultiType;
|
} multiType_e;
|
||||||
|
|
||||||
// Custom mixer data per motor
|
// Custom mixer data per motor
|
||||||
typedef struct motorMixer_t {
|
typedef struct motorMixer_t {
|
||||||
|
|
|
@ -74,8 +74,10 @@
|
||||||
|
|
||||||
#include "serial_cli.h"
|
#include "serial_cli.h"
|
||||||
|
|
||||||
// we unset this on 'exit'
|
extern uint16_t cycleTime; // FIXME dependency on mw.c
|
||||||
extern uint8_t cliMode;
|
|
||||||
|
static serialPort_t *cliPort;
|
||||||
|
|
||||||
static void cliAux(char *cmdline);
|
static void cliAux(char *cmdline);
|
||||||
static void cliAdjustmentRange(char *cmdline);
|
static void cliAdjustmentRange(char *cmdline);
|
||||||
static void cliCMix(char *cmdline);
|
static void cliCMix(char *cmdline);
|
||||||
|
@ -83,18 +85,6 @@ static void cliDefaults(char *cmdline);
|
||||||
static void cliDump(char *cmdLine);
|
static void cliDump(char *cmdLine);
|
||||||
static void cliExit(char *cmdline);
|
static void cliExit(char *cmdline);
|
||||||
static void cliFeature(char *cmdline);
|
static void cliFeature(char *cmdline);
|
||||||
#ifdef GPS
|
|
||||||
static void cliGpsPassthrough(char *cmdline);
|
|
||||||
#endif
|
|
||||||
static void cliHelp(char *cmdline);
|
|
||||||
static void cliMap(char *cmdline);
|
|
||||||
#ifdef LED_STRIP
|
|
||||||
static void cliLed(char *cmdline);
|
|
||||||
static void cliColor(char *cmdline);
|
|
||||||
#endif
|
|
||||||
#ifndef CJMCU
|
|
||||||
static void cliMixer(char *cmdline);
|
|
||||||
#endif
|
|
||||||
static void cliMotor(char *cmdline);
|
static void cliMotor(char *cmdline);
|
||||||
static void cliProfile(char *cmdline);
|
static void cliProfile(char *cmdline);
|
||||||
static void cliRateProfile(char *cmdline);
|
static void cliRateProfile(char *cmdline);
|
||||||
|
@ -105,9 +95,21 @@ static void cliGet(char *cmdline);
|
||||||
static void cliStatus(char *cmdline);
|
static void cliStatus(char *cmdline);
|
||||||
static void cliVersion(char *cmdline);
|
static void cliVersion(char *cmdline);
|
||||||
|
|
||||||
extern uint16_t cycleTime; // FIXME dependency on mw.c
|
#ifdef GPS
|
||||||
|
static void cliGpsPassthrough(char *cmdline);
|
||||||
|
#endif
|
||||||
|
|
||||||
static serialPort_t *cliPort;
|
static void cliHelp(char *cmdline);
|
||||||
|
static void cliMap(char *cmdline);
|
||||||
|
|
||||||
|
#ifdef LED_STRIP
|
||||||
|
static void cliLed(char *cmdline);
|
||||||
|
static void cliColor(char *cmdline);
|
||||||
|
#endif
|
||||||
|
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
|
static void cliMixer(char *cmdline);
|
||||||
|
#endif
|
||||||
|
|
||||||
// signal that we're in cli mode
|
// signal that we're in cli mode
|
||||||
uint8_t cliMode = 0;
|
uint8_t cliMode = 0;
|
||||||
|
@ -116,7 +118,8 @@ uint8_t cliMode = 0;
|
||||||
static char cliBuffer[48];
|
static char cliBuffer[48];
|
||||||
static uint32_t bufferIndex = 0;
|
static uint32_t bufferIndex = 0;
|
||||||
|
|
||||||
// sync this with mutiType_e
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
|
// sync this with multiType_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",
|
||||||
|
@ -125,6 +128,7 @@ static const char * const mixerNames[] = {
|
||||||
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
|
"HEX6H", "PPM_TO_SERVO", "DUALCOPTER", "SINGLECOPTER",
|
||||||
"ATAIL4", "CUSTOM", NULL
|
"ATAIL4", "CUSTOM", NULL
|
||||||
};
|
};
|
||||||
|
#endif
|
||||||
|
|
||||||
// sync this with features_e
|
// sync this with features_e
|
||||||
static const char * const featureNames[] = {
|
static const char * const featureNames[] = {
|
||||||
|
@ -170,7 +174,7 @@ const clicmd_t cmdTable[] = {
|
||||||
{ "led", "configure leds", cliLed },
|
{ "led", "configure leds", cliLed },
|
||||||
#endif
|
#endif
|
||||||
{ "map", "mapping of rc channel order", cliMap },
|
{ "map", "mapping of rc channel order", cliMap },
|
||||||
#ifndef CJMCU
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
{ "mixer", "mixer name or list", cliMixer },
|
{ "mixer", "mixer name or list", cliMixer },
|
||||||
#endif
|
#endif
|
||||||
{ "motor", "get/set motor output value", cliMotor },
|
{ "motor", "get/set motor output value", cliMotor },
|
||||||
|
@ -573,7 +577,7 @@ static void cliAdjustmentRange(char *cmdline)
|
||||||
|
|
||||||
static void cliCMix(char *cmdline)
|
static void cliCMix(char *cmdline)
|
||||||
{
|
{
|
||||||
#ifdef CJMCU
|
#ifdef USE_QUAD_MIXER_ONLY
|
||||||
UNUSED(cmdline);
|
UNUSED(cmdline);
|
||||||
#else
|
#else
|
||||||
int i, check = 0;
|
int i, check = 0;
|
||||||
|
@ -753,9 +757,12 @@ static void cliDump(char *cmdline)
|
||||||
{
|
{
|
||||||
unsigned int i;
|
unsigned int i;
|
||||||
char buf[16];
|
char buf[16];
|
||||||
float thr, roll, pitch, yaw;
|
|
||||||
uint32_t mask;
|
uint32_t mask;
|
||||||
|
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
|
float thr, roll, pitch, yaw;
|
||||||
|
#endif
|
||||||
|
|
||||||
uint8_t dumpMask = DUMP_ALL;
|
uint8_t dumpMask = DUMP_ALL;
|
||||||
if (strcasecmp(cmdline, "master") == 0) {
|
if (strcasecmp(cmdline, "master") == 0) {
|
||||||
dumpMask = DUMP_MASTER; // only
|
dumpMask = DUMP_MASTER; // only
|
||||||
|
@ -775,6 +782,7 @@ static void cliDump(char *cmdline)
|
||||||
printf("\r\n# dump master\r\n");
|
printf("\r\n# dump master\r\n");
|
||||||
printf("\r\n# mixer\r\n");
|
printf("\r\n# mixer\r\n");
|
||||||
|
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
|
printf("mixer %s\r\n", mixerNames[masterConfig.mixerConfiguration - 1]);
|
||||||
|
|
||||||
if (masterConfig.customMixer[0].throttle != 0.0f) {
|
if (masterConfig.customMixer[0].throttle != 0.0f) {
|
||||||
|
@ -801,6 +809,7 @@ static void cliDump(char *cmdline)
|
||||||
}
|
}
|
||||||
printf("cmix %d 0 0 0 0\r\n", i + 1);
|
printf("cmix %d 0 0 0 0\r\n", i + 1);
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
printf("\r\n\r\n# feature\r\n");
|
printf("\r\n\r\n# feature\r\n");
|
||||||
|
|
||||||
|
@ -1014,7 +1023,7 @@ static void cliMap(char *cmdline)
|
||||||
printf("%s\r\n", out);
|
printf("%s\r\n", out);
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifndef CJMCU
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
static void cliMixer(char *cmdline)
|
static void cliMixer(char *cmdline)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
|
|
@ -1340,9 +1340,11 @@ static bool processInCommand(void)
|
||||||
masterConfig.batteryConfig.currentMeterOffset = read16();
|
masterConfig.batteryConfig.currentMeterOffset = read16();
|
||||||
break;
|
break;
|
||||||
|
|
||||||
|
#ifndef USE_QUAD_MIXER_ONLY
|
||||||
case MSP_SET_MIXER:
|
case MSP_SET_MIXER:
|
||||||
masterConfig.mixerConfiguration = read8();
|
masterConfig.mixerConfiguration = read8();
|
||||||
break;
|
break;
|
||||||
|
#endif
|
||||||
|
|
||||||
case MSP_SET_RX_CONFIG:
|
case MSP_SET_RX_CONFIG:
|
||||||
masterConfig.rxConfig.serialrx_provider = read8();
|
masterConfig.rxConfig.serialrx_provider = read8();
|
||||||
|
@ -1364,8 +1366,8 @@ static bool processInCommand(void)
|
||||||
|
|
||||||
case MSP_SET_CONFIG:
|
case MSP_SET_CONFIG:
|
||||||
|
|
||||||
#ifdef CJMCU
|
#ifdef USE_QUAD_MIXER_ONLY
|
||||||
read8(); // multitype
|
read8(); // multitype ignored
|
||||||
#else
|
#else
|
||||||
masterConfig.mixerConfiguration = read8(); // multitype
|
masterConfig.mixerConfiguration = read8(); // multitype
|
||||||
#endif
|
#endif
|
||||||
|
|
|
@ -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 mixerConfiguration, motorMixer_t *customMixers);
|
void mixerInit(multiType_e mixerConfiguration, 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);
|
||||||
|
|
|
@ -67,3 +67,6 @@
|
||||||
// USART2, PA3
|
// USART2, PA3
|
||||||
#define BIND_PORT GPIOA
|
#define BIND_PORT GPIOA
|
||||||
#define BIND_PIN Pin_3
|
#define BIND_PIN Pin_3
|
||||||
|
|
||||||
|
// Since the CJMCU PCB has holes for 4 motors in each corner we can save same flash space by disabling support for other mixers.
|
||||||
|
#define USE_QUAD_MIXER_ONLY
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue