From fe30a8a8ce1883b532607e6ca977190b5e39828e Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 23 Jul 2023 00:24:27 +0900 Subject: [PATCH] refactoring --- src/main/fc/fc_init.c | 1 + src/main/fc/fc_msp_box.c | 4 +- src/main/fc/settings.yaml | 5 +- src/main/flight/mixer.c | 4 +- src/main/flight/mixer_profile.c | 157 +++++++++++-------------------- src/main/flight/mixer_profile.h | 4 +- src/main/flight/servos.c | 2 +- src/main/navigation/navigation.h | 1 - src/main/target/common.h | 1 + 9 files changed, 65 insertions(+), 114 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index d04defd3e95..f64edd4d021 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -304,6 +304,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 mixerConfigInit(); + checkMixerProfileHotSwitchAvalibility(); // Some sanity checking if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index c6ad2cfc0ee..98c0e878909 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -421,8 +421,8 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCHANGEMISSION)), BOXCHANGEMISSION); #endif #if (MAX_MIXER_PROFILE_COUNT > 1) - CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex)); - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION))); + CHECK_ACTIVE_BOX(IS_ENABLED(currentMixerProfileIndex), BOXMIXERPROFILE); + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION)), BOXMIXERTRANSITION); #endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9becd267704..9064ef63ad0 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -203,7 +203,6 @@ constants: MAX_CONTROL_RATE_PROFILE_COUNT: 3 MAX_BATTERY_PROFILE_COUNT: 3 - MAX_MIXER_PROFILE_COUNT: 2 groups: @@ -1164,10 +1163,10 @@ groups: default_value: "AUTO" field: mixer_config.outputMode table: output_mode - - name: motorstop_feature + - name: motorstop_on_low description: "If enabled, motor will stop when throttle is low" default_value: OFF - field: mixer_config.motorstopFeature + field: mixer_config.motorstopOnLow type: bool - name: mixer_pid_profile_linking description: "If enabled, pid profile index will follow mixer profile index" diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6a97ab4e17d..b77e39c0b88 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -116,7 +116,7 @@ static void computeMotorCount(void) } bool ifMotorstopFeatureEnabled(void){ - return currentMixerConfig.motorstopFeature; + return currentMixerConfig.motorstopOnLow; } uint8_t getMotorCount(void) { @@ -598,7 +598,7 @@ void FAST_CODE mixTable(void) 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))) { + if (isInMixerTransition && 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); } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index d92761f7665..c6864094cc0 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -28,6 +28,7 @@ mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; +bool isInMixerTransition; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); @@ -41,8 +42,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 + .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, + .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT } ); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { @@ -105,10 +106,6 @@ static int computeServoCountByMixerProfileIndex(int index) const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; for (int i = 0; i < MAX_SERVO_RULES; i++) { - // mixerServoMixersByIndex(index)[i]->targetChannel will occour problem after i=1 - // LOG_INFO(PWM, "i:%d, targetChannel:%d, inputSource:%d, rate:%d",i,mixerServoMixersByIndex(index)[i]->targetChannel,mixerServoMixersByIndex(index)[i]->inputSource,mixerServoMixersByIndex(index)[i]->rate); - // LOG_INFO(PWM, "i:%d, targetChannel:%d, inputSource:%d, rate:%d",i,mixerProfiles_SystemArray[index].ServoMixers[i].targetChannel,mixerProfiles_SystemArray[index].ServoMixers[i].inputSource,mixerProfiles_SystemArray[index].ServoMixers[i].rate); - // LOG_INFO(PWM, "i:%d, targetChannel:%d, inputSource:%d, rate:%d",i,temp_servomixers[i].targetChannel,temp_servomixers[i].inputSource,temp_servomixers[i].rate); if (temp_servomixers[i].rate == 0) break; @@ -130,9 +127,51 @@ static int computeServoCountByMixerProfileIndex(int index) } } +bool checkMixerProfileHotSwitchAvalibility(void) +{ + static int allow_hot_switch = -1; + //pwm mapping maps 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 + if (MAX_MIXER_PROFILE_COUNT!=2) + { + return false; + } + if (allow_hot_switch == 0) + { + return false; + } + if (allow_hot_switch == 1) + { + return true; + } +#ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP + bool MCFW_hotswap_available = true; +#else + bool MCFW_hotswap_available = false; +#endif + uint8_t platform_type0 = mixerConfigByIndex(0)->platformType; + uint8_t platform_type1 = mixerConfigByIndex(1)->platformType; + bool platform_type_mc0 = (platform_type0 == PLATFORM_MULTIROTOR) || (platform_type0 == PLATFORM_TRICOPTER); + bool platform_type_mc1 = (platform_type1 == PLATFORM_MULTIROTOR) || (platform_type1 == PLATFORM_TRICOPTER); + bool is_mcfw_switching = platform_type_mc0 ^ platform_type_mc1; + if ((!MCFW_hotswap_available) && is_mcfw_switching) + { + allow_hot_switch = 0; + return false; + } + //do not allow switching if motor or servos counts are different + if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) + { + allow_hot_switch = 0; + return false; + } + allow_hot_switch = 1; + return true; +} void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); + isInMixerTransition = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); outputProfileHotSwitch((int) IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); } @@ -153,117 +192,27 @@ bool outputProfileHotSwitch(int profile_index) } if (profile_index < 0 || profile_index >= MAX_MIXER_PROFILE_COUNT) { // sanity check - LOG_INFO(PWM, "invalid mixer profile index"); + // LOG_INFO(PWM, "invalid mixer profile index"); return false; } 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 - if (ARMING_FLAG(ARMED) && (navigationInAnyMode() || isUsingNavigationModes())){ - LOG_INFO(PWM, "mixer switch failed, navModesEnabled"); + // do not allow switching when user activated navigation mode + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE))); + if (ARMING_FLAG(ARMED) && navBoxModesEnabled){ + // LOG_INFO(PWM, "mixer switch failed, navModesEnabled"); return false; } - //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 = 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; - bool is_mcfw_switching = old_platform_type_mc ^ new_platform_type_mc; - 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; + if (!checkMixerProfileHotSwitchAvalibility()){ + // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); return false; } if (!setConfigMixerProfile(profile_index)){ - LOG_INFO(PWM, "mixer switch failed to set config"); + // LOG_INFO(PWM, "mixer switch failed to set config"); return false; } stopMotorsNoDelay(); mixerConfigInit(); - return true; } - -// static int min_ab(int a,int b) -// { -// return a > b ? b : a; -// } - -// void checkOutputMapping(int profile_index)//debug purpose -// { -// timMotorServoHardware_t old_timOutputs; -// pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); -// stopMotors(); -// delay(1000); //check motor stop -// if (!setConfigMixerProfile(profile_index)){ -// LOG_INFO(PWM, "failed to set config"); -// return; -// } -// servosInit(); -// mixerUpdateStateFlags(); -// mixerInit(); -// timMotorServoHardware_t timOutputs; -// pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); -// bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount; -// bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount; -// LOG_INFO(PWM, "maxTimMotorCount:%d,%d",old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); -// 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, "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"); -// } -// } - -//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; -// } \ No newline at end of file diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 4302c60b40e..25d4e793625 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -15,7 +15,7 @@ typedef struct mixerConfig_s { bool hasFlaps; int16_t appliedMixerPreset; uint8_t outputMode; - bool motorstopFeature; + bool motorstopOnLow; bool PIDProfileLinking; } mixerConfig_t; typedef struct mixerProfile_s { @@ -28,6 +28,7 @@ PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; +extern bool isInMixerTransition; #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) @@ -45,5 +46,6 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) #define mixerServoMixersByIndex(index) (&(mixerProfiles(index)->ServoMixers)) bool outputProfileHotSwitch(int profile_index); +bool checkMixerProfileHotSwitchAvalibility(void); void mixerConfigInit(void); void outputProfileUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index f46619753a4..c08ee293eeb 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -317,7 +317,7 @@ 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 + input[INPUT_MIXER_TRANSITION] = isInMixerTransition * 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: diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 57d7f49c8e3..b6ff19f7116 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -590,7 +590,6 @@ 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); diff --git a/src/main/target/common.h b/src/main/target/common.h index 70916fdbd30..b56c242675e 100644 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -187,5 +187,6 @@ #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE +#else #define MAX_MIXER_PROFILE_COUNT 1 #endif