Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-25 17:25:18 +03:00
Code Issues Wiki Activity

refactoring

Browse source
This commit is contained in:
shota 2023-07-23 00:24:27 +09:00
parent 1273d862aa
commit fe30a8a8ce
9 changed files with 65 additions and 114 deletions
Download patch file Download diff file Expand all files Collapse all files

1
src/main/fc/fc_init.c
View file

@ -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) {

4
src/main/fc/fc_msp_box.c
View file

@ -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++) {

5
src/main/fc/settings.yaml
View file

@ -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"

4
src/main/flight/mixer.c
View file

@ -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);
}

157
src/main/flight/mixer_profile.c
View file

@ -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;
// }

4
src/main/flight/mixer_profile.h
View file

@ -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);

2
src/main/flight/servos.c
View file

@ -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:

1
src/main/navigation/navigation.h
View file

@ -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);

1
src/main/target/common.h
View file

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