1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 09:16:01 +03:00

Refactor motor mixer to prepare for primary and secondary mixer bank (#4467)

* Refactor motor mixer to use getter instead of array copy and use getter to access

* Fix FASTRAM problem

* Solve the problem of 3D mixer scaling

* Prepare for primary and secondary motor mixer

* enable build on SPRF3NEO
This commit is contained in:
Paweł Spychalski 2019-03-07 21:35:37 +01:00 committed by GitHub
parent 2892d9d11a
commit 5bfcdd8089
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
12 changed files with 110 additions and 100 deletions

View file

@ -44,5 +44,12 @@
#else
#define FASTRAM __attribute__ ((section(".fastram_bss"), aligned(4)))
#endif
#if defined (STM32F4) || defined (STM32F7)
#define EXTENDED_FASTRAM FASTRAM
#else
#define EXTENDED_FASTRAM
#endif
#define STATIC_FASTRAM static FASTRAM
#define STATIC_FASTRAM_UNIT_TESTED STATIC_UNIT_TESTED FASTRAM

View file

@ -983,7 +983,7 @@ static void cliAdjustmentRange(char *cmdline)
}
}
static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer, const motorMixer_t *defaultCustomMotorMixer)
static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer)
{
const char *format = "mmix %d %s %s %s %s";
char buf0[FTOA_BUFFER_SIZE];
@ -991,18 +991,18 @@ static void printMotorMix(uint8_t dumpMask, const motorMixer_t *customMotorMixer
char buf2[FTOA_BUFFER_SIZE];
char buf3[FTOA_BUFFER_SIZE];
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (customMotorMixer[i].throttle == 0.0f)
if (primaryMotorMixer[i].throttle == 0.0f)
break;
const float thr = customMotorMixer[i].throttle;
const float roll = customMotorMixer[i].roll;
const float pitch = customMotorMixer[i].pitch;
const float yaw = customMotorMixer[i].yaw;
const float thr = primaryMotorMixer[i].throttle;
const float roll = primaryMotorMixer[i].roll;
const float pitch = primaryMotorMixer[i].pitch;
const float yaw = primaryMotorMixer[i].yaw;
bool equalsDefault = false;
if (defaultCustomMotorMixer) {
const float thrDefault = defaultCustomMotorMixer[i].throttle;
const float rollDefault = defaultCustomMotorMixer[i].roll;
const float pitchDefault = defaultCustomMotorMixer[i].pitch;
const float yawDefault = defaultCustomMotorMixer[i].yaw;
if (defaultprimaryMotorMixer) {
const float thrDefault = defaultprimaryMotorMixer[i].throttle;
const float rollDefault = defaultprimaryMotorMixer[i].roll;
const float pitchDefault = defaultprimaryMotorMixer[i].pitch;
const float yawDefault = defaultprimaryMotorMixer[i].yaw;
const bool equalsDefault = thr == thrDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
@ -1027,11 +1027,11 @@ static void cliMotorMix(char *cmdline)
const char *ptr;
if (isEmpty(cmdline)) {
printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL);
printMotorMix(DUMP_MASTER, primaryMotorMixer(0), NULL);
} else if (sl_strncasecmp(cmdline, "reset", 5) == 0) {
// erase custom mixer
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
customMotorMixerMutable(i)->throttle = 0.0f;
primaryMotorMixerMutable(i)->throttle = 0.0f;
}
} else {
ptr = cmdline;
@ -1039,28 +1039,28 @@ static void cliMotorMix(char *cmdline)
if (i < MAX_SUPPORTED_MOTORS) {
ptr = nextArg(ptr);
if (ptr) {
customMotorMixerMutable(i)->throttle = fastA2F(ptr);
primaryMotorMixerMutable(i)->throttle = fastA2F(ptr);
check++;
}
ptr = nextArg(ptr);
if (ptr) {
customMotorMixerMutable(i)->roll = fastA2F(ptr);
primaryMotorMixerMutable(i)->roll = fastA2F(ptr);
check++;
}
ptr = nextArg(ptr);
if (ptr) {
customMotorMixerMutable(i)->pitch = fastA2F(ptr);
primaryMotorMixerMutable(i)->pitch = fastA2F(ptr);
check++;
}
ptr = nextArg(ptr);
if (ptr) {
customMotorMixerMutable(i)->yaw = fastA2F(ptr);
primaryMotorMixerMutable(i)->yaw = fastA2F(ptr);
check++;
}
if (check != 4) {
cliShowParseError();
} else {
printMotorMix(DUMP_MASTER, customMotorMixer(0), NULL);
printMotorMix(DUMP_MASTER, primaryMotorMixer(0), NULL);
}
} else {
cliShowArgumentRangeError("index", 0, MAX_SUPPORTED_MOTORS - 1);
@ -2836,9 +2836,9 @@ static void printConfig(const char *cmdline, bool doDiff)
//printResource(dumpMask, &defaultConfig);
cliPrintHashLine("mixer");
cliDumpPrintLinef(dumpMask, customMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n");
cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n");
printMotorMix(dumpMask, customMotorMixer_CopyArray, customMotorMixer(0));
printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0));
// print custom servo mixer if exists
cliPrintHashLine("servo mix");

View file

@ -347,7 +347,7 @@ void init(void)
// pwmInit() needs to be called as soon as possible for ESC compatibility reasons
pwmInit(&pwm_params);
mixerUsePWMIOConfiguration();
mixerPrepare();
if (!pwm_params.useFastPwm)
motorControlEnable = true;

View file

@ -491,10 +491,10 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, customMotorMixer(i)->throttle * 1000);
sbufWriteU16(dst, constrainf(customMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(customMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(customMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, primaryMotorMixer(i)->throttle * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->roll + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 2.0f, 0.0f, 4.0f) * 1000);
}
break;
@ -1884,10 +1884,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
customMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f);
customMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
customMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
customMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f);
primaryMotorMixerMutable(tmp_u8)->roll = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
primaryMotorMixerMutable(tmp_u8)->pitch = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
} else
return MSP_RESULT_ERROR;
break;

