mixer and pid profile linking

src/main/fc/config.c

@@ -332,6 +332,9 @@ void readEEPROM(void)
     setConfigProfile(getConfigProfile());
     setConfigBatteryProfile(getConfigBatteryProfile());
     setConfigMixerProfile(getConfigMixerProfile());
+    if(mixerConfig()->PIDProfileLinking){
+        setConfigProfile(getConfigMixerProfile());
+    }
 
     validateAndFixConfig();
     activateConfig();
@@ -494,8 +497,10 @@ void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex)
 {
     if (setConfigMixerProfile(profileIndex)) {
         // profile has changed, so ensure current values saved before new profile is loaded
+        suspendRxSignal();
         writeEEPROM();
         readEEPROM();
+        resumeRxSignal();
     }
     beeperConfirmationBeeps(profileIndex + 1);
 }

src/main/fc/config.h

@@ -36,7 +36,7 @@ typedef enum {
     FEATURE_VBAT = 1 << 1,
     FEATURE_TX_PROF_SEL = 1 << 2,       // Profile selection by TX stick command
     FEATURE_BAT_PROFILE_AUTOSWITCH = 1 << 3,
-    FEATURE_UNUSED_12 = 1 << 4,
+    FEATURE_UNUSED_12 = 1 << 4,  //was FEATURE_MOTOR_STOP
     FEATURE_UNUSED_1 = 1 << 5,   // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS
     FEATURE_SOFTSERIAL = 1 << 6,
     FEATURE_GPS = 1 << 7,

src/main/fc/settings.yaml

@@ -1134,7 +1134,7 @@ groups:
 
   - name: PG_MIXER_PROFILE
     type: mixerProfile_t
-    headers: ["flight/mixer.h","flight/mixer_profile.h"]
+    headers: ["flight/mixer_profile.h"]
     value_type: MIXER_CONFIG_VALUE
     members:
       - name: motor_direction_inverted
@@ -1169,6 +1169,11 @@ groups:
         default_value: OFF
         field: mixer_config.motorstopFeature
         type: bool
+      - name: mixer_pid_profile_linking
+        description: "If enabled, pid profile index will follow mixer profile index"
+        default_value: OFF
+        field: mixer_config.PIDProfileLinking
+        type: bool
 
   - name: PG_REVERSIBLE_MOTORS_CONFIG
     type: reversibleMotorsConfig_t

src/main/flight/mixer.c

@@ -426,10 +426,15 @@ void writeAllMotors(int16_t mc)
     writeMotors();
 }
 
-void stopMotors(void)
+
+void stopMotorsNoDelay(void)
 {
     writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);
-
+}
+void stopMotors(void)
+{
+    stopMotorsNoDelay();
+    
     delay(50); // give the timers and ESCs a chance to react.
 }
 

src/main/flight/mixer.h

@@ -62,15 +62,6 @@ typedef struct motorMixer_s {
     float yaw;
 } motorMixer_t;
 
-typedef struct mixerConfig_s {
-    int8_t motorDirectionInverted;
-    uint8_t platformType;
-    bool hasFlaps;
-    int16_t appliedMixerPreset;
-    uint8_t outputMode;
-    bool motorstopFeature;
-} mixerConfig_t;
-
 typedef struct reversibleMotorsConfig_s {
     uint16_t deadband_low;                // min 3d value
     uint16_t deadband_high;               // max 3d value
@@ -125,6 +116,7 @@ void processServoAutotrim(const float dT);
 void processServoAutotrimMode(void);
 void processContinuousServoAutotrim(const float dT);
 void stopMotors(void);
+void stopMotorsNoDelay(void);
 void stopPwmAllMotors(void);
 
 void loadPrimaryMotorMixer(void);

src/main/flight/mixer_profile.c

@@ -37,6 +37,8 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance)
                 .hasFlaps = SETTING_HAS_FLAPS_DEFAULT,
                 .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only
                 .outputMode = SETTING_OUTPUT_MODE_DEFAULT,
+                .motorstopFeature = SETTING_MOTORSTOP_FEATURE_DEFAULT,
+                .PIDProfileLinking = SETTING_OUTPUT_MODE_DEFAULT
             }
         );
         for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) {
@@ -108,29 +110,17 @@ static int computeServoCountByMixerProfileIndex(int index)
     }
 }
 
