From 13305dd2e4128e60437fa263e98cb01d91e21fe8 Mon Sep 17 00:00:00 2001 From: Dominic Clifton Date: Sun, 21 Dec 2014 23:39:58 +0000 Subject: [PATCH] Disable mixer configuration on CJMCU to save flash size. --- src/main/flight/mixer.c | 48 ++++++++++++++++++++++++++++++++++------ src/main/io/serial_cli.c | 10 +++++++++ src/main/io/serial_msp.c | 16 ++++++++++++++ 3 files changed, 67 insertions(+), 7 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index adc37eb49..0036706f1 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -21,6 +21,8 @@ #include "platform.h" +#include "build_config.h" + #include "common/axis.h" #include "common/maths.h" @@ -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 @@ -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 @@ -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) { @@ -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; @@ -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) { @@ -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; @@ -335,6 +368,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers) customMixers[i] = mixers[index].motor[i]; } } +#endif static void updateGimbalServos(void) { diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index c96e93c4a..55fd5e8e9 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -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); @@ -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 }, @@ -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; @@ -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 @@ -1000,6 +1008,7 @@ static void cliMap(char *cmdline) printf("%s\r\n", out); } +#ifndef CJMCU static void cliMixer(char *cmdline) { int i; @@ -1033,6 +1042,7 @@ static void cliMixer(char *cmdline) } } } +#endif static void cliMotor(char *cmdline) { diff --git a/src/main/io/serial_msp.c b/src/main/io/serial_msp.c index 4a513a5bf..79d24e058 100644 --- a/src/main/io/serial_msp.c +++ b/src/main/io/serial_msp.c @@ -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); @@ -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(); @@ -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