View file

@ -89,20 +89,20 @@ extern uint32_t flightModeFlags;
#define FLIGHT_MODE(mask) (flightModeFlags & (mask))
typedef enum {
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
FLAPERON_AVAILABLE = (1 << 6),
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED= (1 << 9),
PWM_DRIVER_AVAILABLE = (1 << 10),
NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13),
GPS_FIX_HOME = (1 << 0),
GPS_FIX = (1 << 1),
CALIBRATE_MAG = (1 << 2),
SMALL_ANGLE = (1 << 3),
FIXED_WING = (1 << 4), // set when in flying_wing or airplane mode. currently used by althold selection code
ANTI_WINDUP = (1 << 5),
FLAPERON_AVAILABLE = (1 << 6),
NAV_MOTOR_STOP_OR_IDLE = (1 << 7), // navigation requests MOTOR_STOP or motor idle regardless of throttle stick, will only activate if MOTOR_STOP feature is available
COMPASS_CALIBRATED = (1 << 8),
ACCELEROMETER_CALIBRATED = (1 << 9),
PWM_DRIVER_AVAILABLE = (1 << 10),
NAV_CRUISE_BRAKING = (1 << 11),
NAV_CRUISE_BRAKING_BOOST = (1 << 12),
NAV_CRUISE_BRAKING_LOCKED = (1 << 13),
} stateFlags_t;
#define DISABLE_STATE(mask) (stateFlags &= ~(mask))

View file

