add sannity check in hot mixprofile switching, and fix cli

src/main/drivers/pwm_mapping.c

@@ -395,11 +395,11 @@ bool pwmMotorAndServoInit(void)
     return (pwmInitError == PWM_INIT_ERROR_NONE);
 }
 
-bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs)
+// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs)
-{
+// {
-    resetAllocatedOutputPortCount();
+//     resetAllocatedOutputPortCount();
-    pwmInitMotors(timOutputs);
+//     pwmInitMotors(timOutputs);
-    pwmInitServos(timOutputs);
+//     pwmInitServos(timOutputs);
 
-    return (pwmInitError == PWM_INIT_ERROR_NONE);
+//     return (pwmInitError == PWM_INIT_ERROR_NONE);
-}
+// }

src/main/drivers/pwm_mapping.h

@@ -81,7 +81,7 @@ typedef struct {
 
 void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos);
 bool pwmMotorAndServoInit(void);
-bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs);
+// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs);
 const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto);
 pwmInitError_e getPwmInitError(void);
 const char * getPwmInitErrorMessage(void);

src/main/fc/cli.c

@@ -3077,12 +3077,8 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask)
     cliPrintLinef("mixer_profile %d\r\n", getConfigMixerProfile() + 1);
     dumpAllValues(MIXER_CONFIG_VALUE, dumpMask);
     cliPrintHashLine("mixer");
-    cliPrintHashLine("dumpMask");
+    cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n");
-    char buf0[FTOA_BUFFER_SIZE];
+    printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0));
-    itoa(dumpMask,buf0,2);
-    cliPrintHashLine(buf0);
-    cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n");
-    printMotorMix(dumpMask, primaryMotorMixer(0), NULL);
 }
 
 #ifdef USE_CLI_BATCH

src/main/flight/mixer_profile.c

@@ -11,7 +11,8 @@
 #include "drivers/pwm_mapping.h"
 #include "drivers/time.h"
 #include "flight/mixer.h"
-// #include "flight/pid.h"
+#include "common/axis.h"
+#include "flight/pid.h"
 #include "flight/servos.h"
 
 #include "fc/config.h"
@@ -20,6 +21,8 @@
 
 #include "common/log.h"
 
+#include "navigation/navigation.h"
+
 PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1);
 
 void pgResetFn_mixerProfiles(mixerProfile_t *instance)
@@ -43,62 +46,95 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
 
 // PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0);
 
+bool OutputProfileHotSwitch(int profile_index)
+{   
+    // does not work with timerHardwareOverride
+    LOG_INFO(PWM, "OutputProfileHotSwitch");
+
+    //do not allow switching between multi rotor and non multi rotor
+#ifdef ENABLE_MCFW_MIXER_PROFILE_HOTSWAP
+    bool MCFW_hotswap_unavailable = false;
+#else
+    bool MCFW_hotswap_unavailable = true;
+#endif
+    uint8_t old_platform_type = mixerConfig()->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;
+    bool is_mcfw_switching = old_platform_type_mc ^ new_platform_type_mc;
+    if (MCFW_hotswap_unavailable && is_mcfw_switching)
+    {
+        LOG_INFO(PWM, "MCFW_hotswap_unavailable");
+        return false;
+    }
+
+    //do not allow switching in navigation mode
+    if (ARMING_FLAG(ARMED) && navigationInAnyMode()){
+        LOG_INFO(PWM, "navModesEnabled");
+        return false;
+    }
+    
+    if (!setConfigMixerProfile(profile_index)){
+        LOG_INFO(PWM, "failed to set config");
+        return false;
+    }
+    // stopMotors();
+    writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors
+    servosInit();
+    mixerUpdateStateFlags();
+    mixerInit();
+
+    if(old_platform_type!=mixerConfig()->platformType)
+    {
+        pidInit();
+        pidInitFilters();
+        schedulePidGainsUpdate();
+        navigationInit();
+    }
+    return true;
+}
+
 int min_ab(int a,int b)
 {
     return a > b ? b : a;
 }
 
-bool OutputProfileHotSwitch(int profile_index)
+void checkOutputMapping(int profile_index)
 {
-#ifdef ENABLE_MIXER_PROFILE_HOTSWAP
-    // does not work with timerHardwareOverride
-    LOG_INFO(PWM, "OutputProfileHotSwitch");
-
-    //store current output for check
-    uint8_t old_platform_type=mixerConfig()->platformType;
     timMotorServoHardware_t old_timOutputs;
     pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos());
-    if (!setConfigMixerProfile(profile_index)){
-        return false;
-    }
     stopMotors();
-    // delay(3000); //check motor stop
+    delay(1000); //check motor stop
+    if (!setConfigMixerProfile(profile_index)){
+        LOG_INFO(PWM, "failed to set config");
+        return;
+    }
     servosInit();
     mixerUpdateStateFlags();
     mixerInit();
