Skip to content

Commit

Permalink
Disable mixer configuration on CJMCU to save flash size.
Browse files Browse the repository at this point in the history
  • Loading branch information
hydra committed Dec 21, 2014
1 parent d605ded commit 13305dd
Show file tree
Hide file tree
Showing 3 changed files with 67 additions and 7 deletions.
48 changes: 41 additions & 7 deletions src/main/flight/mixer.c
Original file line number Diff line number Diff line change
Expand Up @@ -21,6 +21,8 @@

#include "platform.h"

#include "build_config.h"

#include "common/axis.h"
#include "common/maths.h"

Expand Down Expand Up @@ -62,6 +64,14 @@ static gimbalConfig_t *gimbalConfig;
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static MultiType currentMixerConfiguration;

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 CJMCU
static const motorMixer_t mixerTri[] = {
{ 1.0f, 0.0f, 1.333333f, 0.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, 0.0f }, // RIGHT
Expand All @@ -75,13 +85,6 @@ static const motorMixer_t mixerQuadP[] = {
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
};

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
};

static const motorMixer_t mixerBi[] = {
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
Expand Down Expand Up @@ -210,6 +213,7 @@ const mixer_t mixers[] = {
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
};
#endif

void mixerUseConfigs(servoParam_t *servoConfToUse, flight3DConfig_t *flight3DConfigToUse, escAndServoConfig_t *escAndServoConfigToUse, mixerConfig_t *mixerConfigToUse, airplaneConfig_t *airplaneConfigToUse, rxConfig_t *rxConfigToUse, gimbalConfig_t *gimbalConfigToUse)
{
Expand Down Expand Up @@ -253,6 +257,8 @@ int servoDirection(int nr, int lr)

static motorMixer_t *customMixers;


#ifndef CJMCU
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
{
currentMixerConfiguration = mixerConfiguration;
Expand Down Expand Up @@ -310,6 +316,32 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura

mixerResetMotors();
}
#else

void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
{
currentMixerConfiguration = mixerConfiguration;

customMixers = initialCustomMixers;

useServo = false;
}

void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration)
{
UNUSED(pwmOutputConfiguration);
motorCount = 4;
servoCount = 0;

uint8_t i;
for (i = 0; i < motorCount; i++) {
currentMixer[i] = mixerQuadX[i];
}

mixerResetMotors();
}
#endif


void mixerResetMotors(void)
{
Expand All @@ -319,6 +351,7 @@ void mixerResetMotors(void)
motor_disarmed[i] = feature(FEATURE_3D) ? flight3DConfig->neutral3d : escAndServoConfig->mincommand;
}

#ifndef CJMCU
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
int i;
Expand All @@ -335,6 +368,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
customMixers[i] = mixers[index].motor[i];
}
}
#endif

static void updateGimbalServos(void)
{
Expand Down
10 changes: 10 additions & 0 deletions src/main/io/serial_cli.c
Original file line number Diff line number Diff line change
Expand Up @@ -87,7 +87,9 @@ static void cliMap(char *cmdline);
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 cliProfile(char *cmdline);
static void cliRateProfile(char *cmdline);
Expand Down Expand Up @@ -163,7 +165,9 @@ const clicmd_t cmdTable[] = {
{ "led", "configure leds", cliLed },
#endif
{ "map", "mapping of rc channel order", cliMap },
#ifndef CJMCU
{ "mixer", "mixer name or list", cliMixer },
#endif
{ "motor", "get/set motor output value", cliMotor },
{ "profile", "index (0 to 2)", cliProfile },
{ "rateprofile", "index (0 to 2)", cliRateProfile },
Expand Down Expand Up @@ -563,6 +567,9 @@ static void cliAdjustmentRange(char *cmdline)

static void cliCMix(char *cmdline)
{
#ifdef CJMCU
UNUSED(cmdline);
#else
int i, check = 0;
int num_motors = 0;
uint8_t len;
Expand Down Expand Up @@ -649,6 +656,7 @@ static void cliCMix(char *cmdline)
printf("Motor number must be between 1 and %d\r\n", MAX_SUPPORTED_MOTORS);
}
}
#endif
}

#ifdef LED_STRIP
Expand Down Expand Up @@ -1000,6 +1008,7 @@ static void cliMap(char *cmdline)
printf("%s\r\n", out);
}

#ifndef CJMCU
static void cliMixer(char *cmdline)
{
int i;
Expand Down Expand Up @@ -1033,6 +1042,7 @@ static void cliMixer(char *cmdline)
}
}
}
#endif

static void cliMotor(char *cmdline)
{
Expand Down
16 changes: 16 additions & 0 deletions src/main/io/serial_msp.c
Original file line number Diff line number Diff line change
Expand Up @@ -910,9 +910,15 @@ static bool processOutCommand(uint8_t cmdMSP)

serialize16(currentProfile->failsafeConfig.failsafe_throttle);

#ifdef GPS
serialize8(masterConfig.gpsConfig.provider); // gps_type
serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
#else
serialize8(0); // gps_type
serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
serialize8(0); // gps_ubx_sbas
#endif
serialize8(masterConfig.batteryConfig.multiwiiCurrentMeterOutput);
serialize8(masterConfig.rxConfig.rssi_channel);
serialize8(0);
Expand Down Expand Up @@ -1209,9 +1215,15 @@ static bool processInCommand(void)

currentProfile->failsafeConfig.failsafe_throttle = read16();

#ifdef GPS
masterConfig.gpsConfig.provider = read8(); // gps_type
read8(); // gps_baudrate
masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
#else
read8(); // gps_type
read8(); // gps_baudrate
read8(); // gps_ubx_sbas
#endif
masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
masterConfig.rxConfig.rssi_channel = read8();
read8();
Expand Down Expand Up @@ -1348,7 +1360,11 @@ static bool processInCommand(void)

case MSP_SET_CONFIG:

#ifdef CJMCU
masterConfig.mixerConfiguration = read8(); // multitype
#else
read8(); // multitype
#endif

featureClearAll();
featureSet(read32()); // features bitmap
Expand Down

0 comments on commit 13305dd

Please sign in to comment.