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

Merge pull request #9364 from shota3527/sh_mixer_confi

Browse source 
fix mixer_profie configurator issue
This commit is contained in:
Paweł Spychalski 2023-10-25 08:54:14 +02:00 • committed by GitHub
commit 580cd678b4
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
4 changed files with 37 additions and 6 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -462,6 +462,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));
}
@ -523,6 +524,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:
@ -568,11 +581,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:
@ -2121,7 +2141,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;
@ -3015,6 +3035,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) {

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

@ -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

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

@ -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())

2
src/main/msp/msp_protocol_v2_inav.h
View file

@ -97,3 +97,5 @@
#define MSP2_INAV_EZ_TUNE 0x2070
#define MSP2_INAV_EZ_TUNE_SET 0x2071
#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080