-//pid init will be done by the following pid profile change
+void SwitchPidProfileByMixerProfile()
-static bool CheckIfPidInitNeededInSwitch(void)
 {
-    static bool ret = true;
+    LOG_INFO(PWM, "mixer switch pidInit");
-    if (!ret)
+    setConfigProfile(getConfigMixerProfile());
-    {
+    pidInit();
-        return false;
+    pidInitFilters();
-    }
+    schedulePidGainsUpdate();
-    for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++)
+    navigationUsePIDs(); //set navigation pid gains
-    {
-        const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
-        const logicOperand_t *operandA = &(logicConditions(i)->operandA);
-        if (logicConditions(i)->enabled && activatorValue && logicConditions(i)->operation == LOGIC_CONDITION_SET_PROFILE &&
-            operandA->type == LOGIC_CONDITION_OPERAND_TYPE_FLIGHT && operandA->value == LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE &&
-            logicConditions(i)->flags == 0)
-        {
-            ret = false;
-            return false;
-        }
-    }
-    return true;
 }
 
+//switch mixerprofile without reboot
 bool OutputProfileHotSwitch(int profile_index)
 {
     // does not work with timerHardwareOverride
@@ -144,7 +134,7 @@ bool OutputProfileHotSwitch(int profile_index)
     {
         return false;
     }
-    if (areSensorsCalibrating()) {//it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D
+    if (areSensorsCalibrating()) {//it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D,TODO
         return false;
     }
     //do not allow switching in navigation mode
@@ -154,16 +144,16 @@ bool OutputProfileHotSwitch(int profile_index)
     }
     //do not allow switching between multi rotor and non multi rotor
 #ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP
-    bool MCFW_hotswap_unavailable = false;
+    bool MCFW_hotswap_available = true;
 #else
-    bool MCFW_hotswap_unavailable = true;
+    bool MCFW_hotswap_available = false;
 #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)
+    if ((!MCFW_hotswap_available) && is_mcfw_switching)
     {
         LOG_INFO(PWM, "mixer MCFW_hotswap_unavailable");
         return false;
@@ -171,7 +161,7 @@ bool OutputProfileHotSwitch(int profile_index)
     //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, motor/servo count will change");
+        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));
         return false;
@@ -180,23 +170,14 @@ bool OutputProfileHotSwitch(int profile_index)
         LOG_INFO(PWM, "mixer switch failed to set config");
         return false;
     }
-    // stopMotors();
+    stopMotorsNoDelay();
-    writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors without delay
     servosInit();
     mixerUpdateStateFlags();
     mixerInit();
 
-    if(old_platform_type!=mixerConfig()->platformType)
+    if(mixerConfig()->PIDProfileLinking)
     {
-        navigationYawControlInit();
+        SwitchPidProfileByMixerProfile();
-        if (CheckIfPidInitNeededInSwitch())
-        {
-            LOG_INFO(PWM, "mixer switch pidInit");
-            pidInit();
-            pidInitFilters();
-            schedulePidGainsUpdate();
-            navigationUsePIDs();
-        }
     }
     return true;
 }
@@ -244,4 +225,25 @@ bool OutputProfileHotSwitch(int profile_index)
 //     }
 // }
 
-
+//check if a pid profile switch followed on a mixer profile switch
+// static bool CheckIfPidInitNeededInSwitch(void)
+// {
+//     static bool ret = true;
+//     if (!ret)
+//     {
+//         return false;
+//     }
+//     for (uint8_t i = 0; i < MAX_LOGIC_CONDITIONS; i++)
+//     {
+//         const int activatorValue = logicConditionGetValue(logicConditions(i)->activatorId);
+//         const logicOperand_t *operandA = &(logicConditions(i)->operandA);
+//         if (logicConditions(i)->enabled && activatorValue && logicConditions(i)->operation == LOGIC_CONDITION_SET_PROFILE &&
+//             operandA->type == LOGIC_CONDITION_OPERAND_TYPE_FLIGHT && operandA->value == LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE &&
+//             logicConditions(i)->flags == 0)
+//         {
+//             ret = false;
+//             return false;
+//         }
+//     }
+//     return true;
+// }

