diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5fa74dcfbd..e6dcea7254 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -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) { diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 6f2b6b0920..64a6c39380 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 d2208d7b8c..50fbb4a82f 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()) diff --git a/src/main/msp/msp_protocol_v2_inav.h b/src/main/msp/msp_protocol_v2_inav.h index 0d7f107fa1..eba96690a2 100755 --- a/src/main/msp/msp_protocol_v2_inav.h +++ b/src/main/msp/msp_protocol_v2_inav.h @@ -97,3 +97,5 @@ #define MSP2_INAV_EZ_TUNE 0x2070 #define MSP2_INAV_EZ_TUNE_SET 0x2071 + +#define MSP2_INAV_SELECT_MIXER_PROFILE 0x2080