From ee015969baab8bd80ea9f74891bf108846b888a4 Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 12 Oct 2023 22:29:17 +0900 Subject: [PATCH 1/4] fix mixer_profie configurator issue --- src/main/fc/fc_msp.c | 23 +++++++++++++++++++++-- src/main/flight/mixer_profile.c | 8 ++++---- src/main/flight/mixer_profile.h | 1 + 3 files changed, 26 insertions(+), 6 deletions(-) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0f3d80829ab..83e11723739 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -522,6 +522,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, -1); #endif } + if(MAX_MIXER_PROFILE_COUNT==1) break; + for (int i = 0; i < MAX_SERVO_RULES; i++) { + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].targetChannel); + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].inputSource); + sbufWriteU16(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].rate); + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].speed); + #ifdef USE_PROGRAMMING_FRAMEWORK + sbufWriteU8(dst, mixerServoMixersByIndex(nextMixerProfileIndex)[i].conditionId); + #else + sbufWriteU8(dst, -1); + #endif + } break; #ifdef USE_PROGRAMMING_FRAMEWORK case MSP2_INAV_LOGIC_CONDITIONS: @@ -567,11 +579,18 @@ 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, primaryMotorMixer(i)->throttle * 1000); + sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->throttle + 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); } + if (MAX_MIXER_PROFILE_COUNT==1) break; + for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].throttle + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].roll + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].pitch + 2.0f, 0.0f, 4.0f) * 1000); + sbufWriteU16(dst, constrainf(mixerMotorMixersByIndex(nextMixerProfileIndex)[i].yaw + 2.0f, 0.0f, 4.0f) * 1000); + } break; case MSP_MOTOR: @@ -2100,7 +2119,7 @@ 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)) { - primaryMotorMixerMutable(tmp_u8)->throttle = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 1.0f); + primaryMotorMixerMutable(tmp_u8)->throttle = 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; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 6f2b6b0920b..64a6c39380d 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -35,7 +35,7 @@ int currentMixerProfileIndex; bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; -int nextProfileIndex; +int nextMixerProfileIndex; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); @@ -81,7 +81,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) void activateMixerConfig(){ currentMixerProfileIndex = getConfigMixerProfile(); currentMixerConfig = *mixerConfig(); - nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; + nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; } void mixerConfigInit(void) @@ -113,7 +113,7 @@ bool platformTypeConfigured(flyingPlatformType_e platformType) if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ return false; } - return mixerConfigByIndex(nextProfileIndex)->platformType == platformType; + return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType; } bool checkMixerATRequired(mixerProfileATRequest_e required_action) @@ -171,7 +171,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) isMixerTransitionMixing_requested = true; if (millis() > mixerProfileAT.transitionTransEndTime){ isMixerTransitionMixing_requested = false; - outputProfileHotSwitch(nextProfileIndex); + outputProfileHotSwitch(nextMixerProfileIndex); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; reprocessState = true; //transition is done diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index d2208d7b8c0..50fbb4a82f1 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -54,6 +54,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; +extern int nextMixerProfileIndex; extern bool isMixerTransitionMixing; #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) From d36d3ccd8126713148a35f4c089fcb5d0eaad3e0 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 15 Oct 2023 17:25:04 +0900 Subject: [PATCH 2/4] mixer_profile switch support --- src/main/fc/config.c | 3 +++ src/main/fc/fc_msp.c | 9 +++++++++ src/main/msp/msp_protocol_v2_inav.h | 2 ++ 3 files changed, 14 insertions(+) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 3950d763762..2f5c4d6c6b3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -500,6 +500,9 @@ void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex) resumeRxSignal(); } beeperConfirmationBeeps(profileIndex + 1); + if (mixerConfig()->PIDProfileLinking) { + setConfigProfileAndWriteEEPROM(profileIndex); + } } void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]) diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 83e11723739..4e0c102d330 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -461,6 +461,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, packSensorStatus()); sbufWriteU16(dst, averageSystemLoadPercent); sbufWriteU8(dst, (getConfigBatteryProfile() << 4) | getConfigProfile()); + sbufWriteU8(dst, getConfigMixerProfile()); sbufWriteU32(dst, armingFlags); sbufWriteData(dst, &mspBoxModeFlags, sizeof(mspBoxModeFlags)); } @@ -3013,6 +3014,14 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) } break; + case MSP2_INAV_SELECT_MIXER_PROFILE: + if (!ARMING_FLAG(ARMED) && sbufReadU8Safe(&tmp_u8, src)) { + setConfigMixerProfileAndWriteEEPROM(tmp_u8); + } else { + return MSP_RESULT_ERROR; + } + break; + #ifdef USE_TEMPERATURE_SENSOR case MSP2_INAV_SET_TEMP_SENSOR_CONFIG: if (dataSize == sizeof(tempSensorConfig_t) * MAX_TEMP_SENSORS) { diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 9cf1881c732..9b4abbdbe45 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -94,3 +94,5 @@ #define MSP2_INAV_RATE_DYNAMICS 0x2060 #define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 + +#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2070 From 070175bc7dc18de0d683d66c8676b1f5bf6cfda6 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 16 Oct 2023 09:33:49 +0900 Subject: [PATCH 3/4] minor fix --- src/main/fc/config.c | 3 --- 1 file changed, 3 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 2f5c4d6c6b3..3950d763762 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -500,9 +500,6 @@ void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex) resumeRxSignal(); } beeperConfirmationBeeps(profileIndex + 1); - if (mixerConfig()->PIDProfileLinking) { - setConfigProfileAndWriteEEPROM(profileIndex); - } } void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]) From 01fcebdfefc2ac0c1a2a627995edc12fa07d681b Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 24 Oct 2023 19:17:49 +0900 Subject: [PATCH 4/4] fix conflict --- src/main/msp/msp_protocol_v2_inav.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 9b4abbdbe45..4556c86a511 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -95,4 +95,4 @@ #define MSP2_INAV_RATE_DYNAMICS 0x2060 #define MSP2_INAV_SET_RATE_DYNAMICS 0x2061 -#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2070 +#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080