-
+    timMotorServoHardware_t timOutputs;
-    // timMotorServoHardware_t timOutputs;
+    pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
-    // pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos());
+    bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount;
-    // bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount;
+    bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount;
-    // bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount;
+    LOG_INFO(PWM, "maxTimMotorCount:%d,%d",old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount);
-    // LOG_INFO(PWM, "motor_output_type_not_changed:%d,%d,%d",motor_output_type_not_changed,old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount);
+    for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++)
-    // for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++)
-    // {
-    //     LOG_INFO(PWM, "motor_output_type_not_changed:%d,%d",i,motor_output_type_not_changed);
-    //     motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag;
-    // }
-    // LOG_INFO(PWM, "motor_output_type_not_changed:%d",motor_output_type_not_changed);
-
-    // LOG_INFO(PWM, "servo_output_type_not_changed:%d,%d,%d",servo_output_type_not_changed,old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount);
-    // for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++)
-    // {
-    //     LOG_INFO(PWM, "servo_output_type_not_changed:%d,%d",i,servo_output_type_not_changed);
-    //     servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag;
-    // }
-    // LOG_INFO(PWM, "servo_output_type_not_changed:%d",servo_output_type_not_changed);
-
-    // if(!motor_output_type_not_changed || !servo_output_type_not_changed){
-    //     LOG_INFO(PWM, "pwmMotorAndServoHotInit");
-    //     pwmMotorAndServoHotInit(&timOutputs);
-    // }
-    if(old_platform_type!=mixerConfig()->platformType)
     {
-        
+        LOG_INFO(PWM, "motor_output_type_not_changed:%d,%d",i,motor_output_type_not_changed);
+        motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag;
+    }
+    LOG_INFO(PWM, "motor_output_type_not_changed:%d",motor_output_type_not_changed);
+
+    LOG_INFO(PWM, "maxTimServoCount:%d,%d",old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount);
+    for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++)
+    {
+        LOG_INFO(PWM, "servo_output_type_not_changed:%d,%d",i,servo_output_type_not_changed);
+        servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag;
+    }
+    LOG_INFO(PWM, "servo_output_type_not_changed:%d",servo_output_type_not_changed);
+
+    if(!motor_output_type_not_changed || !servo_output_type_not_changed){
+        LOG_INFO(PWM, "pwm output mapping has changed");
     }
-    return true;
-#else
-    profile_index=0; //prevent compilier warning: parameter 'profile_index' set but not used
-    return (bool) profile_index;
-#endif
 }
 
+

src/main/flight/mixer_profile.h

@@ -22,4 +22,9 @@ PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles);
 #define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index])
 #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index))
 
+static inline const mixerProfile_t* mixerProfiles_CopyArray_macro(int _index) { return &mixerProfiles_CopyArray[_index]; }
+#define primaryMotorMixer_CopyArray() (mixerProfiles_CopyArray_macro(systemConfig()->current_mixer_profile_index)->MotorMixers)
+
+#define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config))
+
 bool OutputProfileHotSwitch(int profile_index);

src/main/navigation/navigation.c

@@ -4263,6 +4263,13 @@ bool navigationIsExecutingAnEmergencyLanding(void)
     return navGetCurrentStateFlags() & NAV_CTL_EMERG;
 }
 
+
+bool navigationInAnyMode(void)
+{
+    navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();
+    return !(stateFlags ==0);
+}
+
 bool navigationInAutomaticThrottleMode(void)
 {
     navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags();

src/main/navigation/navigation.h

@@ -582,6 +582,7 @@ void abortForcedEmergLanding(void);
 emergLandState_e getStateOfForcedEmergLanding(void);
 
 /* Getter functions which return data about the state of the navigation system */
+bool navigationInAnyMode(void);
 bool navigationInAutomaticThrottleMode(void);
 bool navigationIsControllingThrottle(void);
 bool isFixedWingAutoThrottleManuallyIncreased(void);

src/main/programming/logic_condition.c

@@ -369,10 +369,7 @@ static int logicConditionCompute(
         case LOGIC_CONDITION_SET_MIXER_PROFILE:
             operandA--;
             if ( getConfigMixerProfile() != operandA  && (operandA >= 0 && operandA < MAX_MIXER_PROFILE_COUNT)) {
-                bool mixerprofileChanged = false;
+                bool mixerprofileChanged = OutputProfileHotSwitch(operandA);
-                if (OutputProfileHotSwitch(operandA)) {
-                    mixerprofileChanged = true;
-                }
                 return mixerprofileChanged;
             } else {
                 return false;

src/main/target/MATEKF405/target.h

@@ -197,5 +197,5 @@
 #define MAX_PWM_OUTPUT_PORTS       6
 
 #ifdef MATEKF405VTOL
-#define ENABLE_MIXER_PROFILE_HOTSWAP
+#define ENABLE_MCFW_MIXER_PROFILE_HOTSWAP
 #endif