Disable mixer configuration on CJMCU to save flash size.
This commit is contained in:
parent
d605ded161
commit
13305dd2e4
|
@ -21,6 +21,8 @@
|
||||||
|
|
||||||
#include "platform.h"
|
#include "platform.h"
|
||||||
|
|
||||||
|
#include "build_config.h"
|
||||||
|
|
||||||
#include "common/axis.h"
|
#include "common/axis.h"
|
||||||
#include "common/maths.h"
|
#include "common/maths.h"
|
||||||
|
|
||||||
|
@ -62,6 +64,14 @@ static gimbalConfig_t *gimbalConfig;
|
||||||
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
|
||||||
static MultiType currentMixerConfiguration;
|
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[] = {
|
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
|
||||||
|
@ -75,13 +85,6 @@ static const motorMixer_t mixerQuadP[] = {
|
||||||
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
|
{ 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[] = {
|
static const motorMixer_t mixerBi[] = {
|
||||||
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
|
{ 1.0f, 1.0f, 0.0f, 0.0f }, // LEFT
|
||||||
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
|
{ 1.0f, -1.0f, 0.0f, 0.0f }, // RIGHT
|
||||||
|
@ -210,6 +213,7 @@ const mixer_t mixers[] = {
|
||||||
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
|
{ 4, 0, mixerAtail4 }, // MULTITYPE_ATAIL4
|
||||||
{ 0, 0, NULL }, // MULTITYPE_CUSTOM
|
{ 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)
|
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;
|
static motorMixer_t *customMixers;
|
||||||
|
|
||||||
|
|
||||||
|
#ifndef CJMCU
|
||||||
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
void mixerInit(MultiType mixerConfiguration, motorMixer_t *initialCustomMixers)
|
||||||
{
|
{
|
||||||
currentMixerConfiguration = mixerConfiguration;
|
currentMixerConfiguration = mixerConfiguration;
|
||||||
|
@ -310,6 +316,32 @@ void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfigura
|
||||||
|
|
||||||
mixerResetMotors();
|
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)
|
void mixerResetMotors(void)
|
||||||
{
|
{
|
||||||
|
@ -319,6 +351,7 @@ 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)
|
void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
@ -335,6 +368,7 @@ void mixerLoadMix(int index, motorMixer_t *customMixers)
|
||||||
customMixers[i] = mixers[index].motor[i];
|
customMixers[i] = mixers[index].motor[i];
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
static void updateGimbalServos(void)
|
static void updateGimbalServos(void)
|
||||||
{
|
{
|
||||||
|
|
|
@ -87,7 +87,9 @@ static void cliMap(char *cmdline);
|
||||||
static void cliLed(char *cmdline);
|
static void cliLed(char *cmdline);
|
||||||
static void cliColor(char *cmdline);
|
static void cliColor(char *cmdline);
|
||||||
#endif
|
#endif
|
||||||
|
#ifndef CJMCU
|
||||||
static void cliMixer(char *cmdline);
|
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);
|
||||||
|
@ -163,7 +165,9 @@ 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
|
||||||
{ "mixer", "mixer name or list", cliMixer },
|
{ "mixer", "mixer name or list", cliMixer },
|
||||||
|
#endif
|
||||||
{ "motor", "get/set motor output value", cliMotor },
|
{ "motor", "get/set motor output value", cliMotor },
|
||||||
{ "profile", "index (0 to 2)", cliProfile },
|
{ "profile", "index (0 to 2)", cliProfile },
|
||||||
{ "rateprofile", "index (0 to 2)", cliRateProfile },
|
{ "rateprofile", "index (0 to 2)", cliRateProfile },
|
||||||
|
@ -563,6 +567,9 @@ static void cliAdjustmentRange(char *cmdline)
|
||||||
|
|
||||||
static void cliCMix(char *cmdline)
|
static void cliCMix(char *cmdline)
|
||||||
{
|
{
|
||||||
|
#ifdef CJMCU
|
||||||
|
UNUSED(cmdline);
|
||||||
|
#else
|
||||||
int i, check = 0;
|
int i, check = 0;
|
||||||
int num_motors = 0;
|
int num_motors = 0;
|
||||||
uint8_t len;
|
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);
|
printf("Motor number must be between 1 and %d\r\n", MAX_SUPPORTED_MOTORS);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
}
|
}
|
||||||
|
|
||||||
#ifdef LED_STRIP
|
#ifdef LED_STRIP
|
||||||
|
@ -1000,6 +1008,7 @@ static void cliMap(char *cmdline)
|
||||||
printf("%s\r\n", out);
|
printf("%s\r\n", out);
|
||||||
}
|
}
|
||||||
|
|
||||||
|
#ifndef CJMCU
|
||||||
static void cliMixer(char *cmdline)
|
static void cliMixer(char *cmdline)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
@ -1033,6 +1042,7 @@ static void cliMixer(char *cmdline)
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
#endif
|
||||||
|
|
||||||
static void cliMotor(char *cmdline)
|
static void cliMotor(char *cmdline)
|
||||||
{
|
{
|
||||||
|
|
|
@ -910,9 +910,15 @@ static bool processOutCommand(uint8_t cmdMSP)
|
||||||
|
|
||||||
serialize16(currentProfile->failsafeConfig.failsafe_throttle);
|
serialize16(currentProfile->failsafeConfig.failsafe_throttle);
|
||||||
|
|
||||||
|
#ifdef GPS
|
||||||
serialize8(masterConfig.gpsConfig.provider); // gps_type
|
serialize8(masterConfig.gpsConfig.provider); // gps_type
|
||||||
serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
|
serialize8(0); // TODO gps_baudrate (an index, cleanflight uses a uint32_t
|
||||||
serialize8(masterConfig.gpsConfig.sbasMode); // gps_ubx_sbas
|
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.batteryConfig.multiwiiCurrentMeterOutput);
|
||||||
serialize8(masterConfig.rxConfig.rssi_channel);
|
serialize8(masterConfig.rxConfig.rssi_channel);
|
||||||
serialize8(0);
|
serialize8(0);
|
||||||
|
@ -1209,9 +1215,15 @@ static bool processInCommand(void)
|
||||||
|
|
||||||
currentProfile->failsafeConfig.failsafe_throttle = read16();
|
currentProfile->failsafeConfig.failsafe_throttle = read16();
|
||||||
|
|
||||||
|
#ifdef GPS
|
||||||
masterConfig.gpsConfig.provider = read8(); // gps_type
|
masterConfig.gpsConfig.provider = read8(); // gps_type
|
||||||
read8(); // gps_baudrate
|
read8(); // gps_baudrate
|
||||||
masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
|
masterConfig.gpsConfig.sbasMode = read8(); // gps_ubx_sbas
|
||||||
|
#else
|
||||||
|
read8(); // gps_type
|
||||||
|
read8(); // gps_baudrate
|
||||||
|
read8(); // gps_ubx_sbas
|
||||||
|
#endif
|
||||||
masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
|
masterConfig.batteryConfig.multiwiiCurrentMeterOutput = read8();
|
||||||
masterConfig.rxConfig.rssi_channel = read8();
|
masterConfig.rxConfig.rssi_channel = read8();
|
||||||
read8();
|
read8();
|
||||||
|
@ -1348,7 +1360,11 @@ static bool processInCommand(void)
|
||||||
|
|
||||||
case MSP_SET_CONFIG:
|
case MSP_SET_CONFIG:
|
||||||
|
|
||||||
|
#ifdef CJMCU
|
||||||
masterConfig.mixerConfiguration = read8(); // multitype
|
masterConfig.mixerConfiguration = read8(); // multitype
|
||||||
|
#else
|
||||||
|
read8(); // multitype
|
||||||
|
#endif
|
||||||
|
|
||||||
featureClearAll();
|
featureClearAll();
|
||||||
featureSet(read32()); // features bitmap
|
featureSet(read32()); // features bitmap
|
||||||
|
|
Loading…
Reference in New Issue