Unverified Commit bf06c9d5 authored by Dylan Jones's avatar Dylan Jones
Browse files

Initial 6DOF mixer upgrade

parent 81f0f517
......@@ -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;
......
......@@ -1014,39 +1014,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 %d %d %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),
  • It might be better to leave the order the same and Tack on forward and lateral at the end. this is least destructive to the rest of the code.

Please register or sign in to reply
ftoa(lat, buf2),
ftoa(roll, buf3),
ftoa(pitch, buf4),
ftoa(yaw, buf5));
}
}
......@@ -1072,6 +1082,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++;
......@@ -1086,7 +1106,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);
......
......@@ -576,6 +576,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);
......@@ -2020,6 +2022,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 9) && (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;
......
......@@ -131,8 +131,9 @@ 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++;
......
......@@ -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;
......
......@@ -68,12 +68,12 @@ void targetConfiguration(void)
controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT;
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