src/main/flight/mixer_profile.h

@@ -9,6 +9,15 @@
 #define MAX_MIXER_PROFILE_COUNT 2
 #endif
 
+typedef struct mixerConfig_s {
+    int8_t motorDirectionInverted;
+    uint8_t platformType;
+    bool hasFlaps;
+    int16_t appliedMixerPreset;
+    uint8_t outputMode;
+    bool motorstopFeature;
+    bool PIDProfileLinking;
+} mixerConfig_t;
 typedef struct mixerProfile_s {
     mixerConfig_t mixer_config;
     motorMixer_t MotorMixers[MAX_SUPPORTED_MOTORS];

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_SOURCE_COUNT
 } inputSource_e;
 

src/main/navigation/navigation.c

@@ -4177,7 +4177,7 @@ void navigationUsePIDs(void)
     );
 }
 
-void navigationYawControlInit(void){
+void navigationInitYawControl(void){
     if (
         mixerConfig()->platformType == PLATFORM_BOAT ||
         mixerConfig()->platformType == PLATFORM_ROVER ||
@@ -4222,8 +4222,16 @@ void navigationInit(void)
 
     /* Use system config */
     navigationUsePIDs();
-    navigationYawControlInit();
+
-    
+    if (
+        mixerConfig()->platformType == PLATFORM_BOAT ||
+        mixerConfig()->platformType == PLATFORM_ROVER ||
+        navConfig()->fw.useFwNavYawControl
+    ) {
+        ENABLE_STATE(FW_HEADING_USE_YAW);
+    } else {
+        DISABLE_STATE(FW_HEADING_USE_YAW);
+    }
 #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE)
     /* configure WP missions at boot */
 #ifdef USE_MULTI_MISSION

src/main/navigation/navigation.h

@@ -479,7 +479,7 @@ typedef struct {
 } navSystemStatus_t;
 
 void navigationUsePIDs(void);
-void navigationYawControlInit(void);
+void navigationInitYawControl(void);
 void navigationInit(void);
 
 /* Position estimator update functions */

src/main/programming/logic_condition.c

@@ -425,7 +425,7 @@ static int logicConditionCompute(
                     pidInit();
                     pidInitFilters();
                     schedulePidGainsUpdate();
-                    navigationUsePIDs();
+                    navigationUsePIDs(); //set navigation pid gains
                     profileChanged = true;
                 }
                 return profileChanged;
@@ -434,16 +434,6 @@ static int logicConditionCompute(
             }
             break;
 
-        case LOGIC_CONDITION_SET_MIXER_PROFILE:
-            operandA--;
-            if ( getConfigMixerProfile() != operandA  && (operandA >= 0 && operandA < MAX_MIXER_PROFILE_COUNT)) {
-                bool mixerprofileChanged = OutputProfileHotSwitch(operandA);
-                return mixerprofileChanged;
-            } else {
-                return false;
-            }
-            break;
-
         case LOGIC_CONDITION_LOITER_OVERRIDE:
             logicConditionValuesByType[LOGIC_CONDITION_LOITER_OVERRIDE] = constrain(operandA, 0, 100000);
             LOGIC_CONDITION_GLOBAL_FLAG_ENABLE(LOGIC_CONDITION_GLOBAL_FLAG_OVERRIDE_LOITER_RADIUS);

src/main/programming/logic_condition.h

@@ -135,6 +135,7 @@ typedef enum {
     LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2                      // 35
     LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2                             // 36
     LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int                   // 37
+    LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int              // TBD
 } logicFlightOperands_e;
 
 typedef enum {

src/main/target/MATEKF405TE/target.h

@@ -183,4 +183,5 @@
 
 #define USE_SERIAL_4WAY_BLHELI_INTERFACE
 #define USE_DSHOT
-#define USE_ESC_SENSOR
+#define USE_ESC_SENSOR
+

src/main/target/common.h

@@ -187,5 +187,5 @@
 #define USE_SERIALRX_SUMD
 #define USE_TELEMETRY_HOTT
 #define USE_HOTT_TEXTMODE
-
+#define MAX_MIXER_PROFILE_COUNT 1
 #endif