Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-24 00:35:34 +03:00
Code Issues Wiki Activity

fix mixer_profie configurator issue

Browse source
This commit is contained in:
shota 2023-10-12 22:29:17 +09:00
parent 9c3a466bf6
commit ee015969ba
3 changed files with 26 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

23
src/main/fc/fc_msp.c
View file

@ -522,6 +522,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
sbufWriteU8(dst, -1); sbufWriteU8(dst, -1);
#endif #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; break;
#ifdef USE_PROGRAMMING_FRAMEWORK #ifdef USE_PROGRAMMING_FRAMEWORK
case MSP2_INAV_LOGIC_CONDITIONS: case MSP2_INAV_LOGIC_CONDITIONS:
@ -567,11 +579,18 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF
#endif #endif
case MSP2_COMMON_MOTOR_MIXER: case MSP2_COMMON_MOTOR_MIXER:
for (uint8_t i = 0; i < MAX_SUPPORTED_MOTORS; i++) { 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)->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)->pitch + 2.0f, 0.0f, 4.0f) * 1000);
sbufWriteU16(dst, constrainf(primaryMotorMixer(i)->yaw + 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; break;
case MSP_MOTOR: case MSP_MOTOR:
@ -2100,7 +2119,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src)
case MSP2_COMMON_SET_MOTOR_MIXER: case MSP2_COMMON_SET_MOTOR_MIXER:
sbufReadU8Safe(&tmp_u8, src); sbufReadU8Safe(&tmp_u8, src);
if ((dataSize == 9) && (tmp_u8 < MAX_SUPPORTED_MOTORS)) { 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)->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)->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; primaryMotorMixerMutable(tmp_u8)->yaw = constrainf(sbufReadU16(src) / 1000.0f, 0.0f, 4.0f) - 2.0f;

8
src/main/flight/mixer_profile.c
View file

@ -35,7 +35,7 @@ int currentMixerProfileIndex;
bool isMixerTransitionMixing; bool isMixerTransitionMixing;
bool isMixerTransitionMixing_requested; bool isMixerTransitionMixing_requested;
mixerProfileAT_t mixerProfileAT; mixerProfileAT_t mixerProfileAT;
int nextProfileIndex; int nextMixerProfileIndex;
PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); 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(){ void activateMixerConfig(){
currentMixerProfileIndex = getConfigMixerProfile(); currentMixerProfileIndex = getConfigMixerProfile();
currentMixerConfig = *mixerConfig(); currentMixerConfig = *mixerConfig();
nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; nextMixerProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT;
} }
void mixerConfigInit(void) void mixerConfigInit(void)
@ -113,7 +113,7 @@ bool platformTypeConfigured(flyingPlatformType_e platformType)
if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){ if (!isModeActivationConditionPresent(BOXMIXERPROFILE)){
return false; return false;
} }
return mixerConfigByIndex(nextProfileIndex)->platformType == platformType; return mixerConfigByIndex(nextMixerProfileIndex)->platformType == platformType;
} }
bool checkMixerATRequired(mixerProfileATRequest_e required_action) bool checkMixerATRequired(mixerProfileATRequest_e required_action)
@ -171,7 +171,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action)
isMixerTransitionMixing_requested = true; isMixerTransitionMixing_requested = true;
if (millis() > mixerProfileAT.transitionTransEndTime){ if (millis() > mixerProfileAT.transitionTransEndTime){
isMixerTransitionMixing_requested = false; isMixerTransitionMixing_requested = false;
outputProfileHotSwitch(nextProfileIndex); outputProfileHotSwitch(nextMixerProfileIndex);
mixerProfileAT.phase = MIXERAT_PHASE_IDLE; mixerProfileAT.phase = MIXERAT_PHASE_IDLE;
reprocessState = true; reprocessState = true;
//transition is done //transition is done

1
src/main/flight/mixer_profile.h
View file

@ -54,6 +54,7 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action);
extern mixerConfig_t currentMixerConfig; extern mixerConfig_t currentMixerConfig;
extern int currentMixerProfileIndex; extern int currentMixerProfileIndex;
extern int nextMixerProfileIndex;
extern bool isMixerTransitionMixing; extern bool isMixerTransitionMixing;
#define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config))
#define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig())