Commit a7b8a27e authored by Dylan Jones's avatar Dylan Jones
Browse files

Merge branch '6dof-mixer-upgrade' into 'master'

6DOF mixer upgrade

See merge request !3
parents 944fab8b 489bc74c
......@@ -44,6 +44,8 @@
static uint8_t currentMotorMixerIndex = 0;
static uint8_t tmpcurrentMotorMixerIndex = 1;
static uint16_t tmpMotorMixerThrottle;
static int16_t tmpMotorMixerForward;
static int16_t tmpMotorMixerLateral;
static int16_t tmpMotorMixerRoll;
static int16_t tmpMotorMixerYaw;
static int16_t tmpMotorMixerPitch;
......@@ -221,6 +223,8 @@ const CMS_Menu cmsx_menuServoMixer = {
static void loadMotorMixerSettings(void)
{
tmpMotorMixerThrottle = primaryMotorMixer(currentMotorMixerIndex)->throttle * 1000;
tmpMotorMixerForward = primaryMotorMixer(currentMotorMixerIndex)->forward * 1000;
tmpMotorMixerLateral = primaryMotorMixer(currentMotorMixerIndex)->lateral * 1000;
tmpMotorMixerRoll = primaryMotorMixer(currentMotorMixerIndex)->roll * 1000;
tmpMotorMixerPitch = primaryMotorMixer(currentMotorMixerIndex)->pitch * 1000;
tmpMotorMixerYaw = primaryMotorMixer(currentMotorMixerIndex)->yaw * 1000;
......@@ -229,6 +233,8 @@ static void loadMotorMixerSettings(void)
static void saveMotorMixerSettings(void)
{
primaryMotorMixerMutable(currentMotorMixerIndex)->throttle = tmpMotorMixerThrottle / 1000.0f;
primaryMotorMixerMutable(currentMotorMixerIndex)->forward = tmpMotorMixerForward / 1000.0f;
primaryMotorMixerMutable(currentMotorMixerIndex)->lateral = tmpMotorMixerLateral / 1000.0f;
primaryMotorMixerMutable(currentMotorMixerIndex)->roll = tmpMotorMixerRoll / 1000.0f;
primaryMotorMixerMutable(currentMotorMixerIndex)->pitch = tmpMotorMixerPitch / 1000.0f;
primaryMotorMixerMutable(currentMotorMixerIndex)->yaw = tmpMotorMixerYaw / 1000.0f;
......
......@@ -1015,39 +1015,49 @@ static void cliAdjustmentRange(char *cmdline)
static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer)
{
const char *format = "mmix %d %s %s %s %s";
const char *format = "mmix %d %s %s %s %s %s %s";
char buf0[FTOA_BUFFER_SIZE];
char buf1[FTOA_BUFFER_SIZE];
char buf2[FTOA_BUFFER_SIZE];
char buf3[FTOA_BUFFER_SIZE];
char buf4[FTOA_BUFFER_SIZE];
char buf5[FTOA_BUFFER_SIZE];
for (uint32_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
if (primaryMotorMixer[i].throttle == 0.0f)
break;
const float thr = primaryMotorMixer[i].throttle;
const float forward = primaryMotorMixer[i].forward;
const float lat = primaryMotorMixer[i].lateral;
const float roll = primaryMotorMixer[i].roll;
const float pitch = primaryMotorMixer[i].pitch;
const float yaw = primaryMotorMixer[i].yaw;
bool equalsDefault = false;
if (defaultprimaryMotorMixer) {
const float thrDefault = defaultprimaryMotorMixer[i].throttle;
const float forwardDefault = defaultprimaryMotorMixer[i].forward;
const float latDefault = defaultprimaryMotorMixer[i].lateral;
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;
const bool equalsDefault = thr == thrDefault && forward == forwardDefault && lat == latDefault && roll == rollDefault && pitch == pitchDefault && yaw == yawDefault;
cliDefaultPrintLinef(dumpMask, equalsDefault, format,
i,
ftoa(thrDefault, buf0),
ftoa(rollDefault, buf1),
ftoa(pitchDefault, buf2),
ftoa(yawDefault, buf3));
ftoa(forwardDefault, buf1),
ftoa(latDefault, buf2),
ftoa(rollDefault, buf3),
ftoa(pitchDefault, buf4),
ftoa(yawDefault, buf5));
}
cliDumpPrintLinef(dumpMask, equalsDefault, format,
i,
ftoa(thr, buf0),
ftoa(roll, buf1),
ftoa(pitch, buf2),
ftoa(yaw, buf3));
ftoa(forward, buf1),
ftoa(lat, buf2),
ftoa(roll, buf3),
ftoa(pitch, buf4),
ftoa(yaw, buf5));
}
}
......@@ -1073,6 +1083,16 @@ static void cliMotorMix(char *cmdline)
check++;
}
ptr = nextArg(ptr);
if (ptr) {
primaryMotorMixerMutable(i)->forward = fastA2F(ptr);
check++;
}
ptr = nextArg(ptr);
if (ptr) {
primaryMotorMixerMutable(i)->lateral = fastA2F(ptr);
check++;
}
ptr = nextArg(ptr);
if (ptr) {
primaryMotorMixerMutable(i)->roll = fastA2F(ptr);
check++;
......@@ -1087,7 +1107,7 @@ static void cliMotorMix(char *cmdline)
primaryMotorMixerMutable(i)->yaw = fastA2F(ptr);
check++;
}
if (check != 4) {
if (check != 6) {
cliShowParseError();
} else {
printMotorMix(DUMP_MASTER, primaryMotorMixer(0), NULL);
......
......@@ -580,6 +580,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
sbufWriteU16(dst, primaryMotorMixer(i)->throttle * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->forward + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->lateral + 2.0f, 0.0f, 4.0f) * 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);
......@@ -2106,8 +2108,10 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
#endif
case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
if ((dataSize == 13) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) {
primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f);
primaryMotorMixerMutable(tmp_u8)->forward = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;
primaryMotorMixerMutable(tmp_u8)->lateral = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.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;
......
......@@ -57,6 +57,8 @@ FILE_COMPILE_FOR_SPEED
#include "sensors/battery.h"
FASTRAM int16_t mixerForwardCommand = 0;
FASTRAM int16_t mixerLateralCommand = 0;
FASTRAM int16_t motor[MAX_SUPPORTED_MOTORS];
FASTRAM int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
static float motorMixRange;
......@@ -141,14 +143,35 @@ static void computeMotorCount(void)
{
motorCount = 0;
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
// check if done
if (primaryMotorMixer(i)->throttle == 0.0f) {
// Stop counting when we hit a motor that doesn't propel us at all
const motorMixer_t *m = primaryMotorMixer(i);
if (m->throttle == 0.0f && m->forward == 0.0f && m->lateral == 0.0f) {
break;
}
motorCount++;
}
}
bool mixerHasForwardAuthority(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
const motorMixer_t *m = primaryMotorMixer(i);
if (m->forward != 0) {
return true;
}
}
return false;
}
bool mixerHasLateralAuthority(void) {
for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) {
const motorMixer_t *m = primaryMotorMixer(i);
if (m->lateral != 0) {
return true;
}
}
return false;
}
uint8_t getMotorCount(void) {
return motorCount;
}
......@@ -538,18 +561,24 @@ void FAST_CODE mixTable(const float dT)
}
#endif
int16_t input[3]; // RPY, range [-500:+500]
int16_t input[5]; // RPYFL, range [-500:+500]
// Allow direct stick input to motors in passthrough mode on airplanes
if (STATE(FIXED_WING_LEGACY) && FLIGHT_MODE(MANUAL_MODE)) {
// Direct passthru from RX
input[ROLL] = rcCommand[ROLL];
input[PITCH] = rcCommand[PITCH];
input[YAW] = rcCommand[YAW];
// Forward/lateral aren't used in passthrough
// TODO figure out where to make FORWARD and LATERAL enums
input[3] = 0;
input[4] = 0;
}
else {
input[ROLL] = axisPID[ROLL];
input[PITCH] = axisPID[PITCH];
input[YAW] = axisPID[YAW];
input[3] = mixerForwardCommand;
input[4] = mixerLateralCommand;
}
// Initial mixer concept by bdoiron74 reused and optimized for Air Mode
......@@ -562,7 +591,9 @@ void FAST_CODE mixTable(const float dT)
rpyMix[i] =
(input[PITCH] * currentMixer[i].pitch +
input[ROLL] * currentMixer[i].roll +
-motorYawMultiplier * input[YAW] * currentMixer[i].yaw) * mixerScale;
-motorYawMultiplier * input[YAW] * currentMixer[i].yaw +
input[3] * currentMixer[i].forward +
input[4] * currentMixer[i].lateral) * mixerScale;
if (rpyMix[i] > rpyMixMax) rpyMixMax = rpyMix[i];
if (rpyMix[i] < rpyMixMin) rpyMixMin = rpyMix[i];
......
......@@ -55,6 +55,8 @@ typedef struct motorAxisCorrectionLimits_s {
// Custom mixer data per motor
typedef struct motorMixer_s {
float throttle;
float forward;
float lateral;
float roll;
float pitch;
float yaw;
......@@ -112,12 +114,16 @@ typedef enum {
extern int16_t motor[MAX_SUPPORTED_MOTORS];
extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS];
extern int mixerThrottleCommand;
extern int16_t mixerForwardCommand;
extern int16_t mixerLateralCommand;
int getThrottleIdleValue(void);
int16_t getThrottlePercent(void);
uint8_t getMotorCount(void);
float getMotorMixRange(void);
bool mixerIsOutputSaturated(void);
bool mixerHasForwardAuthority(void);
bool mixerHasLateralAuthority(void);
motorStatus_e getMotorStatus(void);
void writeAllMotors(int16_t mc);
......
......@@ -621,8 +621,18 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA
const float desiredPitch = atan2_approx(accelForward, GRAVITY_CMSS);
const float desiredRoll = atan2_approx(accelRight * cos_approx(desiredPitch), GRAVITY_CMSS);
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
// Decide whether to get the requested acceleration from roll/pitch or directly from actuators
if (mixerHasForwardAuthority()) {
mixerForwardCommand = accelForward;
} else {
posControl.rcAdjustment[PITCH] = constrain(RADIANS_TO_DECIDEGREES(desiredPitch), -maxBankAngle, maxBankAngle);
}
if (mixerHasLateralAuthority()) {
mixerLateralCommand = accelRight;
} else {
posControl.rcAdjustment[ROLL] = constrain(RADIANS_TO_DECIDEGREES(desiredRoll), -maxBankAngle, maxBankAngle);
}
}
static void applyMulticopterPositionController(timeUs_t currentTimeUs)
......
......@@ -65,12 +65,12 @@ void targetConfiguration(void)
failsafeConfigMutable()->failsafe_off_delay = 0;
parseRcChannels("TAER1234");
*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
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, 0.0f, 0.0f,-0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, 0.0f, 0.0f,-0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, 0.0f, 0.0f,-1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, 0.0f, 0.0f,-1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}
......@@ -83,12 +83,12 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[YAW].P = 64;
pidProfileMutable()->bank_mc.pid[YAW].D = 18;
*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
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}
......@@ -85,12 +85,12 @@ void targetConfiguration(void)
pidProfileMutable()->bank_mc.pid[YAW].P = 64;
pidProfileMutable()->bank_mc.pid[YAW].D = 18;
*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
*primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R
*primaryMotorMixerMutable(1) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -0.414178f, -1.0f, 1.0f }; // FRONT_R
*primaryMotorMixerMutable(2) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, 1.0f, 1.0f }; // REAR_L
*primaryMotorMixerMutable(3) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 0.414178f, -1.0f, -1.0f }; // FRONT_L
*primaryMotorMixerMutable(4) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -1.0f, -0.414178f, -1.0f }; // MIDFRONT_R
*primaryMotorMixerMutable(5) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, -0.414178f, 1.0f }; // MIDFRONT_L
*primaryMotorMixerMutable(6) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, -1.0f, 0.414178f, 1.0f }; // MIDREAR_R
*primaryMotorMixerMutable(7) = (motorMixer_t){ 1.0f, 0.0f, 0.0f, 1.0f, 0.414178f, -1.0f }; // MIDREAR_L
}
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment