Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 01:05:21 +03:00
Code Issues Wiki Activity

and predefined motor and servo mixers are gone

Browse source
This commit is contained in:
Pawel Spychalski (DzikuVx) 2018-03-27 21:19:46 +02:00
parent 900a60b405
commit bea8bd2abb
6 changed files with 14 additions and 365 deletions
Download patch file Download diff file Expand all files Collapse all files

35
src/main/fc/cli.c
View file

@ -1003,7 +1003,6 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer
static void cliMotorMix(char *cmdline)
{
int check = 0;
uint8_t len;
const char *ptr;
if (isEmpty(cmdline)) {
@ -1013,23 +1012,6 @@ static void cliMotorMix(char *cmdline)
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMotorMixerMutable(i)->throttle = 0.0f;
}
} else if (sl_strncasecmp(cmdline, "load", 4) == 0) {
ptr = nextArg(cmdline);
if (ptr) {
len = strlen(ptr);
for (uint32_t i = 0; ; i++) {
if (mixerNames[i] == NULL) {
cliPrintLine("Invalid name");
break;
}
if (sl_strncasecmp(ptr, mixerNames[i], len) == 0) {
mixerLoadMix(i, customMotorMixerMutable(0));
cliPrintLinef("Loaded %s", mixerNames[i]);
cliMotorMix("");
break;
}
}
}
} else {
ptr = cmdline;
uint32_t i = fastA2I(ptr); // get motor number
@ -1441,23 +1423,6 @@ static void cliServoMix(char *cmdline)
for (uint32_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servoParamsMutable(i)->reversedSources = 0;
}
} else if (sl_strncasecmp(cmdline, "load", 4) == 0) {
const char *ptr = nextArg(cmdline);
if (ptr) {
len = strlen(ptr);
for (uint32_t i = 0; ; i++) {
if (mixerNames[i] == NULL) {
cliPrintLine("Invalid name");
break;
}
if (sl_strncasecmp(ptr, mixerNames[i], len) == 0) {
servoMixerLoadMix(i);
cliPrintLinef("Loaded %s", mixerNames[i]);
cliServoMix("");
break;
}
}
}
} else if (sl_strncasecmp(cmdline, "reverse", 7) == 0) {
enum {SERVO = 0, INPUT, REVERSE, ARGS_COUNT};
char *ptr = strchr(cmdline, ' ');

204
src/main/flight/mixer.c
View file

@ -102,190 +102,6 @@ static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
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
};
#define DEF_MIXER(_mixerMode, _motorCount, _useServos, _motorMap) \
{ .mixerMode=_mixerMode, .motorCount=_motorCount, .useServos=_useServos, .motor=_motorMap }
static const motorMixer_t mixerTricopter[] = {
{ 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 }, // LEFT
};
static const motorMixer_t mixerQuadP[] = {
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
{ 1.0f, -1.0f, 0.0f, 1.0f }, // RIGHT
{ 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
};
#ifndef DISABLE_UNCOMMON_MIXERS
static const motorMixer_t mixerVtail4[] = {
{ 1.0f, -0.58f, 0.58f, 1.0f }, // REAR_R
{ 1.0f, -0.46f, -0.39f, -0.5f }, // FRONT_R
{ 1.0f, 0.58f, 0.58f, -1.0f }, // REAR_L
{ 1.0f, 0.46f, -0.39f, 0.5f }, // FRONT_L
};
static const motorMixer_t mixerAtail4[] = {
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_R
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_L
{ 1.0f, 1.0f, -1.0f, -0.0f }, // FRONT_L
};
static const motorMixer_t mixerY4[] = {
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR_TOP CW
{ 1.0f, -1.0f, -1.0f, 0.0f }, // FRONT_R CCW
{ 1.0f, 0.0f, 1.0f, 1.0f }, // REAR_BOTTOM CCW
{ 1.0f, 1.0f, -1.0f, 0.0f }, // FRONT_L CW
};
#if (MAX_SUPPORTED_MOTORS >= 6)
static const motorMixer_t mixerHex6H[] = {
{ 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
{ 1.0f, 0.0f, 0.0f, 0.0f }, // RIGHT
{ 1.0f, 0.0f, 0.0f, 0.0f }, // LEFT
};
static const motorMixer_t mixerY6[] = {
{ 1.0f, 0.0f, 1.333333f, 1.0f }, // REAR
{ 1.0f, -1.0f, -0.666667f, -1.0f }, // RIGHT
{ 1.0f, 1.0f, -0.666667f, -1.0f }, // LEFT
{ 1.0f, 0.0f, 1.333333f, -1.0f }, // UNDER_REAR
{ 1.0f, -1.0f, -0.666667f, 1.0f }, // UNDER_RIGHT
{ 1.0f, 1.0f, -0.666667f, 1.0f }, // UNDER_LEFT
};
static const motorMixer_t mixerHex6P[] = {
{ 1.0f, -0.866025f, 0.5f, 1.0f }, // REAR_R
{ 1.0f, -0.866025f, -0.5f, -1.0f }, // FRONT_R
{ 1.0f, 0.866025f, 0.5f, 1.0f }, // REAR_L
{ 1.0f, 0.866025f, -0.5f, -1.0f }, // FRONT_L
{ 1.0f, 0.0f, -1.0f, 1.0f }, // FRONT
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
};
#endif
#if (MAX_SUPPORTED_MOTORS >= 8)
static const motorMixer_t mixerOctoFlatP[] = {
{ 1.0f, 0.707107f, -0.707107f, 1.0f }, // FRONT_L
{ 1.0f, -0.707107f, -0.707107f, 1.0f }, // FRONT_R
{ 1.0f, -0.707107f, 0.707107f, 1.0f }, // REAR_R
{ 1.0f, 0.707107f, 0.707107f, 1.0f }, // REAR_L
{ 1.0f, 0.0f, -1.0f, -1.0f }, // FRONT
{ 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
{ 1.0f, 0.0f, 1.0f, -1.0f }, // REAR
{ 1.0f, 1.0f, 0.0f, -1.0f }, // LEFT
};
static const motorMixer_t mixerOctoFlatX[] = {
{ 1.0f, 1.0f, -0.414178f, 1.0f }, // MIDFRONT_L
{ 1.0f, -0.414178f, -1.0f, 1.0f }, // FRONT_R
{ 1.0f, -1.0f, 0.414178f, 1.0f }, // MIDREAR_R
{ 1.0f, 0.414178f, 1.0f, 1.0f }, // REAR_L
{ 1.0f, 0.414178f, -1.0f, -1.0f }, // FRONT_L
{ 1.0f, -1.0f, -0.414178f, -1.0f }, // MIDFRONT_R
{ 1.0f, -0.414178f, 1.0f, -1.0f }, // REAR_R
{ 1.0f, 1.0f, 0.414178f, -1.0f }, // MIDREAR_L
};
static const motorMixer_t mixerOctoX8[] = {
{ 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
{ 1.0f, -1.0f, 1.0f, 1.0f }, // UNDER_REAR_R
{ 1.0f, -1.0f, -1.0f, -1.0f }, // UNDER_FRONT_R
{ 1.0f, 1.0f, 1.0f, -1.0f }, // UNDER_REAR_L
{ 1.0f, 1.0f, -1.0f, 1.0f }, // UNDER_FRONT_L
};
#endif
#endif //DISABLE_UNCOMMON_MIXERS
#if (MAX_SUPPORTED_MOTORS >= 6)
static const motorMixer_t mixerHex6X[] = {
{ 1.0f, -0.5f, 0.866025f, 1.0f }, // REAR_R
{ 1.0f, -0.5f, -0.866025f, 1.0f }, // FRONT_R
{ 1.0f, 0.5f, 0.866025f, -1.0f }, // REAR_L
{ 1.0f, 0.5f, -0.866025f, -1.0f }, // FRONT_L
{ 1.0f, -1.0f, 0.0f, -1.0f }, // RIGHT
{ 1.0f, 1.0f, 0.0f, 1.0f }, // LEFT
};
#endif
static const motorMixer_t mixerDualProp[] = {
{ 1.0f, 0.0f, 0.0f, 0.0f },
{ 1.0f, 0.0f, 0.0f, 0.0f },
};
static const mixer_t mixerTable[] = {
// motors, use servo, motor mixer
// mixerMode motorCount useServos motorMap
DEF_MIXER( MIXER_TRI, 3, true, mixerTricopter ),
DEF_MIXER( MIXER_CUSTOM_TRI, 3, false, NULL ),
DEF_MIXER( MIXER_QUADP, 4, false, mixerQuadP ),
DEF_MIXER( MIXER_QUADX, 4, false, mixerQuadX ),
#if (MAX_SUPPORTED_MOTORS >= 6)
DEF_MIXER( MIXER_HEX6X, 6, false, mixerHex6X ),
#endif
DEF_MIXER( MIXER_CUSTOM, 0, false, NULL ),
//DEF_MIXER( MIXER_BICOPTER, 0, false, NULL ),
//DEF_MIXER( MIXER_GIMBAL, 0, false, NULL ),
DEF_MIXER( MIXER_FLYING_WING, 2, true, mixerDualProp ),
DEF_MIXER( MIXER_AIRPLANE, 2, true, mixerDualProp ),
DEF_MIXER( MIXER_CUSTOM_AIRPLANE, 2, true, NULL ),
//DEF_MIXER( MIXER_HELI_120_CCPM, 1, true, NULL ),
//DEF_MIXER( MIXER_HELI_90_DEG, 1, true, NULL ),
//DEF_MIXER( MIXER_PPM_TO_SERVO, 0, true, NULL ),
//DEF_MIXER( MIXER_DUALCOPTER, 0, false, NULL ),
//DEF_MIXER( MIXER_SINGLECOPTER, 0, false, NULL ),
#if !defined(DISABLE_UNCOMMON_MIXERS)
DEF_MIXER( MIXER_Y4, 4, false, mixerY4 ),
DEF_MIXER( MIXER_ATAIL4, 4, false, mixerAtail4 ),
DEF_MIXER( MIXER_VTAIL4, 4, false, mixerVtail4 ),
#if (MAX_SUPPORTED_MOTORS >= 6)
DEF_MIXER( MIXER_Y6, 6, false, mixerY6 ),
DEF_MIXER( MIXER_HEX6, 6, false, mixerHex6P ),
DEF_MIXER( MIXER_HEX6H, 6, false, mixerHex6H ),
#endif
#if (MAX_SUPPORTED_MOTORS >= 8)
DEF_MIXER( MIXER_OCTOX8, 8, false, mixerOctoX8 ),
DEF_MIXER( MIXER_OCTOFLATP, 8, false, mixerOctoFlatP ),
DEF_MIXER( MIXER_OCTOFLATX, 8, false, mixerOctoFlatX ),
#endif
#endif
};
const mixer_t * findMixer(mixerMode_e mixerMode)
{
for (unsigned ii = 0; ii < sizeof(mixerTable)/sizeof(mixerTable[0]); ii++) {
if (mixerTable[ii].mixerMode == mixerMode)
return &mixerTable[ii];
}
return NULL;
}
uint8_t getMotorCount(void)
{
return motorCount;
@ -321,8 +137,6 @@ void mixerUsePWMIOConfiguration(void)
{
motorCount = 0;
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
// load custom mixer into currentMixer
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
@ -346,24 +160,6 @@ void mixerUsePWMIOConfiguration(void)
mixerResetDisarmedMotors();
}
void mixerLoadMix(int index, motorMixer_t *customMixers)
{
// we're 1-based
index++;
// clear existing
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMixers[i].throttle = 0.0f;
}
// do we have anything here to begin with?
const mixer_t * mixer = findMixer(index);
if (mixer->motor != NULL) {
for (int i = 0; i < mixer->motorCount; i++) {
customMixers[i] = mixer->motor[i];
}
}
}
void mixerResetDisarmedMotors(void)
{
// set disarmed motor values

9
src/main/flight/mixer.h
View file

@ -76,14 +76,6 @@ typedef struct motorMixer_s {
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer);
// Custom mixer configuration
typedef struct mixer_s {
mixerMode_e mixerMode;
const motorMixer_t *motor;
uint8_t motorCount;
bool useServos;
} mixer_t;
typedef struct mixerConfig_s {
uint8_t mixerMode;
int8_t yaw_motor_direction;
@ -122,7 +114,6 @@ uint8_t getMotorCount(void);
bool mixerIsOutputSaturated(void);
void writeAllMotors(int16_t mc);
void mixerLoadMix(int index, motorMixer_t *customMixers);
void mixerUsePWMIOConfiguration(void);
void mixerUpdateStateFlags(void);
void mixerResetDisarmedMotors(void);

127
src/main/flight/servos.c
View file

@ -54,8 +54,6 @@
#include "sensors/gyro.h"
extern const mixer_t * findMixer(mixerMode_e mixerMode);
PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 0);
PG_RESET_TEMPLATE(servoConfig_t, servoConfig,
@ -102,65 +100,6 @@ static bool servoFilterIsSet;
static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS];
static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES];
#define COUNT_SERVO_RULES(rules) (sizeof(rules) / sizeof(servoMixer_t))
// mixer rule format servo, input, rate, speed
static const servoMixer_t servoMixerAirplane[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 100, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, 100, 0 },
{ SERVO_FLAPPERON_1, INPUT_FEATURE_FLAPS, -100, 0 },
{ SERVO_FLAPPERON_2, INPUT_FEATURE_FLAPS, 100, 0 },
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0 },
{ SERVO_ELEVATOR, INPUT_STABILIZED_PITCH, 100, 0 },
};
static const servoMixer_t servoMixerFlyingWing[] = {
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_ROLL, 50, 0 },
{ SERVO_FLAPPERON_1, INPUT_STABILIZED_PITCH, 50, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_ROLL, -50, 0 },
{ SERVO_FLAPPERON_2, INPUT_STABILIZED_PITCH, 50, 0 },
};
static const servoMixer_t servoMixerTri[] = {
{ SERVO_RUDDER, INPUT_STABILIZED_YAW, 100, 0 },
};
// Custom mixer configuration
typedef struct mixerRules_s {
uint8_t servoRuleCount;
uint8_t minServoIndex;
uint8_t maxServoIndex;
const servoMixer_t *rule;
} mixerRules_t;
const mixerRules_t servoMixers[] = {
{ 0, 0, 0, NULL }, // entry 0
{ COUNT_SERVO_RULES(servoMixerTri), SERVO_RUDDER, SERVO_RUDDER, servoMixerTri }, // MULTITYPE_TRI
{ 0, 0, 0, NULL }, // MULTITYPE_QUADP
{ 0, 0, 0, NULL }, // MULTITYPE_QUADX
{ 0, 0, 0, NULL }, // MULTITYPE_BI
{ 0, 0, 0, NULL }, // MULTITYPE_GIMBAL / MIXER_GIMBAL -> disabled
{ 0, 0, 0, NULL }, // MULTITYPE_Y6
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6
{ COUNT_SERVO_RULES(servoMixerFlyingWing), SERVO_FLAPPERONS_MIN, SERVO_FLAPPERONS_MAX, servoMixerFlyingWing },// * MULTITYPE_FLYING_WING
{ 0, 0, 0, NULL }, // MULTITYPE_Y4
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6X
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOX8
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATP
{ 0, 0, 0, NULL }, // MULTITYPE_OCTOFLATX
{ COUNT_SERVO_RULES(servoMixerAirplane), SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, servoMixerAirplane }, // * MULTITYPE_AIRPLANE
{ 0, 0, 0, NULL }, // * MIXER_HELI_120_CCPM -> disabled, never fully implemented in CF
{ 0, 0, 0, NULL }, // * MIXER_HELI_90_DEG -> disabled, never fully implemented in CF
{ 0, 0, 0, NULL }, // MULTITYPE_VTAIL4
{ 0, 0, 0, NULL }, // MULTITYPE_HEX6H
{ 0, 0, 0, NULL }, // * MULTITYPE_PPM_TO_SERVO
{ 0, 0, 0, NULL }, // MULTITYPE_DUALCOPTER
{ 0, 0, 0, NULL }, // MULTITYPE_SINGLECOPTER
{ 0, 0, 0, NULL }, // MULTITYPE_ATAIL4
{ 0, 2, 5, NULL }, // MULTITYPE_CUSTOM
{ 0, SERVO_PLANE_INDEX_MIN, SERVO_PLANE_INDEX_MAX, NULL }, // MULTITYPE_CUSTOM_PLANE
{ 0, SERVO_RUDDER, SERVO_RUDDER, NULL }, // MULTITYPE_CUSTOM_TRI
};
int16_t getFlaperonDirection(uint8_t servoPin)
{
if (servoPin == SERVO_FLAPPERON_2) {
@ -200,59 +139,35 @@ void servoComputeScalingFactors(uint8_t servoIndex) {
void servosInit(void)
{
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
const mixer_t * motorMixer = findMixer(currentMixerMode);
minServoIndex = servoMixers[currentMixerMode].minServoIndex;
maxServoIndex = servoMixers[currentMixerMode].maxServoIndex;
// enable servos for mixes that require them. note, this shifts motor counts.
mixerUsesServos = motorMixer->useServos || feature(FEATURE_SERVO_TILT);
// if we want camstab/trig, that also enables servos, even if mixer doesn't
servoOutputEnabled = motorMixer->useServos || feature(FEATURE_SERVO_TILT) || feature(FEATURE_CHANNEL_FORWARDING);
// give all servos a default command
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
servo[i] = DEFAULT_SERVO_MIDDLE;
}
/*
* If mixer has predefined servo mixes, load them
* load mixer
*/
if (mixerUsesServos) {
servoRuleCount = servoMixers[currentMixerMode].servoRuleCount;
if (servoMixers[currentMixerMode].rule) {
for (int i = 0; i < servoRuleCount; i++) {
currentServoMixer[i] = servoMixers[currentMixerMode].rule[i];
}
}
loadCustomServoMixer();
// If there are servo rules after all, update variables
if (servoRuleCount > 0) {
servoOutputEnabled = 1;
mixerUsesServos = 1;
}
/*
* When we are dealing with CUSTOM mixers, load mixes defined with
* smix and update internal variables
*/
if (loadCustomServoMixer()) {
// If there are servo rules after all, update variables
if (servoRuleCount > 0) {
servoOutputEnabled = 1;
mixerUsesServos = 1;
}
}
// enable servos for mixes that require them. note, this shifts motor counts.
mixerUsesServos = mixerUsesServos || feature(FEATURE_SERVO_TILT);
// if we want camstab/trig, that also enables servos, even if mixer doesn't
servoOutputEnabled = mixerUsesServos || feature(FEATURE_CHANNEL_FORWARDING);
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++)
servoComputeScalingFactors(i);
}
bool loadCustomServoMixer(void)
void loadCustomServoMixer(void)
{
const mixerMode_e currentMixerMode = mixerConfig()->mixerMode;
if (!(currentMixerMode == MIXER_CUSTOM_AIRPLANE || currentMixerMode == MIXER_CUSTOM_TRI || currentMixerMode == MIXER_CUSTOM)) {
return false;
}
// reset settings
servoRuleCount = 0;
minServoIndex = 255;
@ -276,22 +191,6 @@ bool loadCustomServoMixer(void)
memcpy(&currentServoMixer[i], customServoMixers(i), sizeof(servoMixer_t));
servoRuleCount++;
}
return true;
}
void servoMixerLoadMix(int index)
{
// we're 1-based
index++;
// clear existing
for (int i = 0; i < MAX_SERVO_RULES; i++) {
customServoMixersMutable(i)->targetChannel = customServoMixersMutable(i)->inputSource = customServoMixersMutable(i)->rate = 0;
}
for (int i = 0; i < servoMixers[index].servoRuleCount; i++) {
*customServoMixersMutable(i) = servoMixers[index].rule[i];
}
}
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)

3
src/main/flight/servos.h
View file

@ -128,8 +128,7 @@ extern int16_t servo[MAX_SUPPORTED_SERVOS];
bool isServoOutputEnabled(void);
bool isMixerUsingServos(void);
void writeServos(void);
void servoMixerLoadMix(int index);
bool loadCustomServoMixer(void);
void loadCustomServoMixer(void);
int servoDirection(int servoIndex, int fromChannel);
void servoMixer(float dT);
void servoComputeScalingFactors(uint8_t servoIndex);

1
src/main/target/common.h
View file

@ -119,7 +119,6 @@
#define SKIP_TASK_STATISTICS
#define SKIP_CLI_COMMAND_HELP
#define SKIP_CLI_RESOURCES
#define DISABLE_UNCOMMON_MIXERS
#define NAV_MAX_WAYPOINTS 30
#define MAX_BOOTLOG_ENTRIES 32
#endif