@ -53,14 +53,12 @@
#include "sensors/battery.h"
//#define MIXER_DEBUG
static uint8_t motorCount;
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange;
static float mixerScale = 1.0f;
static EXTENDED_FASTRAM motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
static EXTENDED_FASTRAM uint8_t motorCount = 0;
PG_REGISTER_WITH_RESET_TEMPLATE(flight3DConfig_t, flight3DConfig, PG_MOTOR_3D_CONFIG, 0);
@ -104,12 +102,21 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig,
.digitalIdleOffsetValue = 450 // Same scale as in Betaflight
);
static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS];
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer, PG_MOTOR_MIXER, 0);
uint8_t getMotorCount(void)
static void computeMotorCount(void)
{
motorCount = 0;
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (primaryMotorMixer(i)->throttle == 0.0f) {
break;
}
motorCount++;
}
}
uint8_t FAST_CODE NOINLINE getMotorCount(void) {
return motorCount;
}
@ -141,28 +148,13 @@ void mixerUpdateStateFlags(void)
}
}
void mixerUsePWMIOConfiguration(void)
void mixerPrepare(void)
{
motorCount = 0;
// load custom mixer into currentMixer
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (customMotorMixer(i)->throttle == 0.0f)
break;
currentMixer[i] = *customMotorMixer(i);
motorCount++;
}
computeMotorCount();
loadPrimaryMotorMixer();
// in 3D mode, mixer gain has to be halved
if (feature(FEATURE_3D)) {
if (motorCount > 1) {
for (int i = 0; i < motorCount; i++) {
currentMixer[i].pitch *= 0.5f;
currentMixer[i].roll *= 0.5f;
currentMixer[i].yaw *= 0.5f;
}
}
mixerScale = 0.5f;
}
mixerResetDisarmedMotors();
@ -309,9 +301,9 @@ void FAST_CODE NOINLINE mixTable(const float dT)
// motors for non-servo mixes
for (int i = 0; i < motorCount; i++) {
rpyMix[i] =
input[PITCH] * currentMixer[i].pitch +
(input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll +
-mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw;
-mixerConfig()->yaw_motor_direction * input[YAW] * currentMixer[i].yaw) * mixerScale;
if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
@ -419,3 +411,9 @@ motorStatus_e getMotorStatus(void)
return MOTOR_RUNNING;
}
void loadPrimaryMotorMixer(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
currentMixer[i] = *primaryMotorMixer(i);
}
}

View file

@ -60,7 +60,7 @@ typedef struct motorMixer_s {
float yaw;
} motorMixer_t;
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, customMotorMixer);
PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer);
typedef struct mixerConfig_s {
int8_t yaw_motor_direction;
@ -110,7 +110,7 @@ bool mixerIsOutputSaturated(void);
motorStatus_e getMotorStatus(void);
void writeAllMotors(int16_t mc);
void mixerUsePWMIOConfiguration(void);
void mixerPrepare(void);
void mixerUpdateStateFlags(void);
void mixerResetDisarmedMotors(void);
void mixTable(const float dT);
@ -118,3 +118,5 @@ void writeMotors(void);
void processServoAutotrim(void);
void stopMotors(void);
void stopPwmAllMotors(void);
void loadPrimaryMotorMixer(void);

View file

@ -68,12 +68,12 @@ void targetConfiguration(void)
controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
parseRcChannels("TAER1234");
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}

View file

@ -83,12 +83,12 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[YAW].P = 64;
pidProfileMutable()->bank_mc.pid[YAW].D = 18;
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}

View file

@ -85,12 +85,12 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[YAW].P = 64;
pidProfileMutable()->bank_mc.pid[YAW].D = 18;
*customMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*customMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*customMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*customMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*customMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*customMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*customMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*customMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}

View file

@ -180,3 +180,5 @@
#define TARGET_IO_PORTC 0xffff
#define TARGET_IO_PORTD (BIT(2))
#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4))
#undef USE_TELEMETRY_FRSKY

View file

@ -80,4 +80,5 @@ extern SysTick_Type *SysTick;
#include "target.h"
#define FAST_CODE
#define NOINLINE
#define NOINLINE
#define EXTENDED_FASTRAM