mixprofile transition support

src/main/drivers/pwm_mapping.c

@@ -201,7 +201,7 @@ static bool checkPwmTimerConflicts(const timerHardware_t *timHw)
 }
 
 static void timerHardwareOverride(timerHardware_t * timer) {
-    if (mixerConfig()->outputMode == OUTPUT_MODE_SERVOS) {
+    if (currentMixerConfig.outputMode == OUTPUT_MODE_SERVOS) {
         
         //Motors are rewritten as servos
         if (timer->usageFlags & TIM_USE_MC_MOTOR) {
@@ -213,7 +213,7 @@ static void timerHardwareOverride(timerHardware_t * timer) {
             timer->usageFlags = timer->usageFlags | TIM_USE_FW_SERVO;
         }
         
-    } else if (mixerConfig()->outputMode == OUTPUT_MODE_MOTORS) {
+    } else if (currentMixerConfig.outputMode == OUTPUT_MODE_MOTORS) {
         
         // Servos are rewritten as motors
         if (timer->usageFlags & TIM_USE_MC_SERVO) {
@@ -250,7 +250,7 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU
         }
 
         // Determine if timer belongs to motor/servo
-        if (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER) {
+        if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER) {
             // Multicopter
 
             // Make sure first motorCount outputs get assigned to motor

src/main/fc/config.c

@@ -332,9 +332,6 @@ void readEEPROM(void)
     setConfigProfile(getConfigProfile());
     setConfigBatteryProfile(getConfigBatteryProfile());
     setConfigMixerProfile(getConfigMixerProfile());
-    if(mixerConfig()->PIDProfileLinking){
-        setConfigProfile(getConfigMixerProfile());
-    }
 
     validateAndFixConfig();
     activateConfig();

src/main/fc/fc_core.c

@@ -784,7 +784,7 @@ void processRx(timeUs_t currentTimeUs)
          }
      }
 //---------------------------------------------------------
-    if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
+    if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
         DISABLE_FLIGHT_MODE(HEADFREE_MODE);
     }
 

src/main/fc/fc_init.c

@@ -303,9 +303,7 @@ void init(void)
 
     // Initialize servo and motor mixers
     // This needs to be called early to set up platform type correctly and count required motors & servos
-    servosInit();
+    mixerConfigInit();
-    mixerUpdateStateFlags();
-    mixerInit();
 
     // Some sanity checking
     if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) {

src/main/fc/fc_msp_box.c

@@ -99,6 +99,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = {
     { .boxId = BOXSOARING,          .boxName = "SOARING",           .permanentId = 56 },
     { .boxId = BOXCHANGEMISSION,    .boxName = "MISSION CHANGE",    .permanentId = 59 },
     { .boxId = BOXBEEPERMUTE,       .boxName = "BEEPER MUTE",       .permanentId = 60 },
+    { .boxId = BOXMIXERPROFILE,     .boxName = "MIXER RPROFILE 2",  .permanentId = 61 },
+    { .boxId = BOXMIXERTRANSITION,  .boxName = "MIXER TRANSITION",  .permanentId = 62 },
     { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL,                .permanentId = 0xFF }
 };
 

src/main/fc/rc_modes.h

@@ -78,6 +78,8 @@ typedef enum {
     BOXUSER4         = 49,
     BOXCHANGEMISSION = 50,
     BOXBEEPERMUTE    = 51,
+    BOXMIXERPROFILE      = 52,
+    BOXMIXERTRANSITION   = 53,
     CHECKBOX_ITEM_COUNT
 } boxId_e;
 

src/main/flight/mixer.c

@@ -116,7 +116,7 @@ static void computeMotorCount(void)
 }
 
 bool ifMotorstopFeatureEnabled(void){
-    return mixerConfig()->motorstopFeature;
+    return currentMixerConfig.motorstopFeature;
 }
 
 uint8_t getMotorCount(void) {
@@ -142,31 +142,31 @@ void mixerUpdateStateFlags(void)
     DISABLE_STATE(AIRPLANE);
     DISABLE_STATE(MOVE_FORWARD_ONLY);
 
-    if (mixerConfig()->platformType == PLATFORM_AIRPLANE) {
+    if (currentMixerConfig.platformType == PLATFORM_AIRPLANE) {
         ENABLE_STATE(FIXED_WING_LEGACY);
         ENABLE_STATE(AIRPLANE);
         ENABLE_STATE(ALTITUDE_CONTROL);
         ENABLE_STATE(MOVE_FORWARD_ONLY);
-    } if (mixerConfig()->platformType == PLATFORM_ROVER) {
+    } if (currentMixerConfig.platformType == PLATFORM_ROVER) {
         ENABLE_STATE(ROVER);
         ENABLE_STATE(FIXED_WING_LEGACY);
         ENABLE_STATE(MOVE_FORWARD_ONLY);
-    } if (mixerConfig()->platformType == PLATFORM_BOAT) {
+    } if (currentMixerConfig.platformType == PLATFORM_BOAT) {
         ENABLE_STATE(BOAT);
         ENABLE_STATE(FIXED_WING_LEGACY);
         ENABLE_STATE(MOVE_FORWARD_ONLY);
-    } else if (mixerConfig()->platformType == PLATFORM_MULTIROTOR) {
+    } else if (currentMixerConfig.platformType == PLATFORM_MULTIROTOR) {
         ENABLE_STATE(MULTIROTOR);
         ENABLE_STATE(ALTITUDE_CONTROL);
-    } else if (mixerConfig()->platformType == PLATFORM_TRICOPTER) {
+    } else if (currentMixerConfig.platformType == PLATFORM_TRICOPTER) {
         ENABLE_STATE(MULTIROTOR);
         ENABLE_STATE(ALTITUDE_CONTROL);
-    } else if (mixerConfig()->platformType == PLATFORM_HELICOPTER) {
+    } else if (currentMixerConfig.platformType == PLATFORM_HELICOPTER) {
         ENABLE_STATE(MULTIROTOR);
         ENABLE_STATE(ALTITUDE_CONTROL);
     }
 
-    if (mixerConfig()->hasFlaps) {
+    if (currentMixerConfig.hasFlaps) {
         ENABLE_STATE(FLAPERON_AVAILABLE);
     } else {
         DISABLE_STATE(FLAPERON_AVAILABLE);
@@ -192,7 +192,7 @@ void mixerInit(void)
 
     mixerResetDisarmedMotors();
 
-    if (mixerConfig()->motorDirectionInverted) {
+    if (currentMixerConfig.motorDirectionInverted) {
         motorYawMultiplier = -1;
     } else {
         motorYawMultiplier = 1;
@@ -272,7 +272,7 @@ static void applyTurtleModeToMotors(void) {
 
         float signPitch = rcCommand[PITCH] < 0 ? 1 : -1;
         float signRoll = rcCommand[ROLL] < 0 ? 1 : -1;
-        float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (mixerConfig()->motorDirectionInverted ? 1 : -1));
+        float signYaw = (float)((rcCommand[YAW] < 0 ? 1 : -1) * (currentMixerConfig.motorDirectionInverted ? 1 : -1));
 
         float stickDeflectionLength = calc_length_pythagorean_2D(stickDeflectionPitchAbs, stickDeflectionRollAbs);
         float stickDeflectionExpoLength = calc_length_pythagorean_2D(stickDeflectionPitchExpo, stickDeflectionRollExpo);
@@ -593,13 +593,20 @@ void FAST_CODE mixTable(void)
                 motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
             }
             
+            //stop motors
+            if (currentMixer[i].throttle <= 0.0f) {
+                motor[i] = motorZeroCommand;
+            }
+            //spin stopped motors only in mixer transition mode
+            if (IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) {
+                motor[i] = -currentMixer[i].throttle * 1000;
+                motor[i] = constrain(motor[i], throttleRangeMin, throttleRangeMax);
+            }
+
             // Motor stop handling
             if (currentMotorStatus != MOTOR_RUNNING) {
                 motor[i] = motorValueWhenStopped;
             }
-            if (currentMixer[i].throttle <= -1.0f) {
-                motor[i] = motorZeroCommand;
-            }
 #ifdef USE_DEV_TOOLS
             if (systemConfig()->groundTestMode) {
                 motor[i] = motorZeroCommand;

src/main/flight/mixer_profile.c

@@ -19,12 +19,16 @@
 #include "fc/config.h"
 #include "fc/runtime_config.h"
 #include "fc/settings.h"
+#include "fc/rc_modes.h"
 
 #include "programming/logic_condition.h"
 #include "navigation/navigation.h"
 
 #include "common/log.h"
 
+mixerConfig_t currentMixerConfig;
+int currentMixerProfileIndex;
+
 PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
 
 void pgResetFn_mixerProfiles(mixerProfile_t *instance)
@@ -63,6 +67,26 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
     }
 }
 
+void loadMixerConfig(void) {
+    currentMixerProfileIndex=getConfigMixerProfile();
+    currentMixerConfig=*mixerConfig();
+}
+
+void mixerConfigInit(void){
+    loadMixerConfig();
+    servosInit();
+    mixerUpdateStateFlags();
+    mixerInit();
+    if(currentMixerConfig.PIDProfileLinking){
+        LOG_INFO(PWM, "mixer switch pidInit");
+        setConfigProfile(getConfigMixerProfile());
+        pidInit();
+        pidInitFilters();
+        schedulePidGainsUpdate();
+        navigationUsePIDs(); //set navigation pid gains
+    }
+}
+
 static int computeMotorCountByMixerProfileIndex(int index)
 {
     int motorCount = 0;
@@ -110,27 +134,22 @@ static int computeServoCountByMixerProfileIndex(int index)
     }
 }
 
-void SwitchPidProfileByMixerProfile()
-{
-    LOG_INFO(PWM, "mixer switch pidInit");
-    setConfigProfile(getConfigMixerProfile());
-    pidInit();
-    pidInitFilters();
-    schedulePidGainsUpdate();
-    navigationUsePIDs(); //set navigation pid gains
-}
-
 //switch mixerprofile without reboot
 bool OutputProfileHotSwitch(int profile_index)
 {
-    // does not work with timerHardwareOverride
+    static bool allow_hot_switch = true;
+    // does not work with timerHardwareOverride,need to set mixerConfig()->outputMode == OUTPUT_MODE_AUTO
     LOG_INFO(PWM, "OutputProfileHotSwitch");
+    if (!allow_hot_switch)
+    {
+        return false;
+    }
     if (profile_index < 0 || profile_index >= MAX_MIXER_PROFILE_COUNT)
     { // sanity check
         LOG_INFO(PWM, "invalid mixer profile index");
         return false;
     }
-    if (getConfigMixerProfile() == profile_index)
+    if (currentMixerProfileIndex == profile_index)
     {
         return false;
     }
@@ -138,17 +157,18 @@ bool OutputProfileHotSwitch(int profile_index)
         return false;
     }
     //do not allow switching in navigation mode
-    if (ARMING_FLAG(ARMED) && navigationInAnyMode()){
+    if (ARMING_FLAG(ARMED) && (navigationInAnyMode() || isUsingNavigationModes())){
         LOG_INFO(PWM, "mixer switch failed, navModesEnabled");
         return false;
     }
-    //do not allow switching between multi rotor and non multi rotor
+    //pwm mapping map outputs based on platformtype, check if mapping remain unchanged after the switch
+    //do not allow switching between multi rotor and non multi rotor if sannity check fails
 #ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP
     bool MCFW_hotswap_available = true;
 #else
     bool MCFW_hotswap_available = false;
 #endif
-    uint8_t old_platform_type = mixerConfig()->platformType;
+    uint8_t old_platform_type = currentMixerConfig.platformType;
     uint8_t new_platform_type = mixerConfigByIndex(profile_index)->platformType;
     bool old_platform_type_mc = old_platform_type == PLATFORM_MULTIROTOR || old_platform_type == PLATFORM_TRICOPTER;
     bool new_platform_type_mc = new_platform_type == PLATFORM_MULTIROTOR || new_platform_type == PLATFORM_TRICOPTER;
@@ -156,14 +176,17 @@ bool OutputProfileHotSwitch(int profile_index)
     if ((!MCFW_hotswap_available) && is_mcfw_switching)
     {
         LOG_INFO(PWM, "mixer MCFW_hotswap_unavailable");
+        allow_hot_switch = false;
         return false;
     }
     //do not allow switching if motor or servos counts has changed
     if ((getMotorCount() != computeMotorCountByMixerProfileIndex(profile_index)) || (getServoCount() != computeServoCountByMixerProfileIndex(profile_index)))
     {
+
         LOG_INFO(PWM, "mixer switch failed, because of motor/servo count will change");
         // LOG_INFO(PWM, "old motor/servo count:%d,%d",getMotorCount(),getServoCount());
         // LOG_INFO(PWM, "new motor/servo count:%d,%d",computeMotorCountByMixerProfileIndex(profile_index),computeServoCountByMixerProfileIndex(profile_index));
+        allow_hot_switch = false;
         return false;
     }
     if (!setConfigMixerProfile(profile_index)){
@@ -171,14 +194,8 @@ bool OutputProfileHotSwitch(int profile_index)
         return false;
     }
     stopMotorsNoDelay();
-    servosInit();
+    mixerConfigInit();
-    mixerUpdateStateFlags();
-    mixerInit();
 
-    if(mixerConfig()->PIDProfileLinking)
-    {
-        SwitchPidProfileByMixerProfile();
-    }
     return true;
 }
 

src/main/flight/mixer_profile.h

@@ -26,6 +26,8 @@ typedef struct mixerProfile_s {
 
 PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles);
 
+extern mixerConfig_t currentMixerConfig;
+extern int currentMixerProfileIndex;
 #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config))
 #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig())
 
@@ -43,3 +45,4 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index)
 #define mixerServoMixersByIndex(index) (&(mixerProfiles(index)->ServoMixers))
 
 bool OutputProfileHotSwitch(int profile_index);
+void mixerConfigInit(void);

src/main/flight/pid.c

@@ -1215,9 +1215,9 @@ void pidInit(void)
 
     if (pidProfile()->pidControllerType == PID_TYPE_AUTO) {
         if (
-            mixerConfig()->platformType == PLATFORM_AIRPLANE ||
+            currentMixerConfig.platformType == PLATFORM_AIRPLANE ||
-            mixerConfig()->platformType == PLATFORM_BOAT ||
+            currentMixerConfig.platformType == PLATFORM_BOAT ||
-            mixerConfig()->platformType == PLATFORM_ROVER
+            currentMixerConfig.platformType == PLATFORM_ROVER
         ) {
             usedPidControllerType = PID_TYPE_PIFF;
         } else {

src/main/flight/servos.c

@@ -251,7 +251,7 @@ void writeServos(void)
     /*
      * in case of tricopters, there might me a need to zero servo output when unarmed
      */
-    if (mixerConfig()->platformType == PLATFORM_TRICOPTER && !ARMING_FLAG(ARMED) && !servoConfig()->tri_unarmed_servo) {
+    if (currentMixerConfig.platformType == PLATFORM_TRICOPTER && !ARMING_FLAG(ARMED) && !servoConfig()->tri_unarmed_servo) {
         zeroServoValue = true;
     }
 
@@ -281,7 +281,7 @@ void servoMixer(float dT)
 
         // Reverse yaw servo when inverted in 3D mode only for multirotor and tricopter
         if (feature(FEATURE_REVERSIBLE_MOTORS) && (rxGetChannelValue(THROTTLE) < PWM_RANGE_MIDDLE) &&
-        (mixerConfig()->platformType == PLATFORM_MULTIROTOR || mixerConfig()->platformType == PLATFORM_TRICOPTER)) {
+        (currentMixerConfig.platformType == PLATFORM_MULTIROTOR || currentMixerConfig.platformType == PLATFORM_TRICOPTER)) {
             input[INPUT_STABILIZED_YAW] *= -1;
         }
     }
@@ -317,6 +317,8 @@ void servoMixer(float dT)
 
     input[INPUT_STABILIZED_THROTTLE] = mixerThrottleCommand - 1000 - 500;  // Since it derives from rcCommand or mincommand and must be [-500:+500]
 
+    input[BOXMIXERTRANSITION] = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) * 500; //fixed value
+
     // center the RC input value around the RC middle value
     // by subtracting the RC middle value from the RC input value, we get:
     // data - middle = input

src/main/flight/servos.h

@@ -62,7 +62,7 @@ typedef enum {
     INPUT_GVAR_5                    = 35,
     INPUT_GVAR_6                    = 36,
     INPUT_GVAR_7                    = 37,
-    INPUT_MC2FW_TRANSITION          = 38,
+    INPUT_MIXER_TRANSITION          = 38,
     INPUT_SOURCE_COUNT
 } inputSource_e;
 

src/main/navigation/navigation.c

@@ -4179,8 +4179,8 @@ void navigationUsePIDs(void)
 
 void navigationInitYawControl(void){
     if (
-        mixerConfig()->platformType == PLATFORM_BOAT ||
+        currentMixerConfig.platformType == PLATFORM_BOAT ||
-        mixerConfig()->platformType == PLATFORM_ROVER ||
+        currentMixerConfig.platformType == PLATFORM_ROVER ||
         navConfig()->fw.useFwNavYawControl
     ) {
         ENABLE_STATE(FW_HEADING_USE_YAW);
@@ -4224,8 +4224,8 @@ void navigationInit(void)
     navigationUsePIDs();
 
     if (
-        mixerConfig()->platformType == PLATFORM_BOAT ||
+        currentMixerConfig.platformType == PLATFORM_BOAT ||
-        mixerConfig()->platformType == PLATFORM_ROVER ||
+        currentMixerConfig.platformType == PLATFORM_ROVER ||
         navConfig()->fw.useFwNavYawControl
     ) {
         ENABLE_STATE(FW_HEADING_USE_YAW);
@@ -4352,7 +4352,7 @@ bool navigationIsExecutingAnEmergencyLanding(void)
 bool navigationInAnyMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
-    return !(stateFlags ==0);
+    return !(stateFlags == 0);
 }
 
 bool navigationInAutomaticThrottleMode(void)

src/main/programming/logic_condition.c

@@ -773,7 +773,7 @@ static int logicConditionGetFlightOperandValue(int operand) {
             break;
         
         case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int
-            return getConfigMixerProfile() + 1;
+            return currentMixerProfileIndex + 1;
             break;
 
         case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: