From 0b247d9bfc18e092480dd2d3405f17585ebc86c7 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 6 Nov 2022 02:28:52 +0900 Subject: [PATCH 01/60] mixer profile switching by cli --- src/main/cms/cms_menu_mixer_servo.c | 1 + src/main/config/parameter_group_ids.h | 2 +- src/main/fc/cli.c | 29 ++++++++++++++++++ src/main/fc/config.c | 30 +++++++++++++++++++ src/main/fc/config.h | 5 ++++ src/main/fc/fc_msp.c | 5 ++++ src/main/fc/settings.c | 2 ++ src/main/fc/settings.h | 7 +++-- src/main/fc/settings.yaml | 5 ++-- src/main/flight/mixer.c | 35 +++++++++++++++++----- src/main/flight/mixer.h | 19 +++++++++--- src/test/unit/flight_mixer_unittest.cc.txt | 2 +- 12 files changed, 123 insertions(+), 19 deletions(-) diff --git a/src/main/cms/cms_menu_mixer_servo.c b/src/main/cms/cms_menu_mixer_servo.c index 92d18ab3a0a..7c876d7150d 100644 --- a/src/main/cms/cms_menu_mixer_servo.c +++ b/src/main/cms/cms_menu_mixer_servo.c @@ -40,6 +40,7 @@ #include "fc/runtime_config.h" #include "fc/settings.h" +#include "fc/config.h" static uint8_t currentMotorMixerIndex = 0; static uint8_t tmpcurrentMotorMixerIndex = 1; diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 46293524b14..754b7dbadae 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -36,7 +36,7 @@ //#define PG_TRANSPONDER_CONFIG 17 #define PG_SYSTEM_CONFIG 18 #define PG_FEATURE_CONFIG 19 -#define PG_MIXER_CONFIG 20 +#define PG_MIXER_PROFILE 20 #define PG_SERVO_MIXER 21 #define PG_IMU_CONFIG 22 //#define PG_PROFILE_SELECTION 23 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 6dd129b5c36..b6450593a81 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3049,6 +3049,33 @@ static void cliDumpBatteryProfile(uint8_t profileIndex, uint8_t dumpMask) dumpAllValues(BATTERY_CONFIG_VALUE, dumpMask); } +static void cliMixerProfile(char *cmdline) +{ + // CLI profile index is 1-based + if (isEmpty(cmdline)) { + cliPrintLinef("mixer_profile %d", getConfigMixerProfile() + 1); + return; + } else { + const int i = fastA2I(cmdline) - 1; + if (i >= 0 && i < MAX_PROFILE_COUNT) { + setConfigMixerProfileAndWriteEEPROM(i); + cliMixerProfile(""); + } + } +} + +static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask) +{ + if (profileIndex >= MAX_MIXER_PROFILE_COUNT) { + // Faulty values + return; + } + setConfigMixerProfile(profileIndex); + cliPrintHashLine("mixer_profile"); + cliPrintLinef("mixer_profile %d\r\n", getConfigMixerProfile() + 1); + dumpAllValues(MIXER_CONFIG_VALUE, dumpMask); +} + #ifdef USE_CLI_BATCH static void cliPrintCommandBatchWarning(const char *warning) { @@ -3861,6 +3888,8 @@ const clicmd_t cmdTable[] = { "[]", cliProfile), CLI_COMMAND_DEF("battery_profile", "change battery profile", "[]", cliBatteryProfile), + CLI_COMMAND_DEF("mixer_profile", "change mixer profile", + "[]", cliMixerProfile), CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), #if defined(USE_SAFE_HOME) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index b8f665598dd..1a7ced85157 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -107,6 +107,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, .current_battery_profile_index = 0, + .current_mixer_profile_index = 0, .debug_mode = SETTING_DEBUG_MODE_DEFAULT, #ifdef USE_DEV_TOOLS .groundTestMode = SETTING_GROUND_TEST_MODE_DEFAULT, // disables motors, set heading trusted for FW (for dev use) @@ -417,6 +418,35 @@ void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex) beeperConfirmationBeeps(profileIndex + 1); } +uint8_t getConfigMixerProfile(void) +{ + return systemConfig()->current_mixer_profile_index; +} + +bool setConfigMixerProfile(uint8_t profileIndex) +{ + bool ret = true; // return true if current_mixer_profile_index has changed + if (systemConfig()->current_mixer_profile_index == profileIndex) { + ret = false; + } + if (profileIndex >= MAX_MIXER_PROFILE_COUNT) {// sanity check + profileIndex = 0; + } + systemConfigMutable()->current_mixer_profile_index = profileIndex; + // setMixerProfile(profileIndex); + return ret; +} + +void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex) +{ + if (setConfigMixerProfile(profileIndex)) { + // profile has changed, so ensure current values saved before new profile is loaded + writeEEPROM(); + readEEPROM(); + } + beeperConfirmationBeeps(profileIndex + 1); +} + void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]) { gyroConfigMutable()->gyro_zero_cal[X] = getGyroZero[X]; gyroConfigMutable()->gyro_zero_cal[Y] = getGyroZero[Y]; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 57dcbe3726b..ab5a6fff721 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -69,6 +69,7 @@ typedef enum { typedef struct systemConfig_s { uint8_t current_profile_index; uint8_t current_battery_profile_index; + uint8_t current_mixer_profile_index; uint8_t debug_mode; #ifdef USE_DEV_TOOLS bool groundTestMode; // Disables motor ouput, sets heading trusted on FW (for dev use) @@ -135,6 +136,10 @@ uint8_t getConfigBatteryProfile(void); bool setConfigBatteryProfile(uint8_t profileIndex); void setConfigBatteryProfileAndWriteEEPROM(uint8_t profileIndex); +uint8_t getConfigMixerProfile(void); +bool setConfigMixerProfile(uint8_t profileIndex); +void setConfigMixerProfileAndWriteEEPROM(uint8_t profileIndex); + void setGyroCalibrationAndWriteEEPROM(int16_t getGyroZero[XYZ_AXIS_COUNT]); void setGravityCalibrationAndWriteEEPROM(float getGravity); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 524fc6319bf..56c716ade14 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3126,7 +3126,12 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, getConfigBatteryProfile()); sbufWriteU8(dst, MAX_BATTERY_PROFILE_COUNT); break; + case MIXER_CONFIG_VALUE: + sbufWriteU8(dst, getConfigMixerProfile()); + sbufWriteU8(dst, MAX_MIXER_PROFILE_COUNT); + break; } + // If the setting uses a table, send each possible string (null terminated) if (SETTING_MODE(setting) == MODE_LOOKUP) { diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8dc17edc631..200921bf869 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -239,6 +239,8 @@ static uint16_t getValueOffset(const setting_t *value) return value->offset + sizeof(controlRateConfig_t) * getConfigProfile(); case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); + case MIXER_CONFIG_VALUE: + return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); } return 0; } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index b14289ba27c..16c47d766bc 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -15,7 +15,7 @@ typedef struct lookupTableEntry_s { #define SETTING_TYPE_OFFSET 0 #define SETTING_SECTION_OFFSET 4 -#define SETTING_MODE_OFFSET 6 +#define SETTING_MODE_OFFSET 7 typedef enum { // value type, bits 0-3 @@ -29,15 +29,16 @@ typedef enum { } setting_type_e; typedef enum { - // value section, bits 4-5 + // value section, bits 4-6 MASTER_VALUE = (0 << SETTING_SECTION_OFFSET), PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), + MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET), } setting_section_e; typedef enum { - // value mode, bits 6-7 + // value mode, bits 7 MODE_DIRECT = (0 << SETTING_MODE_OFFSET), MODE_LOOKUP = (1 << SETTING_MODE_OFFSET), // 0x40 } setting_mode_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 0af6fe63e77..c96ab3db477 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -206,6 +206,7 @@ constants: MAX_CONTROL_RATE_PROFILE_COUNT: 3 MAX_BATTERY_PROFILE_COUNT: 3 + MAX_MIXER_PROFILE_COUNT: 2 groups: @@ -1145,8 +1146,8 @@ groups: field: powerLimits.burstPowerFalldownTime max: 3000 - - name: PG_MIXER_CONFIG - type: mixerConfig_t + - name: PG_MIXER_PROFILE + type: mixerProfile_t members: - name: motor_direction_inverted description: "Use if you need to inverse yaw motor direction." diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ee4cf4ef544..fb9934a37bf 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -33,6 +33,7 @@ FILE_COMPILE_FOR_SPEED #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "config/config_reset.h" #include "drivers/pwm_output.h" #include "drivers/pwm_mapping.h" @@ -82,16 +83,34 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 5); +void pgResetFn_mixerProfiles(mixerProfile_t *instance) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + RESET_CONFIG(mixerProfile_t, &instance[i], + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .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, + ); + motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; + for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { + instance->MotorMixer[j] = tmp_mixer; + } + } +} -PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, - .platformType = SETTING_PLATFORM_TYPE_DEFAULT, - .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, -); +// PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_PROFILE, 5); + +// PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, +// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, +// .platformType = SETTING_PLATFORM_TYPE_DEFAULT, +// .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, +// ); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index cfb8fb9a603..fa560af504c 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,12 +19,17 @@ #include "config/parameter_group.h" +#ifndef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 +#endif + #if defined(TARGET_MOTOR_COUNT) #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT #else #define MAX_SUPPORTED_MOTORS 12 #endif + // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -64,15 +69,21 @@ typedef struct motorMixer_s { PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); -typedef struct mixerConfig_s { +typedef struct mixerProfile_s { int8_t motorDirectionInverted; uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; uint8_t outputMode; -} mixerConfig_t; + motorMixer_t MotorMixer[MAX_SUPPORTED_MOTORS]; +} mixerProfile_t; -PG_DECLARE(mixerConfig_t, mixerConfig); +PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); +#define mixerConfig() mixerProfiles(systemConfig()->current_mixer_profile_index) +#define mixerConfigMutable() ((mixerProfile_t *)mixerConfig()) +#define primaryMotorMixer(_index) (&((mixerConfig()->MotorMixer)[_index])) +#define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) +extern motorMixer_t primaryMotorMixer_CopyArray[12]; typedef struct reversibleMotorsConfig_s { uint16_t deadband_low; // min 3d value @@ -130,4 +141,4 @@ void stopMotors(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); +bool areMotorsRunning(void); \ No newline at end of file diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index 6b33b6c50c5..c0915b3cc27 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -144,7 +144,7 @@ TEST_F(ChannelForwardingTest, TestForwardAuxChannelsToServosWithLessRemainingSer class BasicMixerIntegrationTest : public ::testing::Test { protected: - mixerConfig_t mixerConfig; + mixerProfile_t mixerConfig; rxConfig_t rxConfig; escAndServoConfig_t escAndServoConfig; servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; From fcf9ca14dea85f68c13ab06734f9a87db8784637 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 6 Nov 2022 16:40:16 +0900 Subject: [PATCH 02/60] add mixer profile switching by logic --- src/main/fc/cli.c | 2 +- src/main/fc/settings.c | 2 +- src/main/programming/logic_condition.c | 30 ++++++++++++++++++++++++++ src/main/programming/logic_condition.h | 4 +++- 4 files changed, 35 insertions(+), 3 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index b6450593a81..d32cf9f610c 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3057,7 +3057,7 @@ static void cliMixerProfile(char *cmdline) return; } else { const int i = fastA2I(cmdline) - 1; - if (i >= 0 && i < MAX_PROFILE_COUNT) { + if (i >= 0 && i < MAX_MIXER_PROFILE_COUNT) { setConfigMixerProfileAndWriteEEPROM(i); cliMixerProfile(""); } diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 200921bf869..8ea69b82f09 100644 --- a/src/main/fc/settings.c +++ b/src/main/fc/settings.c @@ -240,7 +240,7 @@ static uint16_t getValueOffset(const setting_t *value) case BATTERY_CONFIG_VALUE: return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); case MIXER_CONFIG_VALUE: - return value->offset + sizeof(batteryProfile_t) * getConfigBatteryProfile(); + return value->offset + sizeof(mixerProfile_t) * getConfigMixerProfile(); } return 0; } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 990e9c1510f..08cb452b645 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -50,6 +50,10 @@ #include "io/osd_common.h" #include "sensors/diagnostics.h" +#include "flight/mixer.h" +#include "flight/servos.h" +#include "drivers/pwm_mapping.h" + #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -361,6 +365,28 @@ static int logicConditionCompute( } break; + case LOGIC_CONDITION_SET_MIXER_PROFILE: + operandA--; + if ( getConfigMixerProfile() != operandA && (operandA >= 0 && operandA < MAX_MIXER_PROFILE_COUNT)) { + bool mixerprofileChanged = false; + if (setConfigMixerProfile(operandA)) { + stopMotors(); + stopPwmAllMotors(); + servosInit(); + mixerUpdateStateFlags(); + mixerInit(); + pwmMotorAndServoInit(); + if (!STATE(ALTITUDE_CONTROL)) { + featureClear(FEATURE_AIRMODE); + } + mixerprofileChanged = true; + } + 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); @@ -610,6 +636,10 @@ static int logicConditionGetFlightOperandValue(int operand) { case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_PROFILE: // int return getConfigProfile() + 1; break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE: // int + return getConfigMixerProfile() + 1; + break; case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: return getLoiterRadius(navConfig()->fw.loiter_radius); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 0025602984f..2b18be4fa89 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -76,7 +76,8 @@ typedef enum { LOGIC_CONDITION_MAX = 44, LOGIC_CONDITION_FLIGHT_AXIS_ANGLE_OVERRIDE = 45, LOGIC_CONDITION_FLIGHT_AXIS_RATE_OVERRIDE = 46, - LOGIC_CONDITION_LAST = 47, + LOGIC_CONDITION_SET_MIXER_PROFILE = 47, + LOGIC_CONDITION_LAST = 48, } logicOperation_e; typedef enum logicOperandType_s { @@ -132,6 +133,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_AGL_STATUS, //0,1,2 // 38 LOGIC_CONDITION_OPERAND_FLIGHT_AGL, //0,1,2 // 39 LOGIC_CONDITION_OPERAND_FLIGHT_RANGEFINDER_RAW, //int // 40 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 41 } logicFlightOperands_e; typedef enum { From 88e923122eb1d53bdbe577d417edc59940749f52 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 7 Nov 2022 20:22:42 +0900 Subject: [PATCH 03/60] in progress of output_profile dev --- src/main/CMakeLists.txt | 2 ++ src/main/drivers/pwm_mapping.c | 9 +------- src/main/drivers/pwm_mapping.h | 9 ++++++++ src/main/drivers/pwm_output.c | 4 ++++ src/main/drivers/pwm_output.h | 1 + src/main/fc/settings.h | 20 +++++++++------- src/main/flight/mixer.c | 3 +++ src/main/flight/output_profile.c | 39 ++++++++++++++++++++++++++++++++ src/main/flight/output_profile.h | 4 ++++ 9 files changed, 75 insertions(+), 16 deletions(-) create mode 100644 src/main/flight/output_profile.c create mode 100644 src/main/flight/output_profile.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 9f78997cad9..9598676f750 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -310,6 +310,8 @@ main_sources(COMMON_SRC flight/rate_dynamics.h flight/mixer.c flight/mixer.h + flight/output_profile.c + flight/output_profile.h flight/pid.c flight/pid.h flight/pid_autotune.c diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 1d3ab1b584a..b67fcf2f902 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -47,13 +47,6 @@ enum { MAP_TO_SERVO_OUTPUT, }; -typedef struct { - int maxTimMotorCount; - int maxTimServoCount; - const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; - const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; -} timMotorServoHardware_t; - static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE; static const char * pwmInitErrorMsg[] = { @@ -388,7 +381,7 @@ bool pwmMotorAndServoInit(void) // Build temporary timer mappings for motor and servo pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); - + resetAllocatedOutputPortCount(); // At this point we have built tables of timers suitable for motor and servo mappings // Now we can actually initialize them according to motor/servo count from mixer pwmInitMotors(&timOutputs); diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 14c3f9293ba..0b38d098bd4 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -18,6 +18,7 @@ #pragma once #include "drivers/io_types.h" +#include "drivers/timer.h" #include "flight/mixer.h" #include "flight/servos.h" @@ -61,6 +62,13 @@ typedef enum { PWM_INIT_ERROR_TIMER_INIT_FAILED, } pwmInitError_e; +typedef struct { + int maxTimMotorCount; + int maxTimServoCount; + const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; + const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; +} timMotorServoHardware_t; + typedef struct rangefinderIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; @@ -71,6 +79,7 @@ typedef struct { bool isDSHOT; } motorProtocolProperties_t; +void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) bool pwmMotorAndServoInit(void); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 658c3f5cb55..7b4891bf85b 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -113,6 +113,10 @@ static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; #endif +void resetAllocatedOutputPortCount(void){ + allocatedOutputPortCount = 0; +} + static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) { p->tch = tch; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index b3c0fa6be0e..d2ef60690f2 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -34,6 +34,7 @@ void pwmRequestMotorTelemetry(int motorIndex); ioTag_t pwmGetMotorPinTag(int motorIndex); +void resetAllocatedOutputPortCount(void); void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(void); diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index 16c47d766bc..ae7da853172 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -14,11 +14,11 @@ typedef struct lookupTableEntry_s { } lookupTableEntry_t; #define SETTING_TYPE_OFFSET 0 -#define SETTING_SECTION_OFFSET 4 -#define SETTING_MODE_OFFSET 7 +#define SETTING_SECTION_OFFSET 3 +#define SETTING_MODE_OFFSET 6 typedef enum { - // value type, bits 0-3 + // value type, bits 0-2 VAR_UINT8 = (0 << SETTING_TYPE_OFFSET), VAR_INT8 = (1 << SETTING_TYPE_OFFSET), VAR_UINT16 = (2 << SETTING_TYPE_OFFSET), @@ -29,22 +29,26 @@ typedef enum { } setting_type_e; typedef enum { - // value section, bits 4-6 + // value section, bits 3-5 MASTER_VALUE = (0 << SETTING_SECTION_OFFSET), PROFILE_VALUE = (1 << SETTING_SECTION_OFFSET), - CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), // 0x20 + CONTROL_RATE_VALUE = (2 << SETTING_SECTION_OFFSET), BATTERY_CONFIG_VALUE = (3 << SETTING_SECTION_OFFSET), MIXER_CONFIG_VALUE = (4 << SETTING_SECTION_OFFSET), } setting_section_e; typedef enum { - // value mode, bits 7 + // value mode, bits 6-7 MODE_DIRECT = (0 << SETTING_MODE_OFFSET), MODE_LOOKUP = (1 << SETTING_MODE_OFFSET), // 0x40 } setting_mode_e; -#define SETTING_TYPE_MASK (0x0F) -#define SETTING_SECTION_MASK (0x30) +// #define SETTING_TYPE_MASK (0x0F) +// #define SETTING_SECTION_MASK (0x30) +// #define SETTING_MODE_MASK (0xC0) + +#define SETTING_TYPE_MASK (0x07) +#define SETTING_SECTION_MASK (0x38) #define SETTING_MODE_MASK (0xC0) typedef struct settingMinMaxConfig_s { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index fb9934a37bf..a174b513ebe 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -615,6 +615,9 @@ void FAST_CODE mixTable() 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; diff --git a/src/main/flight/output_profile.c b/src/main/flight/output_profile.c new file mode 100644 index 00000000000..2a65264a629 --- /dev/null +++ b/src/main/flight/output_profile.c @@ -0,0 +1,39 @@ +#include +#include +#include + +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/time.h" +#include "flight/mixer.h" +// #include "flight/pid.h" +#include "flight/servos.h" + + +bool MixerProfileHotSwitch(void) +{ + //store current output for check + timMotorServoHardware_t old_timOutputs; + pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); + + 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; + for (int i; i++; i Date: Mon, 7 Nov 2022 23:29:43 +0900 Subject: [PATCH 04/60] add mixer_profile.h --- src/main/CMakeLists.txt | 4 +- src/main/cms/cms_menu_mixer_servo.c | 1 + src/main/drivers/pwm_mapping.c | 10 +- src/main/drivers/pwm_mapping.h | 5 +- src/main/fc/cli.c | 16 ++- src/main/fc/fc_core.c | 1 + src/main/fc/fc_msp.c | 1 + src/main/fc/fc_msp_box.c | 1 + src/main/fc/settings.yaml | 12 ++- src/main/flight/mixer.c | 39 ++++---- src/main/flight/mixer.h | 34 ++++--- src/main/flight/mixer_profile.c | 109 +++++++++++++++++++++ src/main/flight/mixer_profile.h | 26 +++++ src/main/flight/output_profile.c | 39 -------- src/main/flight/output_profile.h | 4 - src/main/flight/pid.c | 1 + src/main/flight/servos.c | 4 + src/main/navigation/navigation.c | 2 +- src/main/programming/logic_condition.c | 12 +-- src/main/telemetry/mavlink.c | 2 +- src/test/unit/flight_mixer_unittest.cc.txt | 2 +- 21 files changed, 221 insertions(+), 104 deletions(-) create mode 100644 src/main/flight/mixer_profile.c create mode 100644 src/main/flight/mixer_profile.h delete mode 100644 src/main/flight/output_profile.c delete mode 100644 src/main/flight/output_profile.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 9598676f750..eb9460f4094 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -310,8 +310,6 @@ main_sources(COMMON_SRC flight/rate_dynamics.h flight/mixer.c flight/mixer.h - flight/output_profile.c - flight/output_profile.h flight/pid.c flight/pid.h flight/pid_autotune.c @@ -321,6 +319,8 @@ main_sources(COMMON_SRC flight/rth_estimator.h flight/servos.c flight/servos.h + flight/mixer_profile.c + flight/mixer_profile.h flight/wind_estimator.c flight/wind_estimator.h flight/gyroanalyse.c diff --git a/src/main/cms/cms_menu_mixer_servo.c b/src/main/cms/cms_menu_mixer_servo.c index 7c876d7150d..e23229e7acd 100644 --- a/src/main/cms/cms_menu_mixer_servo.c +++ b/src/main/cms/cms_menu_mixer_servo.c @@ -31,6 +31,7 @@ #include "build/version.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/servos.h" diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index b67fcf2f902..bbd1c298281 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -381,7 +381,6 @@ bool pwmMotorAndServoInit(void) // Build temporary timer mappings for motor and servo pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); - resetAllocatedOutputPortCount(); // At this point we have built tables of timers suitable for motor and servo mappings // Now we can actually initialize them according to motor/servo count from mixer pwmInitMotors(&timOutputs); @@ -389,3 +388,12 @@ bool pwmMotorAndServoInit(void) return (pwmInitError == PWM_INIT_ERROR_NONE); } + +bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs) +{ + resetAllocatedOutputPortCount(); + pwmInitMotors(timOutputs); + pwmInitServos(timOutputs); + + return (pwmInitError == PWM_INIT_ERROR_NONE); +} \ No newline at end of file diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 0b38d098bd4..3de4431470c 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -19,7 +19,7 @@ #include "drivers/io_types.h" #include "drivers/timer.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/servos.h" #if defined(TARGET_MOTOR_COUNT) @@ -79,8 +79,9 @@ typedef struct { bool isDSHOT; } motorProtocolProperties_t; -void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) +void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos); bool pwmMotorAndServoInit(void); +bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); const char * getPwmInitErrorMessage(void); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index d32cf9f610c..3a3af833048 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -81,7 +81,8 @@ bool cliMode = false; #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/mixer.h" +// #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -282,7 +283,8 @@ typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), DUMP_BATTERY_PROFILE = (1 << 2), - DUMP_RATES = (1 << 3), + // DUMP_RATES = (1 << 3), + DUMP_MIXER_PROFILE = (1 << 3), DUMP_ALL = (1 << 4), DO_DIFF = (1 << 5), SHOW_DEFAULTS = (1 << 6), @@ -3567,6 +3569,8 @@ static void printConfig(const char *cmdline, bool doDiff) dumpMask = DUMP_PROFILE; // only } else if ((options = checkCommand(cmdline, "battery_profile"))) { dumpMask = DUMP_BATTERY_PROFILE; // only + } else if ((options = checkCommand(cmdline, "mixer_profile"))) { + dumpMask = DUMP_MIXER_PROFILE; // only } else if ((options = checkCommand(cmdline, "all"))) { dumpMask = DUMP_ALL; // all profiles and rates } else { @@ -3712,6 +3716,9 @@ static void printConfig(const char *cmdline, bool doDiff) for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) { cliDumpBatteryProfile(ii, dumpMask); } + for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) { + cliDumpMixerProfile(ii, dumpMask); + } setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); @@ -3726,6 +3733,7 @@ static void printConfig(const char *cmdline, bool doDiff) // dump just the current profiles cliDumpProfile(getConfigProfile(), dumpMask); cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); } } @@ -3737,6 +3745,10 @@ static void printConfig(const char *cmdline, bool doDiff) cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } + if (dumpMask & DUMP_MIXER_PROFILE) { + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); + } + if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrintHashLine("save configuration\r\nsave"); } diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b79e97c396f..4253c995a30 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -82,6 +82,7 @@ FILE_COMPILE_FOR_SPEED #include "telemetry/telemetry.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 56c716ade14..587651b21d7 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -73,6 +73,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index aeb14631645..112c8509878 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -30,6 +30,7 @@ #include "fc/fc_msp_box.h" #include "fc/runtime_config.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "io/osd.h" diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c96ab3db477..6c94747e043 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1148,33 +1148,35 @@ groups: - name: PG_MIXER_PROFILE type: mixerProfile_t + headers: ["flight/mixer.h","flight/mixer_profile.h"] + value_type: MIXER_CONFIG_VALUE members: - name: motor_direction_inverted description: "Use if you need to inverse yaw motor direction." default_value: OFF - field: motorDirectionInverted + field: mixer_config.motorDirectionInverted type: bool - name: platform_type description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" default_value: "MULTIROTOR" - field: platformType + field: mixer_config.platformType type: uint8_t table: platform_type - name: has_flaps description: "Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot" default_value: OFF - field: hasFlaps + field: mixer_config.hasFlaps type: bool - name: model_preview_type description: "ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons." default_value: -1 - field: appliedMixerPreset + field: mixer_config.appliedMixerPreset min: -1 max: INT16_MAX - name: output_mode description: "Output function assignment mode. AUTO assigns outputs according to the default mapping, SERVOS assigns all outputs to servos, MOTORS assigns all outputs to motors" default_value: "AUTO" - field: outputMode + field: mixer_config.outputMode table: output_mode - name: PG_REVERSIBLE_MOTORS_CONFIG diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a174b513ebe..37011845bc2 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -83,24 +83,24 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); - -void pgResetFn_mixerProfiles(mixerProfile_t *instance) -{ - for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { - RESET_CONFIG(mixerProfile_t, &instance[i], - .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, - .platformType = SETTING_PLATFORM_TYPE_DEFAULT, - .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, - ); - motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; - for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { - instance->MotorMixer[j] = tmp_mixer; - } - } -} +// PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); + +// void pgResetFn_mixerProfiles(mixerProfile_t *instance) +// { +// for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { +// RESET_CONFIG(mixerProfile_t, &instance[i], +// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, +// .platformType = SETTING_PLATFORM_TYPE_DEFAULT, +// .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, +// ); +// motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; +// for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { +// instance->MotorMixer[j] = tmp_mixer; +// } +// } +// } // PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_PROFILE, 5); @@ -122,8 +122,6 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles ); -PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); - #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f int getThrottleIdleValue(void) @@ -438,7 +436,6 @@ void FAST_CODE writeMotors(void) // We don't define USE_DSHOT motorValue = motor[i]; #endif - pwmWriteMotor(i, motorValue); } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index fa560af504c..3e624634078 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -19,10 +19,6 @@ #include "config/parameter_group.h" -#ifndef MAX_MIXER_PROFILE_COUNT -#define MAX_MIXER_PROFILE_COUNT 2 -#endif - #if defined(TARGET_MOTOR_COUNT) #define MAX_SUPPORTED_MOTORS TARGET_MOTOR_COUNT #else @@ -67,23 +63,31 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; -PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); +// PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); -typedef struct mixerProfile_s { +typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; uint8_t outputMode; - motorMixer_t MotorMixer[MAX_SUPPORTED_MOTORS]; -} mixerProfile_t; - -PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); -#define mixerConfig() mixerProfiles(systemConfig()->current_mixer_profile_index) -#define mixerConfigMutable() ((mixerProfile_t *)mixerConfig()) -#define primaryMotorMixer(_index) (&((mixerConfig()->MotorMixer)[_index])) -#define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) -extern motorMixer_t primaryMotorMixer_CopyArray[12]; +} mixerConfig_t; + +// typedef struct mixerProfile_s { +// int8_t motorDirectionInverted; +// uint8_t platformType; +// bool hasFlaps; +// int16_t appliedMixerPreset; +// uint8_t outputMode; +// motorMixer_t MotorMixer[MAX_SUPPORTED_MOTORS]; +// } mixerProfile_t; + +// PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); +// #define mixerConfig() mixerProfiles(systemConfig()->current_mixer_profile_index) +// #define mixerConfigMutable() ((mixerProfile_t *)mixerConfig()) +// #define primaryMotorMixer(_index) (&((mixerConfig()->MotorMixer)[_index])) +// #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) +// extern motorMixer_t primaryMotorMixer_CopyArray[12]; typedef struct reversibleMotorsConfig_s { uint16_t deadband_low; // min 3d value diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c new file mode 100644 index 00000000000..fab3cbe3e0b --- /dev/null +++ b/src/main/flight/mixer_profile.c @@ -0,0 +1,109 @@ +#include +#include +#include + +#include "config/feature.h" +#include "config/parameter_group.h" +#include "config/parameter_group_ids.h" +#include "config/config_reset.h" + +#include "drivers/pwm_output.h" +#include "drivers/pwm_mapping.h" +#include "drivers/time.h" +#include "flight/mixer.h" +// #include "flight/pid.h" +#include "flight/servos.h" + +#include "fc/config.h" +#include "fc/runtime_config.h" +#include "fc/settings.h" + +#include "common/log.h" + +PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); + +void pgResetFn_mixerProfiles(mixerProfile_t *instance) +{ + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + RESET_CONFIG(mixerProfile_t, &instance[i], + .mixer_config = { + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .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, + } + ); + motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; + for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { + instance->MotorMixers[j] = tmp_mixer; + } + } +} + +PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); + +int min_ab(int a,int b) +{ + return a > b ? b : a; +} + +bool OutputProfileHotSwitch(int profile_index) +{ + // does not work with timerHardwareOverride + LOG_INFO(PWM, "OutputProfileHotSwitch"); + + LOG_INFO(PWM, "stop all motors"); + stopMotors(); + // LOG_INFO(PWM, "stop all pwm motors"); + // stopPwmAllMotors(); + + //store current output for check + LOG_INFO(PWM, "get old_timOutputs"); + // delay(1000); // give time to print + timMotorServoHardware_t old_timOutputs; + LOG_INFO(PWM, "get pwmBuildTimerOutputList"); + // delay(1000); // give time to print + pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); + // delay(1000); // give time to print + if (!setConfigMixerProfile(profile_index)){ + return false; + } + // delay(1000); // give time to print + LOG_INFO(PWM, "servosInit"); + // delay(1000); // give time to print + servosInit(); + LOG_INFO(PWM, "mixerUpdateStateFlags"); + // delay(1000); // give time to print + mixerUpdateStateFlags(); + LOG_INFO(PWM, "mixerInit"); + // delay(1000); // give time to print + mixerInit(); + + LOG_INFO(PWM, "get timOutputs"); + // delay(1000); // give time to print + timMotorServoHardware_t timOutputs; + pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); + LOG_INFO(PWM, "check changes"); + // delay(1000); // give time to print, stuck here + bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount; + bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount; + for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++) + { + motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag; + } + for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++) + { + servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag; + } + if(!motor_output_type_not_changed || !servo_output_type_not_changed){ + LOG_INFO(PWM, "pwmMotorAndServoHotInit"); + // delay(1000); // give time to print + pwmMotorAndServoHotInit(&timOutputs); + } + if (!STATE(ALTITUDE_CONTROL)) { + featureClear(FEATURE_AIRMODE); + } + return true; +} + diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h new file mode 100644 index 00000000000..b192e201703 --- /dev/null +++ b/src/main/flight/mixer_profile.h @@ -0,0 +1,26 @@ +#pragma once + +#include "config/parameter_group.h" + +#include "flight/mixer.h" +#include "flight/servos.h" + +#ifndef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 +#endif + +typedef struct mixerProfile_s { + mixerConfig_t mixer_config; + motorMixer_t MotorMixers[MAX_SUPPORTED_MOTORS]; + servoMixer_t ServoMixers[MAX_SERVO_RULES]; +} mixerProfile_t; + +PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); + +#define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) +#define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) +#define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index]) +#define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) +extern motorMixer_t primaryMotorMixer_CopyArray[12]; + +bool OutputProfileHotSwitch(int profile_index); diff --git a/src/main/flight/output_profile.c b/src/main/flight/output_profile.c deleted file mode 100644 index 2a65264a629..00000000000 --- a/src/main/flight/output_profile.c +++ /dev/null @@ -1,39 +0,0 @@ -#include -#include -#include - -#include "drivers/pwm_output.h" -#include "drivers/pwm_mapping.h" -#include "drivers/time.h" -#include "flight/mixer.h" -// #include "flight/pid.h" -#include "flight/servos.h" - - -bool MixerProfileHotSwitch(void) -{ - //store current output for check - timMotorServoHardware_t old_timOutputs; - pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); - - 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; - for (int i; i++; i = 0 && operandA < MAX_MIXER_PROFILE_COUNT)) { bool mixerprofileChanged = false; - if (setConfigMixerProfile(operandA)) { - stopMotors(); - stopPwmAllMotors(); - servosInit(); - mixerUpdateStateFlags(); - mixerInit(); - pwmMotorAndServoInit(); - if (!STATE(ALTITUDE_CONTROL)) { - featureClear(FEATURE_AIRMODE); - } + if (OutputProfileHotSwitch(operandA)) { mixerprofileChanged = true; } return mixerprofileChanged; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 07df75bcc0b..e180d64723f 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -54,7 +54,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" diff --git a/src/test/unit/flight_mixer_unittest.cc.txt b/src/test/unit/flight_mixer_unittest.cc.txt index c0915b3cc27..6b33b6c50c5 100644 --- a/src/test/unit/flight_mixer_unittest.cc.txt +++ b/src/test/unit/flight_mixer_unittest.cc.txt @@ -144,7 +144,7 @@ TEST_F(ChannelForwardingTest, TestForwardAuxChannelsToServosWithLessRemainingSer class BasicMixerIntegrationTest : public ::testing::Test { protected: - mixerProfile_t mixerConfig; + mixerConfig_t mixerConfig; rxConfig_t rxConfig; escAndServoConfig_t escAndServoConfig; servoParam_t servoConf[MAX_SUPPORTED_SERVOS]; From 23968a64d238b77d076b4397e871931625c3539e Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 9 Nov 2022 03:02:06 +0900 Subject: [PATCH 05/60] in development --- src/main/drivers/pwm_mapping.c | 6 ++ src/main/fc/cli.c | 13 +++- src/main/flight/mixer.h | 1 + src/main/flight/mixer_profile.c | 75 +++++++++++------------- src/main/flight/mixer_profile.h | 1 - src/main/target/MATEKF405/CMakeLists.txt | 1 + src/main/target/MATEKF405/target.c | 16 +++++ src/main/target/MATEKF405/target.h | 4 ++ 8 files changed, 73 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index bbd1c298281..7360a8f01dc 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -220,10 +220,12 @@ static void timerHardwareOverride(timerHardware_t * timer) { void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) { + LOG_INFO(PWM, "pwmBuildTimerOutputList"); timOutputs->maxTimMotorCount = 0; timOutputs->maxTimServoCount = 0; uint8_t motorCount = getMotorCount(); + LOG_INFO(PWM, "motorCount %d", motorCount); uint8_t motorIdx = 0; for (int idx = 0; idx < timerHardwareCount; idx++) { @@ -257,6 +259,10 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU else if (timHw->usageFlags & TIM_USE_MC_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } + else if (timHw->usageFlags & TIM_USE_MC_SERVO){ + type = MAP_TO_SERVO_OUTPUT; + } + } else { // Fixed wing or HELI (one/two motors and a lot of servos if (timHw->usageFlags & TIM_USE_FW_SERVO) { diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 3a3af833048..ebcf4448bf5 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3076,6 +3076,13 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask) cliPrintHashLine("mixer_profile"); cliPrintLinef("mixer_profile %d\r\n", getConfigMixerProfile() + 1); dumpAllValues(MIXER_CONFIG_VALUE, dumpMask); + cliPrintHashLine("mixer"); + cliPrintHashLine("dumpMask"); + char buf0[FTOA_BUFFER_SIZE]; + 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 @@ -3619,10 +3626,10 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("resources"); //printResource(dumpMask, &defaultConfig); - cliPrintHashLine("mixer"); - cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); + // cliPrintHashLine("mixer"); + // cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); - printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); + // printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); // print custom servo mixer if exists cliPrintHashLine("servo mixer"); diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 3e624634078..07d1ab248b9 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -142,6 +142,7 @@ void processServoAutotrim(const float dT); void processServoAutotrimMode(void); void processContinuousServoAutotrim(const float dT); void stopMotors(void); +void stopAndDisableMotors(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index fab3cbe3e0b..561b7d6bb7a 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -41,7 +41,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) } } -PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +// PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); int min_ab(int a,int b) { @@ -50,60 +50,55 @@ int min_ab(int a,int b) bool OutputProfileHotSwitch(int profile_index) { +#ifdef ENABLE_MIXER_PROFILE_HOTSWAP // does not work with timerHardwareOverride LOG_INFO(PWM, "OutputProfileHotSwitch"); - LOG_INFO(PWM, "stop all motors"); - stopMotors(); - // LOG_INFO(PWM, "stop all pwm motors"); - // stopPwmAllMotors(); - //store current output for check - LOG_INFO(PWM, "get old_timOutputs"); - // delay(1000); // give time to print + uint8_t old_platform_type=mixerConfig()->platformType; timMotorServoHardware_t old_timOutputs; - LOG_INFO(PWM, "get pwmBuildTimerOutputList"); - // delay(1000); // give time to print pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); - // delay(1000); // give time to print if (!setConfigMixerProfile(profile_index)){ return false; } - // delay(1000); // give time to print - LOG_INFO(PWM, "servosInit"); - // delay(1000); // give time to print + stopMotors(); + // delay(3000); //check motor stop servosInit(); - LOG_INFO(PWM, "mixerUpdateStateFlags"); - // delay(1000); // give time to print mixerUpdateStateFlags(); - LOG_INFO(PWM, "mixerInit"); - // delay(1000); // give time to print mixerInit(); - LOG_INFO(PWM, "get timOutputs"); - // delay(1000); // give time to print - timMotorServoHardware_t timOutputs; - pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); - LOG_INFO(PWM, "check changes"); - // delay(1000); // give time to print, stuck here - bool motor_output_type_not_changed = old_timOutputs.maxTimMotorCount == timOutputs.maxTimMotorCount; - bool servo_output_type_not_changed = old_timOutputs.maxTimServoCount == timOutputs.maxTimServoCount; - for (int i; i < min_ab(old_timOutputs.maxTimMotorCount,timOutputs.maxTimMotorCount); i++) - { - motor_output_type_not_changed &= old_timOutputs.timMotors[i]->tag==timOutputs.timMotors[i]->tag; - } - for (int i; i < min_ab(old_timOutputs.maxTimServoCount,timOutputs.maxTimServoCount); i++) + // 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, "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++) + // { + // 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) { - servo_output_type_not_changed &= old_timOutputs.timServos[i]->tag==timOutputs.timServos[i]->tag; - } - if(!motor_output_type_not_changed || !servo_output_type_not_changed){ - LOG_INFO(PWM, "pwmMotorAndServoHotInit"); - // delay(1000); // give time to print - pwmMotorAndServoHotInit(&timOutputs); - } - if (!STATE(ALTITUDE_CONTROL)) { - featureClear(FEATURE_AIRMODE); + } return true; +#else + profile_index=0; //prevent compilier warning: parameter 'profile_index' set but not used + return (bool) profile_index; +#endif } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index b192e201703..9d59e65f1f6 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -21,6 +21,5 @@ PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) #define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index]) #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) -extern motorMixer_t primaryMotorMixer_CopyArray[12]; bool OutputProfileHotSwitch(int profile_index); diff --git a/src/main/target/MATEKF405/CMakeLists.txt b/src/main/target/MATEKF405/CMakeLists.txt index c4f839c1c1d..36d193a945b 100644 --- a/src/main/target/MATEKF405/CMakeLists.txt +++ b/src/main/target/MATEKF405/CMakeLists.txt @@ -1,3 +1,4 @@ target_stm32f405xg(MATEKF405) target_stm32f405xg(MATEKF405_SERVOS6) target_stm32f405xg(MATEKF405OSD) +target_stm32f405xg(MATEKF405VTOL) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 40ebf626e5f..34387de6c8d 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -23,6 +23,21 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM +#ifdef MATEKF405VTOL + DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 + DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 UP(2,1) + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 UP(2,1) + + DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED , 0, 0), // S5 UP(1,7) + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) + DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) + + DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 + + DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // TX4 UP(1,6) D(1,2)!S1 + DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // RX4 UP(1,6) D(1,4) +#else #ifdef MATEKF405_SERVOS6 DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 @@ -46,6 +61,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // TX4 UP(1,6) D(1,2)!S1 DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // RX4 UP(1,6) D(1,4) +#endif }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index fa2af63384c..e664fa40b13 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -195,3 +195,7 @@ #define USE_ESC_SENSOR #define MAX_PWM_OUTPUT_PORTS 6 + +#ifdef MATEKF405VTOL +#define ENABLE_MIXER_PROFILE_HOTSWAP +#endif \ No newline at end of file From 97318e4f84c45048dd8620177e08856bb3340022 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Nov 2022 04:47:32 +0900 Subject: [PATCH 06/60] add sannity check in hot mixprofile switching, and fix cli --- src/main/drivers/pwm_mapping.c | 16 ++-- src/main/drivers/pwm_mapping.h | 2 +- src/main/fc/cli.c | 10 +-- src/main/flight/mixer_profile.c | 120 ++++++++++++++++--------- src/main/flight/mixer_profile.h | 5 ++ src/main/navigation/navigation.c | 7 ++ src/main/navigation/navigation.h | 1 + src/main/programming/logic_condition.c | 5 +- src/main/target/MATEKF405/target.h | 2 +- 9 files changed, 105 insertions(+), 63 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 7360a8f01dc..744e9fa2ddd 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -395,11 +395,11 @@ bool pwmMotorAndServoInit(void) return (pwmInitError == PWM_INIT_ERROR_NONE); } -bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs) -{ - resetAllocatedOutputPortCount(); - pwmInitMotors(timOutputs); - pwmInitServos(timOutputs); - - return (pwmInitError == PWM_INIT_ERROR_NONE); -} \ No newline at end of file +// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs) +// { +// resetAllocatedOutputPortCount(); +// pwmInitMotors(timOutputs); +// pwmInitServos(timOutputs); + +// return (pwmInitError == PWM_INIT_ERROR_NONE); +// } \ No newline at end of file diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 3de4431470c..8563d2fd953 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/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); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index ebcf4448bf5..a135e91e2ab 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1040,7 +1040,7 @@ static void cliAdjustmentRange(char *cmdline) } static void printMotorMix(uint8_t dumpMask, const motorMixer_t *primaryMotorMixer, const motorMixer_t *defaultprimaryMotorMixer) -{ +{ const char *format = "mmix %d %s %s %s %s"; char buf0[FTOA_BUFFER_SIZE]; char buf1[FTOA_BUFFER_SIZE]; @@ -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"); - char buf0[FTOA_BUFFER_SIZE]; - itoa(dumpMask,buf0,2); - cliPrintHashLine(buf0); - cliDumpPrintLinef(dumpMask, primaryMotorMixer(0)->throttle == 0.0f, "\r\nmmix reset\r\n"); - printMotorMix(dumpMask, primaryMotorMixer(0), NULL); + cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n"); + printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0)); } #ifdef USE_CLI_BATCH diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 561b7d6bb7a..f4247154210 100644 --- a/src/main/flight/mixer_profile.c +++ b/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); -int min_ab(int a,int b) -{ - return a > b ? b : a; -} - bool OutputProfileHotSwitch(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()); + //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(); - // delay(3000); //check motor stop + // stopMotors(); + writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors 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, "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++) - // { - // 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) { - + pidInit(); + pidInitFilters(); + schedulePidGainsUpdate(); + navigationInit(); } return true; -#else - profile_index=0; //prevent compilier warning: parameter 'profile_index' set but not used - return (bool) profile_index; -#endif } +int min_ab(int a,int b) +{ + return a > b ? b : a; +} + +void checkOutputMapping(int profile_index) +{ + 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"); + } +} + + diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 9d59e65f1f6..249cccf1938 100644 --- a/src/main/flight/mixer_profile.h +++ b/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); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c6c5d6cd51a..08c66766860 100644 --- a/src/main/navigation/navigation.c +++ b/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(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 339da15392f..13f9f58bc92 100644 --- a/src/main/navigation/navigation.h +++ b/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); diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index cd0a728b148..6a49290050e 100644 --- a/src/main/programming/logic_condition.c +++ b/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; - if (OutputProfileHotSwitch(operandA)) { - mixerprofileChanged = true; - } + bool mixerprofileChanged = OutputProfileHotSwitch(operandA); return mixerprofileChanged; } else { return false; diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index e664fa40b13..f0f1c8bc87f 100644 --- a/src/main/target/MATEKF405/target.h +++ b/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 \ No newline at end of file From 80a3dee407178b29404941dbb2fa7c6cb08af4ff Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 11 Nov 2022 18:26:39 +0900 Subject: [PATCH 07/60] added servo to mixer --- src/main/fc/cli.c | 11 +++++++---- src/main/flight/mixer_profile.c | 23 +++++++++++++++++++---- src/main/flight/mixer_profile.h | 8 ++++++-- src/main/flight/servos.c | 32 ++++++++++++++++---------------- 4 files changed, 48 insertions(+), 26 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index a135e91e2ab..778889c11a8 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3079,6 +3079,9 @@ static void cliDumpMixerProfile(uint8_t profileIndex, uint8_t dumpMask) cliPrintHashLine("mixer"); cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n"); printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0)); + cliPrintHashLine("servo mixer"); + cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray()[0].rate == 0, "smix reset\r\n"); + printServoMix(dumpMask, customServoMixers_CopyArray(), customServoMixers(0)); } #ifdef USE_CLI_BATCH @@ -3619,7 +3622,7 @@ static void printConfig(const char *cmdline, bool doDiff) #endif } - cliPrintHashLine("resources"); + // cliPrintHashLine("resources"); //printResource(dumpMask, &defaultConfig); // cliPrintHashLine("mixer"); @@ -3628,9 +3631,9 @@ static void printConfig(const char *cmdline, bool doDiff) // printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); // print custom servo mixer if exists - cliPrintHashLine("servo mixer"); - cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n"); - printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); + // cliPrintHashLine("servo mixer"); + // cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n"); + // printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); // print servo parameters cliPrintHashLine("servo"); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index f4247154210..204d54a2b4f 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -37,9 +37,24 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .outputMode = SETTING_OUTPUT_MODE_DEFAULT, } ); - motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { - instance->MotorMixers[j] = tmp_mixer; + RESET_CONFIG(motorMixer_t, &instance[i].MotorMixers[j], + .throttle=0, + .roll=0, + .pitch=0, + .yaw=0 + ); + } + for (int j = 0; j < MAX_SERVO_RULES; j++) { + RESET_CONFIG(servoMixer_t, &instance[i].ServoMixers[j], + .targetChannel = 0, + .inputSource = 0, + .rate = 0, + .speed = 0 +#ifdef USE_PROGRAMMING_FRAMEWORK + ,.conditionId = -1 +#endif + ); } } } @@ -79,7 +94,7 @@ bool OutputProfileHotSwitch(int profile_index) return false; } // stopMotors(); - writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors + writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors with out delay servosInit(); mixerUpdateStateFlags(); mixerInit(); @@ -99,7 +114,7 @@ int min_ab(int a,int b) return a > b ? b : a; } -void checkOutputMapping(int profile_index) +void checkOutputMapping(int profile_index)//debug purpose { timMotorServoHardware_t old_timOutputs; pwmBuildTimerOutputList(&old_timOutputs, isMixerUsingServos()); diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 249cccf1938..97d5296abde 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -19,11 +19,15 @@ PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) + #define primaryMotorMixer(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->MotorMixers)[_index]) #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) +#define customServoMixers(_index) (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->ServoMixers)[_index]) +#define customServoMixersMutable(_index) ((servoMixer_t *)customServoMixers(_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) +static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) { return &mixerProfiles_CopyArray[_index]; } +#define primaryMotorMixer_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->MotorMixers) +#define customServoMixers_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->ServoMixers) #define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config)) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index fed5519ef9b..93ebeb9991c 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -70,22 +70,22 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT ); -PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); - -void pgResetFn_customServoMixers(servoMixer_t *instance) -{ - for (int i = 0; i < MAX_SERVO_RULES; i++) { - RESET_CONFIG(servoMixer_t, &instance[i], - .targetChannel = 0, - .inputSource = 0, - .rate = 0, - .speed = 0 -#ifdef USE_PROGRAMMING_FRAMEWORK - ,.conditionId = -1 -#endif - ); - } -} +// PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); + +// void pgResetFn_customServoMixers(servoMixer_t *instance) +// { +// for (int i = 0; i < MAX_SERVO_RULES; i++) { +// RESET_CONFIG(servoMixer_t, &instance[i], +// .targetChannel = 0, +// .inputSource = 0, +// .rate = 0, +// .speed = 0 +// #ifdef USE_PROGRAMMING_FRAMEWORK +// ,.conditionId = -1 +// #endif +// ); +// } +// } PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3); From cbbef197210c05dbcd775f9adc8612b5b0277c7e Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 13 Nov 2022 23:02:35 +0900 Subject: [PATCH 08/60] mixer_profile draft pr --- src/main/config/parameter_group_ids.h | 2 +- src/main/fc/cli.c | 13 +++++++++---- src/main/fc/config.c | 1 + src/main/flight/mixer_profile.c | 7 ++++--- src/main/flight/mixer_profile.h | 2 ++ src/main/flight/servos.c | 16 ++++++++++++++++ src/main/flight/servos.h | 1 + src/main/target/MATEKF405/target.c | 2 +- src/main/target/MATEKF405/target.h | 2 +- src/main/target/MATEKF405TE/CMakeLists.txt | 1 + src/main/target/MATEKF405TE/target.c | 14 +++++++++++++- src/main/target/MATEKF405TE/target.h | 3 +++ 12 files changed, 53 insertions(+), 11 deletions(-) diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 754b7dbadae..384fa79ba4f 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -37,7 +37,7 @@ #define PG_SYSTEM_CONFIG 18 #define PG_FEATURE_CONFIG 19 #define PG_MIXER_PROFILE 20 -#define PG_SERVO_MIXER 21 +// #define PG_SERVO_MIXER 21 #define PG_IMU_CONFIG 22 //#define PG_PROFILE_SELECTION 23 #define PG_RX_CONFIG 24 diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 778889c11a8..34932c58c39 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -1864,7 +1864,8 @@ static void cliServoMix(char *cmdline) printServoMix(DUMP_MASTER, customServoMixers(0), NULL); } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer - pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); + // pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); + Reset_servoMixers(customServoMixersMutable(0)); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT}; char *ptr = strtok_r(cmdline, " ", &saveptr); @@ -3589,12 +3590,14 @@ static void printConfig(const char *cmdline, bool doDiff) const int currentProfileIndexSave = getConfigProfile(); const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentMixerProfileIndexSave = getConfigMixerProfile(); backupConfigs(); // reset all configs to defaults to do differencing resetConfigs(); // restore the profile indices, since they should not be reset for proper comparison setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigMixerProfile(currentMixerProfileIndexSave); if (checkCommand(options, "showdefaults")) { dumpMask = dumpMask | SHOW_DEFAULTS; // add default values as comments for changed values @@ -3622,8 +3625,8 @@ static void printConfig(const char *cmdline, bool doDiff) #endif } - // cliPrintHashLine("resources"); - //printResource(dumpMask, &defaultConfig); + cliPrintHashLine("resources"); + // printResource(dumpMask, &defaultConfig); // cliPrintHashLine("mixer"); // cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); @@ -3716,6 +3719,7 @@ static void printConfig(const char *cmdline, bool doDiff) // dump all profiles const int currentProfileIndexSave = getConfigProfile(); const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); + const int currentMixerProfileIndexSave = getConfigMixerProfile(); for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { cliDumpProfile(ii, dumpMask); } @@ -3727,10 +3731,11 @@ static void printConfig(const char *cmdline, bool doDiff) } setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigMixerProfile(currentMixerProfileIndexSave); cliPrintHashLine("restore original profile selection"); cliPrintLinef("profile %d", currentProfileIndexSave + 1); - cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); + cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); #ifdef USE_CLI_BATCH batchModeEnabled = false; diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 1a7ced85157..e0f21442db3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -321,6 +321,7 @@ void readEEPROM(void) setConfigProfile(getConfigProfile()); setConfigBatteryProfile(getConfigBatteryProfile()); + setConfigMixerProfile(getConfigMixerProfile()); validateAndFixConfig(); activateConfig(); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 204d54a2b4f..04974eb22d1 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -62,12 +62,12 @@ 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 +#ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP bool MCFW_hotswap_unavailable = false; #else bool MCFW_hotswap_unavailable = true; @@ -88,6 +88,7 @@ bool OutputProfileHotSwitch(int profile_index) LOG_INFO(PWM, "navModesEnabled"); return false; } + //TODO add check of each motor/servo is mapped before and after the switch if (!setConfigMixerProfile(profile_index)){ LOG_INFO(PWM, "failed to set config"); @@ -104,7 +105,7 @@ bool OutputProfileHotSwitch(int profile_index) pidInit(); pidInitFilters(); schedulePidGainsUpdate(); - navigationInit(); + // navigationInit(); //may need to initilize FW_HEADING_USE_YAW on rover or boat } return true; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 97d5296abde..4eee2a04154 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -30,5 +30,7 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) #define customServoMixers_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->ServoMixers) #define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config)) +#define mixerMotorMixersByIndex(index) (&(mixerProfiles(index)->MotorMixers)) +#define mixerServoMixersByIndex(index) (&(mixerProfiles(index)->ServoMixers)) bool OutputProfileHotSwitch(int profile_index); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 93ebeb9991c..674a7ab3552 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -87,6 +87,22 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, // } // } +void Reset_servoMixers(servoMixer_t *instance) +{ + for (int i = 0; i < MAX_SERVO_RULES; i++) + { + RESET_CONFIG(servoMixer_t, &instance[i], + .targetChannel = 0, + .inputSource = 0, + .rate = 0, + .speed = 0 +#ifdef USE_PROGRAMMING_FRAMEWORK + ,.conditionId = -1 +#endif + ); + } +} + PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3); void pgResetFn_servoParams(servoParam_t *instance) diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 841415ea234..cf303db6a4f 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -151,6 +151,7 @@ typedef struct servoMetadata_s { extern int16_t servo[MAX_SUPPORTED_SERVOS]; +void Reset_servoMixers(servoMixer_t* instance); bool isServoOutputEnabled(void); void setServoOutputEnabled(bool flag); bool isMixerUsingServos(void); diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 34387de6c8d..3f4d7618340 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -23,7 +23,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM -#ifdef MATEKF405VTOL +#ifdef MATEKF405VTOL//development purpose, TODO: remove it in the release DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 UP(2,1) diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index f0f1c8bc87f..e9aff02df4b 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -197,5 +197,5 @@ #define MAX_PWM_OUTPUT_PORTS 6 #ifdef MATEKF405VTOL -#define ENABLE_MCFW_MIXER_PROFILE_HOTSWAP +#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP #endif \ No newline at end of file diff --git a/src/main/target/MATEKF405TE/CMakeLists.txt b/src/main/target/MATEKF405TE/CMakeLists.txt index ce0150c7d3e..d8fc13c77bc 100644 --- a/src/main/target/MATEKF405TE/CMakeLists.txt +++ b/src/main/target/MATEKF405TE/CMakeLists.txt @@ -1,2 +1,3 @@ target_stm32f405xg(MATEKF405TE) target_stm32f405xg(MATEKF405TE_SD) +target_stm32f405xg(MATEKF405TE_SD_VTOL) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 70561e001f3..54e485b3c99 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -25,6 +25,18 @@ #include "drivers/timer_def_stm32f4xx.h" timerHardware_t timerHardware[] = { +#ifdef MATEKF405TE_SD_VTOL +//using d-shot on motors seems to have problems on s3,maybe dma related,maybe my board problem + DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S4 D(2,1,6) UP256 + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 +#else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2,6,0) UP256 @@ -34,7 +46,7 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 - +#endif DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index 93ab3345def..e51a6d9a206 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -180,3 +180,6 @@ #define USE_DSHOT #define USE_ESC_SENSOR +#ifdef MATEKF405TE_SD_VTOL +#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP +#endif \ No newline at end of file From 67390e12edbbdcf4912cf49a2267b4096d983c08 Mon Sep 17 00:00:00 2001 From: "haoxiang.qiu" Date: Mon, 14 Nov 2022 18:43:16 +0900 Subject: [PATCH 09/60] mixprofile development --- src/main/drivers/pwm_mapping.c | 19 ++-- src/main/drivers/pwm_mapping.h | 10 -- src/main/drivers/pwm_output.c | 4 - src/main/drivers/pwm_output.h | 1 - src/main/fc/cli.c | 33 ++---- src/main/fc/fc_msp.c | 1 - src/main/flight/mixer.c | 29 +----- src/main/flight/mixer.h | 22 +--- src/main/flight/mixer_profile.c | 137 ++++++++++++++++--------- src/main/programming/logic_condition.c | 6 +- src/main/target/MATEKF405TE/target.c | 2 +- src/main/target/MATEKF405TE/target.h | 11 +- 12 files changed, 119 insertions(+), 156 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 744e9fa2ddd..3707c716d12 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -47,6 +47,13 @@ enum { MAP_TO_SERVO_OUTPUT, }; +typedef struct { + int maxTimMotorCount; + int maxTimServoCount; + const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; + const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; +} timMotorServoHardware_t; + static pwmInitError_e pwmInitError = PWM_INIT_ERROR_NONE; static const char * pwmInitErrorMsg[] = { @@ -220,12 +227,10 @@ static void timerHardwareOverride(timerHardware_t * timer) { void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos) { - LOG_INFO(PWM, "pwmBuildTimerOutputList"); timOutputs->maxTimMotorCount = 0; timOutputs->maxTimServoCount = 0; uint8_t motorCount = getMotorCount(); - LOG_INFO(PWM, "motorCount %d", motorCount); uint8_t motorIdx = 0; for (int idx = 0; idx < timerHardwareCount; idx++) { @@ -387,6 +392,7 @@ bool pwmMotorAndServoInit(void) // Build temporary timer mappings for motor and servo pwmBuildTimerOutputList(&timOutputs, isMixerUsingServos()); + // At this point we have built tables of timers suitable for motor and servo mappings // Now we can actually initialize them according to motor/servo count from mixer pwmInitMotors(&timOutputs); @@ -394,12 +400,3 @@ bool pwmMotorAndServoInit(void) return (pwmInitError == PWM_INIT_ERROR_NONE); } - -// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs) -// { -// resetAllocatedOutputPortCount(); -// pwmInitMotors(timOutputs); -// pwmInitServos(timOutputs); - -// return (pwmInitError == PWM_INIT_ERROR_NONE); -// } \ No newline at end of file diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 8563d2fd953..4f46f14f9f5 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -18,7 +18,6 @@ #pragma once #include "drivers/io_types.h" -#include "drivers/timer.h" #include "flight/mixer_profile.h" #include "flight/servos.h" @@ -62,13 +61,6 @@ typedef enum { PWM_INIT_ERROR_TIMER_INIT_FAILED, } pwmInitError_e; -typedef struct { - int maxTimMotorCount; - int maxTimServoCount; - const timerHardware_t * timMotors[MAX_PWM_OUTPUT_PORTS]; - const timerHardware_t * timServos[MAX_PWM_OUTPUT_PORTS]; -} timMotorServoHardware_t; - typedef struct rangefinderIOConfig_s { ioTag_t triggerTag; ioTag_t echoTag; @@ -79,9 +71,7 @@ typedef struct { bool isDSHOT; } motorProtocolProperties_t; -void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerUsingServos); bool pwmMotorAndServoInit(void); -// bool pwmMotorAndServoHotInit(timMotorServoHardware_t* timOutputs); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); const char * getPwmInitErrorMessage(void); diff --git a/src/main/drivers/pwm_output.c b/src/main/drivers/pwm_output.c index 7b4891bf85b..658c3f5cb55 100644 --- a/src/main/drivers/pwm_output.c +++ b/src/main/drivers/pwm_output.c @@ -113,10 +113,6 @@ static uint8_t commandsBuff[DHSOT_COMMAND_QUEUE_SIZE]; static currentExecutingCommand_t currentExecutingCommand; #endif -void resetAllocatedOutputPortCount(void){ - allocatedOutputPortCount = 0; -} - static void pwmOutConfigTimer(pwmOutputPort_t * p, TCH_t * tch, uint32_t hz, uint16_t period, uint16_t value) { p->tch = tch; diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index d2ef60690f2..b3c0fa6be0e 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -34,7 +34,6 @@ void pwmRequestMotorTelemetry(int motorIndex); ioTag_t pwmGetMotorPinTag(int motorIndex); -void resetAllocatedOutputPortCount(void); void pwmWriteMotor(uint8_t index, uint16_t value); void pwmShutdownPulsesForAllMotors(uint8_t motorCount); void pwmCompleteMotorUpdate(void); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 34932c58c39..025e98d00f6 100644 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -81,7 +81,6 @@ bool cliMode = false; #include "flight/failsafe.h" #include "flight/imu.h" -// #include "flight/mixer.h" #include "flight/mixer_profile.h" #include "flight/pid.h" #include "flight/servos.h" @@ -283,7 +282,6 @@ typedef enum { DUMP_MASTER = (1 << 0), DUMP_PROFILE = (1 << 1), DUMP_BATTERY_PROFILE = (1 << 2), - // DUMP_RATES = (1 << 3), DUMP_MIXER_PROFILE = (1 << 3), DUMP_ALL = (1 << 4), DO_DIFF = (1 << 5), @@ -1864,7 +1862,6 @@ static void cliServoMix(char *cmdline) printServoMix(DUMP_MASTER, customServoMixers(0), NULL); } else if (sl_strncasecmp(cmdline, "reset", 5) == 0) { // erase custom mixer - // pgResetCopy(customServoMixersMutable(0), PG_SERVO_MIXER); Reset_servoMixers(customServoMixersMutable(0)); } else { enum {RULE = 0, TARGET, INPUT, RATE, SPEED, CONDITION, ARGS_COUNT}; @@ -3626,17 +3623,6 @@ static void printConfig(const char *cmdline, bool doDiff) } cliPrintHashLine("resources"); - // printResource(dumpMask, &defaultConfig); - - // cliPrintHashLine("mixer"); - // cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray[0].throttle == 0.0f, "\r\nmmix reset\r\n"); - - // printMotorMix(dumpMask, primaryMotorMixer_CopyArray, primaryMotorMixer(0)); - - // print custom servo mixer if exists - // cliPrintHashLine("servo mixer"); - // cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n"); - // printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); // print servo parameters cliPrintHashLine("servo"); @@ -3720,34 +3706,39 @@ static void printConfig(const char *cmdline, bool doDiff) const int currentProfileIndexSave = getConfigProfile(); const int currentBatteryProfileIndexSave = getConfigBatteryProfile(); const int currentMixerProfileIndexSave = getConfigMixerProfile(); + for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) { + cliDumpMixerProfile(ii, dumpMask); + } for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { cliDumpProfile(ii, dumpMask); } for (int ii = 0; ii < MAX_BATTERY_PROFILE_COUNT; ++ii) { cliDumpBatteryProfile(ii, dumpMask); } - for (int ii = 0; ii < MAX_MIXER_PROFILE_COUNT; ++ii) { - cliDumpMixerProfile(ii, dumpMask); - } setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); setConfigMixerProfile(currentMixerProfileIndexSave); cliPrintHashLine("restore original profile selection"); - cliPrintLinef("profile %d", currentProfileIndexSave + 1); cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); + cliPrintLinef("profile %d", currentProfileIndexSave + 1); + cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); #ifdef USE_CLI_BATCH batchModeEnabled = false; #endif } else { // dump just the current profiles + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); cliDumpProfile(getConfigProfile(), dumpMask); cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); - cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); } } + if (dumpMask & DUMP_MIXER_PROFILE) { + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); + } + if (dumpMask & DUMP_PROFILE) { cliDumpProfile(getConfigProfile(), dumpMask); } @@ -3756,10 +3747,6 @@ static void printConfig(const char *cmdline, bool doDiff) cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } - if (dumpMask & DUMP_MIXER_PROFILE) { - cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); - } - if ((dumpMask & DUMP_MASTER) || (dumpMask & DUMP_ALL)) { cliPrintHashLine("save configuration\r\nsave"); } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 587651b21d7..66737d43798 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -3132,7 +3132,6 @@ static bool mspSettingInfoCommand(sbuf_t *dst, sbuf_t *src) sbufWriteU8(dst, MAX_MIXER_PROFILE_COUNT); break; } - // If the setting uses a table, send each possible string (null terminated) if (SETTING_MODE(setting) == MODE_LOOKUP) { diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 37011845bc2..21e2a75545a 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -83,34 +83,6 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -// PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); - -// void pgResetFn_mixerProfiles(mixerProfile_t *instance) -// { -// for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { -// RESET_CONFIG(mixerProfile_t, &instance[i], -// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, -// .platformType = SETTING_PLATFORM_TYPE_DEFAULT, -// .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, -// ); -// motorMixer_t tmp_mixer = {.throttle=0,.roll=0,.pitch=0,.yaw=0}; -// for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { -// instance->MotorMixer[j] = tmp_mixer; -// } -// } -// } - -// PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_PROFILE, 5); - -// PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, -// .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, -// .platformType = SETTING_PLATFORM_TYPE_DEFAULT, -// .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, -// ); PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 9); @@ -436,6 +408,7 @@ void FAST_CODE writeMotors(void) // We don't define USE_DSHOT motorValue = motor[i]; #endif + pwmWriteMotor(i, motorValue); } } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 07d1ab248b9..5813e6a73ed 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -25,7 +25,6 @@ #define MAX_SUPPORTED_MOTORS 12 #endif - // Digital protocol has fixed values #define DSHOT_DISARM_COMMAND 0 #define DSHOT_MIN_THROTTLE 48 @@ -63,8 +62,6 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; -// PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); - typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; @@ -73,22 +70,6 @@ typedef struct mixerConfig_s { uint8_t outputMode; } mixerConfig_t; -// typedef struct mixerProfile_s { -// int8_t motorDirectionInverted; -// uint8_t platformType; -// bool hasFlaps; -// int16_t appliedMixerPreset; -// uint8_t outputMode; -// motorMixer_t MotorMixer[MAX_SUPPORTED_MOTORS]; -// } mixerProfile_t; - -// PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); -// #define mixerConfig() mixerProfiles(systemConfig()->current_mixer_profile_index) -// #define mixerConfigMutable() ((mixerProfile_t *)mixerConfig()) -// #define primaryMotorMixer(_index) (&((mixerConfig()->MotorMixer)[_index])) -// #define primaryMotorMixerMutable(_index) ((motorMixer_t *)primaryMotorMixer(_index)) -// extern motorMixer_t primaryMotorMixer_CopyArray[12]; - typedef struct reversibleMotorsConfig_s { uint16_t deadband_low; // min 3d value uint16_t deadband_high; // max 3d value @@ -142,8 +123,7 @@ void processServoAutotrim(const float dT); void processServoAutotrimMode(void); void processContinuousServoAutotrim(const float dT); void stopMotors(void); -void stopAndDisableMotors(void); void stopPwmAllMotors(void); void loadPrimaryMotorMixer(void); -bool areMotorsRunning(void); \ No newline at end of file +bool areMotorsRunning(void); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 04974eb22d1..cf41074e15f 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -59,13 +59,54 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) } } -// PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); +static int computeMotorCountByMixerProfileIndex(int index) +{ + int motorCount = 0; + for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + // check if done + if (mixerMotorMixersByIndex(index)[i]->throttle == 0.0f) { + break; + } + motorCount++; + } + return motorCount; +} + +static int computeServoCountByMixerProfileIndex(int index) +{ + int servoRuleCount = 0; + int minServoIndex = 255; + int maxServoIndex = 0; + for (int i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (mixerServoMixersByIndex(index)[i]->rate == 0) + break; + + if (mixerServoMixersByIndex(index)[i]->targetChannel < minServoIndex) { + minServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel; + } + + if (mixerServoMixersByIndex(index)[i]->targetChannel > maxServoIndex) { + maxServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel; + } + servoRuleCount++; + } + if (servoRuleCount) { + return 1 + maxServoIndex - minServoIndex; + } + else { + return 0; + } +} bool OutputProfileHotSwitch(int profile_index) { // does not work with timerHardwareOverride LOG_INFO(PWM, "OutputProfileHotSwitch"); - + if (profile_index >= MAX_MIXER_PROFILE_COUNT) {// sanity check + LOG_INFO(PWM, "invalid profile index"); + return false; + } //do not allow switching between multi rotor and non multi rotor #ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP bool MCFW_hotswap_unavailable = false; @@ -88,8 +129,12 @@ bool OutputProfileHotSwitch(int profile_index) LOG_INFO(PWM, "navModesEnabled"); return false; } - //TODO add check of each motor/servo is mapped before and after the switch - + //do not allow switching if motor or servos counts has changed + if ((getMotorCount() != computeMotorCountByMixerProfileIndex(profile_index)) || (getServoCount() != computeServoCountByMixerProfileIndex(profile_index))) + { + LOG_INFO(PWM, "motor/servo count will change"); + return false; + } if (!setConfigMixerProfile(profile_index)){ LOG_INFO(PWM, "failed to set config"); return false; @@ -110,47 +155,47 @@ bool OutputProfileHotSwitch(int profile_index) return true; } -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"); - } -} +// 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"); +// } +// } diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6a49290050e..054031e2033 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -46,15 +46,11 @@ #include "sensors/rangefinder.h" #include "flight/imu.h" #include "flight/pid.h" +#include "flight/mixer_profile.h" #include "drivers/io_port_expander.h" #include "io/osd_common.h" #include "sensors/diagnostics.h" -#include "flight/mixer_profile.h" -#include "flight/mixer.h" -#include "flight/servos.h" -#include "drivers/pwm_mapping.h" - #include "navigation/navigation.h" #include "navigation/navigation_private.h" diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 54e485b3c99..ba89313a946 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -26,7 +26,7 @@ timerHardware_t timerHardware[] = { #ifdef MATEKF405TE_SD_VTOL -//using d-shot on motors seems to have problems on s3,maybe dma related,maybe my board problem +//With INAV firmware, DSHOT can not work on S3, S5,S7 because of DMA clash, pls use ONESHOT or MULTISHOT and calibrate ESC PWM range.<-copied from matek website DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2,6,0) UP256 diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index e51a6d9a206..f8edb3d2ba5 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -18,6 +18,11 @@ #pragma once #define USE_TARGET_CONFIG +#ifdef MATEKF405TE_SD_VTOL +#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP +#define MATEKF405TE_SD +#endif + #if defined(MATEKF405TE_SD) # define TARGET_BOARD_IDENTIFIER "MF4T" # define USBD_PRODUCT_STRING "MatekF405TE_SD" @@ -178,8 +183,4 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT -#define USE_ESC_SENSOR - -#ifdef MATEKF405TE_SD_VTOL -#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP -#endif \ No newline at end of file +#define USE_ESC_SENSOR \ No newline at end of file From 6d861fdf3b603b8999541b4320a7aff2ab4344b5 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 16 Nov 2022 00:59:31 +0900 Subject: [PATCH 10/60] mixer profile dev --- src/main/flight/mixer_profile.c | 45 ++++++++++++++++++++++----------- 1 file changed, 30 insertions(+), 15 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index cf41074e15f..98b07c95229 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -62,9 +62,10 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) static int computeMotorCountByMixerProfileIndex(int index) { int motorCount = 0; + const motorMixer_t* temp_motormixers=mixerMotorMixersByIndex(index)[0]; for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { // check if done - if (mixerMotorMixersByIndex(index)[i]->throttle == 0.0f) { + if (temp_motormixers[i].throttle == 0.0f) { break; } motorCount++; @@ -77,18 +78,24 @@ static int computeServoCountByMixerProfileIndex(int index) int servoRuleCount = 0; int minServoIndex = 255; int maxServoIndex = 0; + + const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; for (int i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (mixerServoMixersByIndex(index)[i]->rate == 0) + // 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; - if (mixerServoMixersByIndex(index)[i]->targetChannel < minServoIndex) { - minServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel; + if (temp_servomixers[i].targetChannel < minServoIndex) { + minServoIndex = temp_servomixers[i].targetChannel; } - if (mixerServoMixersByIndex(index)[i]->targetChannel > maxServoIndex) { - maxServoIndex = mixerServoMixersByIndex(index)[i]->targetChannel; + if (temp_servomixers[i].targetChannel > maxServoIndex) { + maxServoIndex = temp_servomixers[i].targetChannel; } + // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); servoRuleCount++; } if (servoRuleCount) { @@ -103,8 +110,13 @@ bool OutputProfileHotSwitch(int profile_index) { // does not work with timerHardwareOverride LOG_INFO(PWM, "OutputProfileHotSwitch"); - if (profile_index >= MAX_MIXER_PROFILE_COUNT) {// sanity check - LOG_INFO(PWM, "invalid profile index"); + 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) + { return false; } //do not allow switching between multi rotor and non multi rotor @@ -120,33 +132,36 @@ bool OutputProfileHotSwitch(int profile_index) 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"); + LOG_INFO(PWM, "mixer MCFW_hotswap_unavailable"); return false; } //do not allow switching in navigation mode if (ARMING_FLAG(ARMED) && navigationInAnyMode()){ - LOG_INFO(PWM, "navModesEnabled"); + LOG_INFO(PWM, "mixer switch navModesEnabled"); 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, "motor/servo count will change"); + LOG_INFO(PWM, "mixer switch 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; } if (!setConfigMixerProfile(profile_index)){ - LOG_INFO(PWM, "failed to set config"); + LOG_INFO(PWM, "mixer switch failed to set config"); return false; } // stopMotors(); - writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors with out delay + writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors without delay servosInit(); mixerUpdateStateFlags(); mixerInit(); if(old_platform_type!=mixerConfig()->platformType) - { + { + LOG_INFO(PWM, "mixer switch pidInit"); pidInit(); pidInitFilters(); schedulePidGainsUpdate(); From 0e94190eb18294aeff852e192f825ee11a424652 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 16 Nov 2022 13:10:28 +0900 Subject: [PATCH 11/60] updates in mixer_profile --- src/main/flight/mixer_profile.c | 47 ++++++++++++++++++++++++++------- 1 file changed, 37 insertions(+), 10 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 98b07c95229..efbd945ad4e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -15,14 +15,16 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "fc/fc_core.h" #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/settings.h" -#include "common/log.h" - +#include "programming/logic_condition.h" #include "navigation/navigation.h" +#include "common/log.h" + PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); void pgResetFn_mixerProfiles(mixerProfile_t *instance) @@ -106,6 +108,29 @@ static int computeServoCountByMixerProfileIndex(int index) } } +//pid init will be done by the following pid profile change +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; +} + bool OutputProfileHotSwitch(int profile_index) { // does not work with timerHardwareOverride @@ -119,6 +144,14 @@ 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 + return false; + } + //do not allow switching in navigation mode + if (ARMING_FLAG(ARMED) && navigationInAnyMode()){ + LOG_INFO(PWM, "mixer switch failed, navModesEnabled"); + return false; + } //do not allow switching between multi rotor and non multi rotor #ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP bool MCFW_hotswap_unavailable = false; @@ -135,16 +168,10 @@ bool OutputProfileHotSwitch(int profile_index) LOG_INFO(PWM, "mixer MCFW_hotswap_unavailable"); return false; } - - //do not allow switching in navigation mode - if (ARMING_FLAG(ARMED) && navigationInAnyMode()){ - LOG_INFO(PWM, "mixer switch navModesEnabled"); - 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 motor/servo count will change"); + LOG_INFO(PWM, "mixer switch failed, 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; @@ -159,7 +186,7 @@ bool OutputProfileHotSwitch(int profile_index) mixerUpdateStateFlags(); mixerInit(); - if(old_platform_type!=mixerConfig()->platformType) + if(old_platform_type!=mixerConfig()->platformType && CheckIfPidInitNeededInSwitch()) { LOG_INFO(PWM, "mixer switch pidInit"); pidInit(); From e0db9ae4c920530d405822f186f3b90f6aaddd2b Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 17 Nov 2022 00:20:15 +0900 Subject: [PATCH 12/60] navigation init on mixer profile --- src/main/flight/mixer_profile.c | 18 +++++++++++------- src/main/navigation/navigation.c | 24 ++++++++++++++---------- src/main/navigation/navigation.h | 1 + src/main/programming/logic_condition.c | 1 + 4 files changed, 27 insertions(+), 17 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index efbd945ad4e..8ca8cca8821 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -186,13 +186,17 @@ bool OutputProfileHotSwitch(int profile_index) mixerUpdateStateFlags(); mixerInit(); - if(old_platform_type!=mixerConfig()->platformType && CheckIfPidInitNeededInSwitch()) - { - LOG_INFO(PWM, "mixer switch pidInit"); - pidInit(); - pidInitFilters(); - schedulePidGainsUpdate(); - // navigationInit(); //may need to initilize FW_HEADING_USE_YAW on rover or boat + if(old_platform_type!=mixerConfig()->platformType) + { + navigationYawControlInit(); + if (CheckIfPidInitNeededInSwitch()) + { + LOG_INFO(PWM, "mixer switch pidInit"); + pidInit(); + pidInitFilters(); + schedulePidGainsUpdate(); + navigationUsePIDs(); + } } return true; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 08c66766860..c802cbe1801 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4108,6 +4108,18 @@ void navigationUsePIDs(void) ); } +void navigationYawControlInit(void){ + 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); + } +} + void navigationInit(void) { /* Initial state */ @@ -4141,16 +4153,8 @@ void navigationInit(void) /* Use system config */ navigationUsePIDs(); - - 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); - } + navigationYawControlInit(); + #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) /* configure WP missions at boot */ #ifdef USE_MULTI_MISSION diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 13f9f58bc92..b2872822e2f 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -472,6 +472,7 @@ typedef struct { } navSystemStatus_t; void navigationUsePIDs(void); +void navigationYawControlInit(void); void navigationInit(void); /* Position estimator update functions */ diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 054031e2033..b0a7b83ecb8 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -354,6 +354,7 @@ static int logicConditionCompute( pidInit(); pidInitFilters(); schedulePidGainsUpdate(); + navigationUsePIDs(); profileChanged = true; } return profileChanged; From 8b72ffa3e2598072da4c7b503f5ceb8d63a2df27 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 22 Nov 2022 02:33:05 +0900 Subject: [PATCH 13/60] move motor stop feature to mixer config --- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 4 ++-- src/main/fc/rc_controls.c | 3 ++- src/main/fc/settings.yaml | 5 +++++ src/main/flight/mixer.c | 8 ++++++-- src/main/flight/mixer.h | 2 ++ src/main/sensors/battery.c | 2 +- src/main/target/ALIENFLIGHTF4/target.h | 2 +- src/main/target/ALIENFLIGHTNGF7/target.h | 2 +- src/main/telemetry/frsky_d.c | 2 +- 10 files changed, 22 insertions(+), 10 deletions(-) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index ab5a6fff721..5b8fe8fdfd8 100644 --- a/src/main/fc/config.h +++ b/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_MOTOR_STOP = 1 << 4, + FEATURE_UNUSED_12 = 1 << 4, FEATURE_UNUSED_1 = 1 << 5, // was FEATURE_SERVO_TILT was FEATURE_DYNAMIC_FILTERS FEATURE_SOFTSERIAL = 1 << 6, FEATURE_GPS = 1 << 7, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4253c995a30..a63d5aeac1d 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -634,7 +634,7 @@ void processRx(timeUs_t currentTimeUs) const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); // When armed and motors aren't spinning, do beeps periodically - if (ARMING_FLAG(ARMED) && feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY)) { + if (ARMING_FLAG(ARMED) && ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY)) { static bool armedBeeperOn = false; if (throttleStatus == THROTTLE_LOW) { @@ -762,7 +762,7 @@ void processRx(timeUs_t currentTimeUs) else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleStatus == THROTTLE_LOW) { if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { - if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) { + if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { ENABLE_STATE(ANTI_WINDUP); } else { diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 8ccc9c63de2..97a85ccc07f 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -49,6 +49,7 @@ #include "flight/pid.h" #include "flight/failsafe.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/beeper.h" @@ -209,7 +210,7 @@ void processRcStickPositions(throttleStatus_e throttleStatus) bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); emergencyArmingUpdate(armingSwitchIsActive); - if (STATE(AIRPLANE) && feature(FEATURE_MOTOR_STOP) && armingConfig()->fixed_wing_auto_arm) { + if (STATE(AIRPLANE) && ifMotorstopFeatureEnabled() && armingConfig()->fixed_wing_auto_arm) { // Auto arm on throttle when using fixedwing and motorstop if (throttleStatus != THROTTLE_LOW) { tryArm(); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6c94747e043..0dfc1fdde16 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1178,6 +1178,11 @@ groups: default_value: "AUTO" field: mixer_config.outputMode table: output_mode + - name: motorstop_feature + description: "If enabled, motor will stop when throttle is low" + default_value: OFF + field: mixer_config.motorstopFeature + type: bool - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 21e2a75545a..82786044d01 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -117,6 +117,10 @@ static void computeMotorCount(void) } } +bool ifMotorstopFeatureEnabled(void){ + return mixerConfig()->motorstopFeature; +} + uint8_t getMotorCount(void) { return motorCount; } @@ -212,7 +216,7 @@ void mixerResetDisarmedMotors(void) reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; - if (feature(FEATURE_MOTOR_STOP)) { + if (ifMotorstopFeatureEnabled()) { motorValueWhenStopped = motorZeroCommand; } else { motorValueWhenStopped = getThrottleIdleValue(); @@ -444,7 +448,7 @@ static int getReversibleMotorsThrottleDeadband(void) directionValue = reversibleMotorsConfig()->deadband_high; } - return feature(FEATURE_MOTOR_STOP) ? reversibleMotorsConfig()->neutral : directionValue; + return ifMotorstopFeatureEnabled() ? reversibleMotorsConfig()->neutral : directionValue; } void FAST_CODE mixTable() diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 5813e6a73ed..eacbc69311a 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -68,6 +68,7 @@ typedef struct mixerConfig_s { bool hasFlaps; int16_t appliedMixerPreset; uint8_t outputMode; + bool motorstopFeature; } mixerConfig_t; typedef struct reversibleMotorsConfig_s { @@ -106,6 +107,7 @@ extern int16_t motor[MAX_SUPPORTED_MOTORS]; extern int16_t motor_disarmed[MAX_SUPPORTED_MOTORS]; extern int mixerThrottleCommand; +bool ifMotorstopFeatureEnabled(void); int getThrottleIdleValue(void); int16_t getThrottlePercent(void); uint8_t getMotorCount(void); diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index adeb9ad3f05..b3222518cb8 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -558,7 +558,7 @@ void currentMeterUpdate(timeUs_t timeDelta) if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; } else { - throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + throttleOffset = ((throttleStatus == THROTTLE_LOW) && ifMotorstopFeatureEnabled()) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 09a27576428..5527b45a1d3 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -152,7 +152,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 59ad543c3b9..5971f3d8484 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -165,7 +165,7 @@ #define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_MOTOR_STOP | FEATURE_BLACKBOX) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX) #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_UART SERIAL_PORT_USART2 #define RX_CHANNELS_TAER diff --git a/src/main/telemetry/frsky_d.c b/src/main/telemetry/frsky_d.c index 0bc65d3457d..83a87375f0c 100644 --- a/src/main/telemetry/frsky_d.c +++ b/src/main/telemetry/frsky_d.c @@ -197,7 +197,7 @@ static void sendThrottleOrBatterySizeAsRpm(void) if (ARMING_FLAG(ARMED)) { const throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); uint16_t throttleForRPM = rcCommand[THROTTLE] / BLADE_NUMBER_DIVIDER; - if (throttleStatus == THROTTLE_LOW && feature(FEATURE_MOTOR_STOP)) + if (throttleStatus == THROTTLE_LOW && ifMotorstopFeatureEnabled()) throttleForRPM = 0; serialize16(throttleForRPM); } else { From 203bb2a47150bf7525c341f7be7fb6151af75cb4 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 30 Jun 2023 04:01:01 +0900 Subject: [PATCH 14/60] mixer and pid profile linking --- src/main/fc/config.c | 5 ++ src/main/fc/config.h | 2 +- src/main/fc/settings.yaml | 7 ++- src/main/flight/mixer.c | 9 ++- src/main/flight/mixer.h | 10 +--- src/main/flight/mixer_profile.c | 78 +++++++++++++------------- src/main/flight/mixer_profile.h | 9 +++ src/main/flight/servos.h | 2 +- src/main/navigation/navigation.c | 14 ++++- src/main/navigation/navigation.h | 2 +- src/main/programming/logic_condition.c | 12 +--- src/main/programming/logic_condition.h | 1 + src/main/target/MATEKF405TE/target.h | 3 +- src/main/target/common.h | 2 +- 14 files changed, 87 insertions(+), 69 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 689ecc5bd39..8822e20254a 100755 --- a/src/main/fc/config.c +++ b/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); } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 003cb9fc985..bafa5c0cbe0 100644 --- a/src/main/fc/config.h +++ b/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, diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fca009364cb..9becd267704 100644 --- a/src/main/fc/settings.yaml +++ b/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 diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b8fafec71a5..a0f62c6dc31 100644 --- a/src/main/flight/mixer.c +++ b/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. } diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index bbf06696d92..871332447ef 100644 --- a/src/main/flight/mixer.h +++ b/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); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 8ca8cca8821..c8e2c596c8b 100644 --- a/src/main/flight/mixer_profile.c +++ b/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 -static bool CheckIfPidInitNeededInSwitch(void) +void SwitchPidProfileByMixerProfile() { - 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; + 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 @@ -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(); - writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand);//stop motors without delay + stopMotorsNoDelay(); servosInit(); mixerUpdateStateFlags(); mixerInit(); - if(old_platform_type!=mixerConfig()->platformType) + if(mixerConfig()->PIDProfileLinking) { - navigationYawControlInit(); - if (CheckIfPidInitNeededInSwitch()) - { - LOG_INFO(PWM, "mixer switch pidInit"); - pidInit(); - pidInitFilters(); - schedulePidGainsUpdate(); - navigationUsePIDs(); - } + SwitchPidProfileByMixerProfile(); } 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; +// } \ No newline at end of file diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 4eee2a04154..7da2c073d96 100644 --- a/src/main/flight/mixer_profile.h +++ b/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]; diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index cf303db6a4f..c8dbd06d1ee 100644 --- a/src/main/flight/servos.h +++ b/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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c340076adce..ca548ba4e84 100644 --- a/src/main/navigation/navigation.c +++ b/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 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 4e419dd14c8..57d7f49c8e3 100644 --- a/src/main/navigation/navigation.h +++ b/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 */ diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f639dc49c1c..cdb3bc7d7ec 100644 --- a/src/main/programming/logic_condition.c +++ b/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); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 779dbb1b985..446532fb291 100644 --- a/src/main/programming/logic_condition.h +++ b/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 { diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index f8edb3d2ba5..73b38e0721d 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -183,4 +183,5 @@ #define USE_SERIAL_4WAY_BLHELI_INTERFACE #define USE_DSHOT -#define USE_ESC_SENSOR \ No newline at end of file +#define USE_ESC_SENSOR + diff --git a/src/main/target/common.h b/src/main/target/common.h index 4a605235488..70916fdbd30 100644 --- a/src/main/target/common.h +++ b/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 From 2fb9601dd6335e76a6fa0e564c699668f0230dce Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 1 Jul 2023 03:29:10 +0900 Subject: [PATCH 15/60] mixprofile transition support --- src/main/drivers/pwm_mapping.c | 6 +-- src/main/fc/config.c | 3 -- src/main/fc/fc_core.c | 2 +- src/main/fc/fc_init.c | 4 +- src/main/fc/fc_msp_box.c | 2 + src/main/fc/rc_modes.h | 2 + src/main/flight/mixer.c | 35 +++++++++------ src/main/flight/mixer_profile.c | 61 ++++++++++++++++---------- src/main/flight/mixer_profile.h | 3 ++ src/main/flight/pid.c | 6 +-- src/main/flight/servos.c | 6 ++- src/main/flight/servos.h | 2 +- src/main/navigation/navigation.c | 10 ++--- src/main/programming/logic_condition.c | 2 +- 14 files changed, 86 insertions(+), 58 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 9bf037242f2..653404a4e28 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/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 diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 8822e20254a..27ed9eddd00 100755 --- a/src/main/fc/config.c +++ b/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(); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 2c0f8ce463d..b5bb2fc3227 100755 --- a/src/main/fc/fc_core.c +++ b/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); } diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 7c36b49f0ba..a5b270a5185 100644 --- a/src/main/fc/fc_init.c +++ b/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(); - mixerUpdateStateFlags(); - mixerInit(); + mixerConfigInit(); // 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 1fa7a712392..8caa76b9231 100644 --- a/src/main/fc/fc_msp_box.c +++ b/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 } }; diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 93b17e6e2fe..43de336b98b 100644 --- a/src/main/fc/rc_modes.h +++ b/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; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index a0f62c6dc31..6a97ab4e17d 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 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); @@ -434,7 +434,7 @@ void stopMotorsNoDelay(void) void stopMotors(void) { stopMotorsNoDelay(); - + delay(50); // give the timers and ESCs a chance to react. } @@ -592,14 +592,21 @@ void FAST_CODE mixTable(void) } else { 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; diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index c8e2c596c8b..f5f6179d858 100644 --- a/src/main/flight/mixer_profile.c +++ b/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(); - mixerUpdateStateFlags(); - mixerInit(); + mixerConfigInit(); - if(mixerConfig()->PIDProfileLinking) - { - SwitchPidProfileByMixerProfile(); - } return true; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 7da2c073d96..4e9d44b3534 100644 --- a/src/main/flight/mixer_profile.h +++ b/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); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 82819d4de3b..eb9abb4b55e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1215,9 +1215,9 @@ void pidInit(void) if (pidProfile()->pidControllerType == PID_TYPE_AUTO) { if ( - mixerConfig()->platformType == PLATFORM_AIRPLANE || - mixerConfig()->platformType == PLATFORM_BOAT || - mixerConfig()->platformType == PLATFORM_ROVER + currentMixerConfig.platformType == PLATFORM_AIRPLANE || + currentMixerConfig.platformType == PLATFORM_BOAT || + currentMixerConfig.platformType == PLATFORM_ROVER ) { usedPidControllerType = PID_TYPE_PIFF; } else { diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 6a16e54be09..f46619753a4 100755 --- a/src/main/flight/servos.c +++ b/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 diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index c8dbd06d1ee..1dd65912218 100644 --- a/src/main/flight/servos.h +++ b/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; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ca548ba4e84..436c3c3796e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4179,8 +4179,8 @@ void navigationUsePIDs(void) void navigationInitYawControl(void){ if ( - mixerConfig()->platformType == PLATFORM_BOAT || - mixerConfig()->platformType == PLATFORM_ROVER || + currentMixerConfig.platformType == PLATFORM_BOAT || + 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 || - mixerConfig()->platformType == PLATFORM_ROVER || + currentMixerConfig.platformType == PLATFORM_BOAT || + 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) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index cdb3bc7d7ec..6a08871508d 100644 --- a/src/main/programming/logic_condition.c +++ b/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: From 1839a458d2f849fe0f06451a50435e87d02641c6 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 1 Jul 2023 13:01:37 +0900 Subject: [PATCH 16/60] add mode to swtich mixer profile --- src/main/fc/fc_msp_box.c | 10 +++++++++- src/main/flight/mixer_profile.c | 25 ++++++++++++++----------- src/main/flight/mixer_profile.h | 3 ++- src/main/programming/programming_task.c | 2 ++ 4 files changed, 27 insertions(+), 13 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 8caa76b9231..c6ad2cfc0ee 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -349,6 +349,11 @@ void initActiveBoxIds(void) ADD_ACTIVE_BOX(BOXTURTLE); } #endif + +#if (MAX_MIXER_PROFILE_COUNT > 1) + ADD_ACTIVE_BOX(BOXMIXERPROFILE); + ADD_ACTIVE_BOX(BOXMIXERTRANSITION); +#endif } #define IS_ENABLED(mask) ((mask) == 0 ? 0 : 1) @@ -415,7 +420,10 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #ifdef USE_MULTI_MISSION 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))); +#endif memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { if (activeBoxes[activeBoxIds[i]]) { diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index f5f6179d858..d92761f7665 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -67,13 +67,9 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) } } -void loadMixerConfig(void) { +void mixerConfigInit(void){ currentMixerProfileIndex=getConfigMixerProfile(); currentMixerConfig=*mixerConfig(); -} - -void mixerConfigInit(void){ - loadMixerConfig(); servosInit(); mixerUpdateStateFlags(); mixerInit(); @@ -134,25 +130,32 @@ static int computeServoCountByMixerProfileIndex(int index) } } + +void outputProfileUpdateTask(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); + outputProfileHotSwitch((int) IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); + +} + //switch mixerprofile without reboot -bool OutputProfileHotSwitch(int profile_index) +bool outputProfileHotSwitch(int profile_index) { static bool allow_hot_switch = true; // does not work with timerHardwareOverride,need to set mixerConfig()->outputMode == OUTPUT_MODE_AUTO - LOG_INFO(PWM, "OutputProfileHotSwitch"); + // LOG_INFO(PWM, "OutputProfileHotSwitch"); if (!allow_hot_switch) { return false; } + if (currentMixerProfileIndex == profile_index) + { + return false; + } if (profile_index < 0 || profile_index >= MAX_MIXER_PROFILE_COUNT) { // sanity check LOG_INFO(PWM, "invalid mixer profile index"); return false; } - if (currentMixerProfileIndex == 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; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 4e9d44b3534..4302c60b40e 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -44,5 +44,6 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) #define mixerMotorMixersByIndex(index) (&(mixerProfiles(index)->MotorMixers)) #define mixerServoMixersByIndex(index) (&(mixerProfiles(index)->ServoMixers)) -bool OutputProfileHotSwitch(int profile_index); +bool outputProfileHotSwitch(int profile_index); void mixerConfigInit(void); +void outputProfileUpdateTask(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/programming/programming_task.c b/src/main/programming/programming_task.c index 69f1e929445..e5b6642d243 100644 --- a/src/main/programming/programming_task.c +++ b/src/main/programming/programming_task.c @@ -26,8 +26,10 @@ #include "programming/logic_condition.h" #include "programming/pid.h" +#include "flight/mixer_profile.h" void programmingFrameworkUpdateTask(timeUs_t currentTimeUs) { programmingPidUpdateTask(currentTimeUs); + outputProfileUpdateTask(currentTimeUs); logicConditionUpdateTask(currentTimeUs); } \ No newline at end of file From fe30a8a8ce1883b532607e6ca977190b5e39828e Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 23 Jul 2023 00:24:27 +0900 Subject: [PATCH 17/60] 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 From fb73715e342df29d007ea2e11f876c34fd45357e Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 24 Jul 2023 12:05:37 +0900 Subject: [PATCH 18/60] minor fix --- src/main/flight/servos.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index c08ee293eeb..45d32bf7ff5 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -131,7 +131,7 @@ static biquadFilter_t servoFilter[MAX_SUPPORTED_SERVOS]; static bool servoFilterIsSet; static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; -static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; +static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; STATIC_FASTRAM pt1Filter_t rotRateFilter; STATIC_FASTRAM pt1Filter_t targetRateFilter; @@ -377,7 +377,7 @@ void servoMixer(float dT) * 10 = 100us/s -> full sweep (from 1000 to 2000) is performed in 10s * 100 = 1000us/s -> full sweep in 1s */ - int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], input[from], currentServoMixer[i].speed * 10, dT); + int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i+MAX_SERVO_RULES*currentMixerProfileIndex], input[from], currentServoMixer[i].speed * 10, dT); servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } @@ -615,6 +615,10 @@ void processServoAutotrim(const float dT) { return; } #endif + if(!STATE(AIRPLANE)) + { + return; + } if (feature(FEATURE_FW_AUTOTRIM)) { processContinuousServoAutotrim(dT); } else { From e0abc47c1031769eda82c60c68ce10082d624adf Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 24 Jul 2023 18:45:56 +0900 Subject: [PATCH 19/60] minor fix --- src/main/target/MATEKF405TE/target.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index ba89313a946..f39e03e6c34 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -36,6 +36,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_MC_MOTOR, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_MC_MOTOR, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) #else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 @@ -46,10 +50,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 -#endif DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) +#endif DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM From d220dcb4794341413ef76494f1f86afecfc1a14d Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 24 Jul 2023 19:31:45 +0900 Subject: [PATCH 20/60] mend --- src/main/target/MATEKF405TE/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index f39e03e6c34..90afefba4b6 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -37,8 +37,8 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_MC_MOTOR, 0, 0), // S9 DMA NONE - DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_MC_MOTOR, 0, 0), // S10 DMA NONE + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) #else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 From 18cefccd308a16a783464fdbe200f27fde46bc91 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 24 Jul 2023 19:32:36 +0900 Subject: [PATCH 21/60] mend --- src/main/target/MATEKF405TE/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 90afefba4b6..bee9ebee56d 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -37,9 +37,9 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S9 DMA NONE + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S11 D(1,0,2) #else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 From 4d8136176f44978b64029b9fd6f56dcb4faf95b8 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 24 Jul 2023 23:22:59 +0900 Subject: [PATCH 22/60] mixer profile switching servo speed support --- src/main/flight/mixer_profile.c | 61 +++++++++++++++++---------------- src/main/flight/servos.c | 54 +++++++++++++++++------------ 2 files changed, 64 insertions(+), 51 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index c6864094cc0..d0585a023f9 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -61,7 +61,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .rate = 0, .speed = 0 #ifdef USE_PROGRAMMING_FRAMEWORK - ,.conditionId = -1 + ,.conditionId = -1, #endif ); } @@ -98,34 +98,34 @@ static int computeMotorCountByMixerProfileIndex(int index) return motorCount; } -static int computeServoCountByMixerProfileIndex(int index) -{ - int servoRuleCount = 0; - int minServoIndex = 255; - int maxServoIndex = 0; - - const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; - for (int i = 0; i < MAX_SERVO_RULES; i++) { - if (temp_servomixers[i].rate == 0) - break; - - if (temp_servomixers[i].targetChannel < minServoIndex) { - minServoIndex = temp_servomixers[i].targetChannel; - } - - if (temp_servomixers[i].targetChannel > maxServoIndex) { - maxServoIndex = temp_servomixers[i].targetChannel; - } - // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); - servoRuleCount++; - } - if (servoRuleCount) { - return 1 + maxServoIndex - minServoIndex; - } - else { - return 0; - } -} +// static int computeServoCountByMixerProfileIndex(int index) +// { +// int servoRuleCount = 0; +// int minServoIndex = 255; +// int maxServoIndex = 0; + +// const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; +// for (int i = 0; i < MAX_SERVO_RULES; i++) { +// if (temp_servomixers[i].rate == 0) +// break; + +// if (temp_servomixers[i].targetChannel < minServoIndex) { +// minServoIndex = temp_servomixers[i].targetChannel; +// } + +// if (temp_servomixers[i].targetChannel > maxServoIndex) { +// maxServoIndex = temp_servomixers[i].targetChannel; +// } +// // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); +// servoRuleCount++; +// } +// if (servoRuleCount) { +// return 1 + maxServoIndex - minServoIndex; +// } +// else { +// return 0; +// } +// } bool checkMixerProfileHotSwitchAvalibility(void) { @@ -160,7 +160,8 @@ bool checkMixerProfileHotSwitchAvalibility(void) return false; } //do not allow switching if motor or servos counts are different - if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) + // if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) + if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1))) { allow_hot_switch = 0; return false; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 45d32bf7ff5..4b421d41180 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -120,7 +120,8 @@ void pgResetFn_servoParams(servoParam_t *instance) int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; -static servoMixer_t currentServoMixer[MAX_SERVO_RULES]; +static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; +static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer static bool servoOutputEnabled; static bool mixerUsesServos; @@ -192,28 +193,31 @@ int getServoCount(void) void loadCustomServoMixer(void) { - // reset settings servoRuleCount = 0; minServoIndex = 255; maxServoIndex = 0; memset(currentServoMixer, 0, sizeof(currentServoMixer)); - // load custom mixer into currentServoMixer - for (int i = 0; i < MAX_SERVO_RULES; i++) { - // check if done - if (customServoMixers(i)->rate == 0) - break; + for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { + const servoMixer_t* tmp_customServoMixers = mixerServoMixersByIndex(j)[0]; + // load custom mixer into currentServoMixer + for (int i = 0; i < MAX_SERVO_RULES; i++) { + // check if done + if (tmp_customServoMixers[i].rate == 0) + break; - if (customServoMixers(i)->targetChannel < minServoIndex) { - minServoIndex = customServoMixers(i)->targetChannel; - } + if (tmp_customServoMixers[i].targetChannel < minServoIndex) { + minServoIndex = tmp_customServoMixers[i].targetChannel; + } - if (customServoMixers(i)->targetChannel > maxServoIndex) { - maxServoIndex = customServoMixers(i)->targetChannel; - } + if (tmp_customServoMixers[i].targetChannel > maxServoIndex) { + maxServoIndex = tmp_customServoMixers[i].targetChannel; + } - memcpy(¤tServoMixer[i], customServoMixers(i), sizeof(servoMixer_t)); - servoRuleCount++; + memcpy(¤tServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t)); + currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex; + servoRuleCount++; + } } } @@ -357,19 +361,22 @@ void servoMixer(float dT) // mix servos according to rules for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + + int16_t inputRaw = input[from]; /* * Check if conditions for a rule are met, not all conditions apply all the time */ #ifdef USE_PROGRAMMING_FRAMEWORK if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { - continue; + inputRaw = 0; } #endif - - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - + if (!currentServoMixerActivative[i]) { + inputRaw = 0; + } /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -377,7 +384,7 @@ void servoMixer(float dT) * 10 = 100us/s -> full sweep (from 1000 to 2000) is performed in 10s * 100 = 1000us/s -> full sweep in 1s */ - int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i+MAX_SERVO_RULES*currentMixerProfileIndex], input[from], currentServoMixer[i].speed * 10, dT); + int16_t inputLimited = (int16_t) rateLimitFilterApply4(&servoSpeedLimitFilter[i], inputRaw, currentServoMixer[i].speed * 10, dT); servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } @@ -452,6 +459,8 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} + // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -473,6 +482,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -485,6 +495,7 @@ void processServoAutotrimMode(void) if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -518,6 +529,7 @@ void processServoAutotrimMode(void) if (trimState == AUTOTRIM_SAVE_PENDING) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { From 3cf482cb59416f3a04154394e986b4e229b4a672 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 25 Jul 2023 10:08:21 +0900 Subject: [PATCH 23/60] fix include for target config.c --- docs/Settings.md | 20 ++++++++++++++++++++ src/main/target/FF_F35_LIGHTNING/config.c | 2 +- src/main/target/MATEKF405SE/config.c | 2 +- 3 files changed, 22 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index dda075cee64..cce1b160066 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2542,6 +2542,16 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly --- +### mixer_pid_profile_linking + +If enabled, pid profile index will follow mixer profile index + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### mode_range_logic_operator Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. @@ -2602,6 +2612,16 @@ Output frequency (in Hz) for motor pins. Applies only to brushed motors. --- +### motorstop_on_low + +If enabled, motor will stop when throttle is low + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### msp_override_channels Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. diff --git a/src/main/target/FF_F35_LIGHTNING/config.c b/src/main/target/FF_F35_LIGHTNING/config.c index d0825a06fe6..cec7b4f8b4c 100644 --- a/src/main/target/FF_F35_LIGHTNING/config.c +++ b/src/main/target/FF_F35_LIGHTNING/config.c @@ -19,7 +19,7 @@ #include #include "config/config_master.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "rx/rx.h" #include "io/serial.h" #include "telemetry/telemetry.h" diff --git a/src/main/target/MATEKF405SE/config.c b/src/main/target/MATEKF405SE/config.c index f82e5fa1090..8cd618e3cf0 100644 --- a/src/main/target/MATEKF405SE/config.c +++ b/src/main/target/MATEKF405SE/config.c @@ -21,7 +21,7 @@ #include "config/config_master.h" #include "config/feature.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "io/serial.h" From ec9ee9df8289bf461705f8fdf315833b662fe531 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 25 Jul 2023 14:30:20 +0900 Subject: [PATCH 24/60] update documents --- docs/MixerProfile.md | 140 +++++++++++++++++++++++++++ docs/Screenshots/mixer_profile.png | Bin 0 -> 24263 bytes src/main/drivers/timer.h | 2 + src/main/fc/fc_msp_box.c | 2 +- src/main/flight/mixer.c | 13 ++- src/main/flight/mixer_profile.c | 36 +++---- src/main/target/MATEKF405TE/target.c | 22 ++--- 7 files changed, 184 insertions(+), 31 deletions(-) create mode 100644 docs/MixerProfile.md create mode 100644 docs/Screenshots/mixer_profile.png diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md new file mode 100644 index 00000000000..3681fcc05c8 --- /dev/null +++ b/docs/MixerProfile.md @@ -0,0 +1,140 @@ +# MixerProfile +A MixerProfile is a set of motor mixer,servomixer,platform type configuration settings. + +Currently Two profiles are supported expect f411 and f722. The default profile is profile `1`. + +Switching between profiles requires reboot to take affect by default. But When the conditions are met, It allows inflight profile switching to allow things like vtol build. + +# Setup for vtol +- Need to keep motor/servo pwm mapping consistent among profiles +- FW and MC have different way to map the pwm mapping, we need to change `timerHardware`` for to make it have same mapping. +- A vtol specific fc target is required. There might more official vtol fc target in the future +- Set mixer_profile, pid_profile separately, and set RC mode to switch them +## FC target +To keep motor/servo pwm mapping consistent and enable hot switching. Here is MATEKF405TE_SD board (folder name `MATEKF405TE`) used for vtol as a example. + +The target name for vtol build is MATEKF405TE_SD_VTOL, target folder name is MATEKF405TE. +when it is done, build your target and flash it to your fc,. + +### CMakeLists.txt +```c +target_stm32f405xg(MATEKF405TE) +target_stm32f405xg(MATEKF405TE_SD) +target_stm32f405xg(MATEKF405TE_SD_VTOL) //new target added +``` +### target.c +It is **important** to map all the serial output to `TIM_USE_VTOL_MOTOR` or `TIM_USE_VTOL_SERVO` to ensure same mapping among MC mapping and FW mapping. +```c +timerHardware_t timerHardware[] = { +#ifdef MATEKF405TE_SD_VTOL // Vtol target specific mapping start from there + DEF_TIM(TIM8, CH4, PC9, TIM_USE_VTOL_MOTOR, 0, 0), // S1 for motor + DEF_TIM(TIM8, CH3, PC8, TIM_USE_VTOL_MOTOR, 0, 0), // S2 for motor + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_VTOL_MOTOR, 0, 0), // S3 for motor + DEF_TIM(TIM1, CH1, PA8, TIM_USE_VTOL_MOTOR, 0, 1), // S4 for motor + + DEF_TIM(TIM2, CH4, PB11, TIM_USE_VTOL_SERVO, 0, 0), // S5 for servo + DEF_TIM(TIM2, CH3, PB10, TIM_USE_VTOL_SERVO, 0, 0), // S6 for servo + DEF_TIM(TIM2, CH2, PB3, TIM_USE_VTOL_SERVO, 0, 0), // S7 for servo + DEF_TIM(TIM2, CH1, PA15, TIM_USE_VTOL_SERVO, 0, 0), // S8 for servo + + DEF_TIM(TIM12, CH1, PB14, TIM_USE_VTOL_SERVO, 0, 0), // S9 for servo + DEF_TIM(TIM13, CH1, PA6, TIM_USE_VTOL_SERVO, 0, 0), // S10 for servo + DEF_TIM(TIM4, CH1, PB6, TIM_USE_VTOL_MOTOR, 0, 0), // S11 for motor +#else // Vtol target specific mapping end there + //Non votl target start from here + .........omitted +#endif + DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) + DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM + + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 + DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx +}; +``` +### target.h + +Then define `ENABLE_MIXER_PROFILE_MCFW_HOTSWAP` to enable mixer_profile hot switching once you have set the pwm mapping +```c +#ifdef MATEKF405TE_SD_VTOL +#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP //Enable hot swap +#define MATEKF405TE_SD //Define the original target name keep its original configuration such as USBD_PRODUCT_STRING +#endif +``` + +## Configuration +### Profile switch +Setup the FW mode and MC mode separately in two different mixer profile, + +I will use FW mode as mixer_profile 1, and MC as mixer_profile 2 as example. +At current state, inav-configurator do not support mixer_profile, some of the settings have to be done in cli + +`set mixer_pid_profile_linking = ON` to enable pid_profile auto handling. It will change the pid_profile index according to mixer_profile index on fc boot and mixer_profile hot switching(recommended) +``` +mixer_profile 1 #switch to mixer_profile by cli + +set platform_type = AIRPLANE +set model_preview_type = 26 +set motorstop_on_low = ON # motor stop feature have been moved to here +set mixer_pid_profile_linking = ON # enable pid_profile auto handling(recommended). +save +``` +Then finish the airplane setting on mixer_profile 1 + +``` +mixer_profile 2 + +set platform_type = TRICOPTER +set model_preview_type = 1 +set mixer_pid_profile_linking = ON # also enable pid_profile auto handling +save +``` +Then finish the multirotor setting on mixer_profile 2. + +You can use `MAX` servo input to set a fixed input for the tilting servo. speed setting for `MAX` input is available in cli. + +It is recommended to have some amount of control surface (elevon/elevator) mapped for stabilization even in MC mode to get improved authority when airspeed is high. + +**Double check all settings in cli by `diff all` command**, make sure you have correct settings. And see what will change with mixer_profile. For example servo output min/max range will not change. But `smix` and `mmix` will change. + +### Mixer Transition input +Normally 'transition input' will be useful in MC mode to gain airspeed. +Servo mixer and motor mixer can accept transition mode as input. +It will move accordingly when transition mode is activated. + +The use of Transition mode is recommended to enable further features/developments like failsafe support. Map motor to servo output, or servo with logic condition is not recommended +#### servo +38 is the input source for transition input, use this to tilt motor to gain airspeed. + +example:Add servo 1 output by +45 in speed of 150 when transition mode is activate for tilted motor setup +``` +# rule no; servo index; input source; rate; speed; activate logic function number +smix 6 1 38 45 150 -1 +``` + +#### motor +the default mmix throttle value is 0.0, It will not show in diff command when throttle value is 0.0(unused), which motor will stop. +- 0.0fM6}k72;) zA5YML*BV*&K;Z46$2-}#530tgc7ZRCZ6uT>9z3W`!n!s?1-@gr%ISGLc!1k?|Mzgf zrPT7l17kyZDG5y<7Y`S$*H zcB?x1BSjNS<_?Xo4rlBG`^AWz%0XI=ELWr*M3vE4!;F~3(V@@iObMQ0EDXBW-Rub4 zz%1Pbb}Ug<#xH}~ohMkCY__qv5tm~wFn`9fnNZ`az4GcDx|DGo^4DN;0{p)vh7}Hn zkE`umF}N|&|4RZ{7JZ5T`2l{)(*kgA^gkl^l(|U(b4yjLM0^L|7w7qPA*3HKMj2QKWpHRrk>zB@bb@vyVb)ZBW}M2 z@sW+4ojrN!DJh8(VQ6L-$Lr@loXfst6$IYyz5nUqjk1vuHOOT(@wx1)9OeNEF%fi& zfkf$d?>=&G=`PDdzn5149nK+Y_nJqa!xQ!_NPiA=cTyuZ?kZ|5TO9XuLt%C2tGd+z z1``vtza~NDB_XSfwDdxR($j^xh=7dy-xIug)Xy?dn7mPthx}cb6w0^>Wjna&Y zZWf1b<|D$lMlQQFz^H&1dis=s$58=W#DiB?SDm>7a7ElBZD583e-e?N zi3@uj)DXHw$}hrKlMwBc{LWObU%x(pH#&&BuAqJ^!Rz(i$=!)>oa$p{_;x>41bWaA zQzs`ja|;VD*R^iYgS68`oiBq{%^4b6S`o#?j0{XnOH0DWe7r+ zfwJyx&YeDUUeZndZbM};#m;UAg8q&QPXQVe9X;q~6)N$%@8{2-Ohw6EXZLCs#0N&D zdT_D_Nq>Jpv%kIcD2afOFz0KYh7MpGev4Y5=4LY4L@K*B3VfJuN^wcab7A47EI^N4 zoZz)I148@bv@^g$`j)CLuF^ZfTbC^D@|^oR^&*|_ekL3yK&Qc+wP+?|%^L-`eFz$F zrO7P+#oD4=`Nd1Gj4er4MwOi3z$FZE*P)=5DUxtQzP zhh;V}YsQpc8}M`dl<~~WJ}|>q0#Y0bp3+KGnlrDg_0snCc1aywF;PAyrV=)Jpy^a^ zlc39@)yMhOLAlK%)Z9egs#G6xW@XyzH=okHIu`IK;Jo}}b^*U&u;iOrkEq;wIZhr+ z8y_ztA&*kP{gJB-{xWYNfUH`xfnJOo$2*_B)Gfwvk#P zIvjBd@}$T0NqqcfHg3e}!P+rnjpICVH@+I^X7=XDaYG}GIPb2~ZjA`GOy6@g==N<+ zF{2~0j9?;AJHp;U^zg_s-G!P#dP^3*x3lF_RDmid2cBcHUb|Y48C2dzZ_v+Q{@{ta zyAgR(b2dWvz#76_k<{d{8e01GxplfFD z>v;Gf^7H4!BSm>g!mOWR3ULyZg9Hu^&4Gwo3=>Gl+< zI>@XKO(~PJC(HfB)kr~6QC&~ZE+aFOOzSs2!P7o0LZ4R21Sr%no=ULJDbMADy|MoY zYiczs?1AUq?M<_lGXCd^rUqR&>grI_Qt*`Q2q~a6JBxm@6`!aqoR#_t;@a8nR1c50 z^z6XRg{>EI=5$8zNINTGLWI9#>ZYp+y-V`aBV@-FB&;=AVlfrn2Cfu{TxHx}ljP<{ zP@>DCB%JAXu5)6_;Px36TGV>zkgurHglEe_3QE5aAsj0QvUV)jLF%xoZ+7h~p2D$05h4)k7( zu8(2u&68Q`Egzm@m0?I=2^0NoX=C15C7B2(ehbE!F2as0=)_l43A3>Y(Cwx&?apY{ zX1sJdV#OaXl~LIpqC#VACg5{yx`d;-@aP?JOZ4Ln+;gSU9vv}Wr|DAla{&}E>~U!n zZo293nhJ}^8#j*Bh=;GRh)GDye@0U9=zsO7=ZzT>uDg>t`wVOge|T9SoLso6UR*!l z;7ku0$ z#(`5(*OTV50XsE^8h`qbuRH;kj!E$GNJ1}3P?Q+|`AWpH($BB0f9!qTMv`R_weG$1jlQwqQJ1bxtfxx3aB8!IV(bi)+0P0`Xy!t^ zca!ds;>5wR;6jPz`kc}%wP5HSb7J3E>S9J0x6{)gQ4daQnYs096CTsy-7wCU{wtMi zJoDWU8Gqjrq2PQC7dDqbL6OZmp@5*q8+M1A5`+CC$1oK_ZZ6!&T5c~o*kFj zuqnst{;?pXJKgt8S*3vUQ{_Nd6(;+uJqd@{k&vxszCC?b@wIr$kYZ%r>dq0j(MWq& zAG8Ln?PYJrL~qn@`eT~3xa!Yl$WL4Ys5i^q!OIbOE+B8AfRNY4|UTzF-^Z~lguMhqAbDO9NiLeu__iTu`z)Sp68OK^J6N({Cdcno<4TXwQNyL0&1v4Xq&SZ|{$Z$69adau0{JM%DU zy7=_4`K*A;XaB@ZHyJ;nu1>$*cTm1rFF!RqHDq-wp|_f;dxiH?`*FpIQ*0{<8 zM^#jD8SHWcnqC8JWfBq^JOl+5M@v8A_dw|%(jyr^`BjAV3)N^47MfMtE%_z(IB5Is ze#BNfIAL3@K;dD;)ff<1z5DatkUw@v%?6aDF;KF3g%9ukso$=lNxK*$EF8i|n9s`L z+~%j&eoZ*~N4epQNIdu#j?WPJnSK&_rQJ>izYgo{dWgUE#^R1*&gFVcT5}5l1N|g1 ztb4-b>2HJlKhigoX%oK~{98pQe6`g77Qwv!7(ueI`9iT)g1+}UI%4(pKFW#xB@a*S zYLzC9ycUYQnsgceT#H5*G`;0lNGTL`F{Xgi_tzU> zT9Ik5(eb*i1mWj3>PG5e#KJ5!Q1&#RrUHx9){A*tuxz90+=@Hv4gH|#as{<0-T2}{ z((GD`>PU`1*!KlP|8tu+*aY_b$J>d0J}!-azjEwnQ*TyKuOQV(HXqmkv5JH4<$XH``Vq$ z`u^88Zch?Z5q(qRLze)r$0Lf8$Zn&%KR7P+55+*4T5;zCJ6*$bY$#a89N%g9h zqK07W8C))vu4w`Rn+gNV`C4#?>d+Y*#9m=BqBvO@3SY;kr9JN^%=j?kf&L6#dERS~ z!rK}dJ3$tWYH`VdXn{Kve*O@8eXi4NVL^O;A>WQ_NgXczf_UYqs}XzRKos5u> zBzBL;o!9MKI%YhY{YTE zlG`_rmHC$~NnadEF&Lip6K{%HRjLeDrUF}>)MH^ZjK&{wZt8=W%U~l17BO+%sN94% z_U1$)yq>x`BrlPaaf$;a>l>P1*516xLta0*^=u}GA5aL3p-Sd>do~X&>+N3nX%7Sy z^dni|3U1FuP(IeIeF_oaeZ&7wRfF1iK_kHl=cjTRCWj^d0U&KT|WBg^a+aL`OKK z3z2d6{68` zj3If2p_?$k>F~0W#GF&fYM>b3)M4h&%F;10;b2{4JtU$o?Jbw4@gX2^yPUU-_j1md zYawxDF9m0Bv8{kNYP8$i3m@&FBQZT~1XgG|cW2 z&2=0^GCfOMIK@RYQ-)Ixlr%J*#3_TVTE1sGx;$MecX?Lj4bqB2oH@*`wcM<1cR5+z zdCX3)oU9xrE_*Jujnl-%w0N)hmPnqA=Qb?0@%!)_09)|A#ISN9>W+*#ZtP@RxTpPT zq`rQw#TU*hWpU`fjntl=ncl1noVlnNN9JwB`Z+diA2~hVD-)g9SPH+HZNNilZ`=vb zJFenYwU*JK;;_A}c90oT#}lipSC$BUqkR|gS7%=;TFCv&Db?5QAao9w{*@D}Hf&P?iP<-~ zBD3B?hH+0dxL|z9zKIW{w}Zr#+)^sq0b=2FP^7mJ!Plu@Q)kUeiig8E8E}DIgNV z`x>3GtyL902e#j>4@KClArZ#WDdP+rVq;#6LFXf$wl}_OvtE!d5@DRdw*eJ~Ep?Us zMmF+ofyi^S-#@;N&kt#6XoT(U*#kNFbH?J4EPMPISC}(tyD0%FX;ec)!+H8d)!Xfv z{6cNfPaih|XMQ%PK0sWnt~XjzqM_u+|Z!HLJM$h*s18ThE=o@4i1+rqO(* zrZhP^j(~t*k>>4unw8=ayuG!&pdz>ecGmaD0wV4uEJ>g1&&IJ#zSnxV)MPQ9BRH-r zyML6_t3;J8u+O<1^!gyK+3PN@c_nuZi)VQ*5*^EGH1CBf$nz-?*{4$o^!h6&4ls_~!3N85+)>g;ej?qyin^F(DWi9P*EaP3ff`%RwN{0JE zp$C!ZT&H;s$57?mrqKN?0#=jXPIhyt=%$&@^?Nide|rJ?ke6;uH>=V6aPxhk9FIV+ zCE`ae`^_r)KhGc`ygqQ${j4+tZ)Rk}F7~062qUAWTxqQ_7!YP!lqm*Gva(s_M4i@5 z2%r$1I|+{MZt4xh9`lIiIOl$gsdqp|`)&gU*4R*a`zpZV^)P~Amn4e%!JAPtnbG`8)iMO`xpZS~v z!16vd3&TG(rzX#)f{lWL#H>d%@!eFpog5v(kvNo`X1$MXhKKrS3DPGg-?h!;M$6fd znREW$EN4EjpLmd_)Zs1?Dn+=Gx%FDWiVm%`Vqnx=?;@{L9|V{~GzXzW=6v`I%?)}+ z7<8lr`0qstI5)f@j{)Wr%9=ri7@YwYscNxtfQ_@S{w}wQ z3s!*2%k9f=Bxbe?yZ@3~N(*rvP~a976}9~6MNhxn|1?a3)?_SgxvcC}JQ-I3+siW? zwINX`A?!zFpY^5n1)!(%j`L>6`T&)qLw9K)(0RmF8yTZWL@Leq;vRnDxd&fwk}$ZH zKIm&`SePVpN_=$XwovfV~uGHS7dI7*%tQGj`p0DHzROKcoCqjgw84zLs{2KVMZ zY_t-p9S0AOdLyMD!)D{$M>{NG(uh@bixr1QN}Y9?lx+2`^YI=uCOD^mp~M4m!d2>q zL<{#uQ4GMta+b$9uP^OmF_f13$F7yVFVQ|T3V4!s4iLTG|BYHe2_l5eB?&@E%_VF@Z7E95Q{8plFX!xqQ{lK>pb)7&GUav*c-tqc@>Dm0 zNYe%!hZtRtarfWob@q4QPR|X!-KG2oECrIo$DyzfKOV>QgvtSVVYW@|(M}eE66Wcq znsJPXnsCVdn%H+g0!$0Md&T~9oy{W&La8fCzUrRXb(g%hOho3~n?3@;%u@fuH(}gp z{<=c|R4+*WsQ(4b?3AK{>~^~)&Ns;yhllm6{NF43D^%yba`*4f4XDnjJ9I>dW+dg- zOgUzN+qKc6ctv}Y?a59w$2?6n;q+c)lemenk!n}?uMyunh3M$;XD?vi z?iNAVzP_i&^8*7b`Nh`>{j=@;x}NjGd3Q}m>n#)QeI4ryZ3GCT>?o_H9E@qhuxp)+ zHIU0#{oGFL5^diiflXe2y#p%A(&_dlNOrEEc_GUTgHIJ$J@-PNP2fFQt=vM7CoDD} zoA|xWyuxTXn%PF6Fdbvn7n8pk;$Lvl5#=^l4;Mh1+k~z&OUMtug(5SwbUXZr^KP}N z@~%(yr{_;WYccsjp$m8hrz_V?Fp4%z?>mpC8ykro03^Q`xqF9v)yRf2V6g(5nYXnW zqKe2q_JXmFDtJQJE>%HIA4h^ugngX%ohvT7PE#^6qAr`f9O+2qwwdSFSNb!aEj4Kw zPWp^?_=vpT=qO~i$q%#3C+@u+yiV}BIdQQtPUNK`Oj6I782B{D*Q+vtn*Fb05x#1S z@a~REPs4~p&357W=rRm%74m1KxM!&FQa#>=)+Ej?&IGbIQx3nd>esC^k87G)$s@j4 z(Ne4dL4z#N;ev(9_!?n#VdIa3KReVQ8#8J*hj+%!f-^k3?c=W8iv7HOETl z8sGc%cQ1fz?aByT4_K>gtnqLfYpZozey>EEVLE|jIm}-L`NVVNdLF-!`A&1=a8bzl zVV<8Sm$l7&eJdU9$jM2v_hkUJuN8;0l@ zS;+T#E@2P!&X?$~Ba_|+eMREeBWwM4mO#tZhJ>9n51WWBUP=|p5ua&`*RjszXE%81)m z4shaOnskwR>jdBhBgcIla>GWkBphZdYE5>=a~-;dYa0^2H8tSLoBRkO$^ohF0AD$5 z#Xqi0icJeT72dLNb5D`)8R|ARTTH@Wr;+BvKfXn)F5)*mY(j0qXj0aV%a7LENjJYP zs&7jUzzW4HX`a37V}2Cla{Xw6z2)JlPf}a^WwVnn^Y+~~nv3`H1;)jS=v3|2xbI-w zwLSE5_q|ASxBMt-HfiXE#TVM}wf}8@2A^uP!EW~tdb|o05*xSFsA-n<_u_L6184tT z6#bSTGf(1uK*MrtxAfQAyf*+2v$SRn#C&&y)4k5_<%(2POZ)GSQc?_$rIj|S%E~as zZ{bO96QYPOAkb`>Rcn2V@FVB0f)%=Z7A}=N-tSF4}B<7YOa$h3vsXJvvAG z@uRD?OWMEI)4i>m_3nMIn=s^?B;PA7DneBiyJSGarJ7HdYP-GK$0y$EI9dWZE!Di= zeJA`AlJ2eq8_VKGKiwXa1j?$n=S^MfKnH~(36xQ%6XLW6AjcjQJdqC#4Lr!B7&aJ9 z(1n>&*6aRT;j5|q+i=M!-1TwcBdPcAd5nWDt&kp>0__(Yafj*tg)#|s1GK@{ZaF|U z>Q#^wd^+Y5M7Vl6?w$PUQ+RLhTaoj53!j}_?}LdVhpHCHEi^V3Qv?ns1Y9O>v1)!| zwo4~@NuFQXX;q^0^<0q6?_ZM|R+5CNOZfpOF`}b^$X+ZCsepiX(Qym&o2xThXJ<%_ zSs$mvEUUjz;BgQ7`87}nLSC$0yL$ZjtzKc&HY?UShS=nxNfkM7tM8b%_eMCoa z?wF2da-G+Ni{CDtKKi&o3q@ii(2CR8oAINymd#0xrb~2&!K)V??=&=or}<%rYi-q!7jb*eIWSzVn-G<8 zP-NJK90qt3zIKCHb=mLOZma(DO$8Sed?pJ!lI(=j!9_aU^7ZA_or@|#@HaaFnqwH8?Ug68qxOS9kJ|Qgc8MHUFnWjciG^5B9I87=Vq#+E8|+d=lf)m<2L4IuxO-h| zWMbmdm2olbrm_>znRJB5v@M&wI<@9DLOTtN!^^>+ORzQgF38ZWpV-M0r>P@c+`C%} z8YOmj?a}7EBL=<+K}IgnKB6}0ITyunVNBj}=@Q3YBd?%L(Ivg4u>o5|t}b79*1Be^ z)f_(kO?GfJ8dII^FPs?|G4=9?*Aq~%yw9l)X$v9>29^ebI(btOahlhb(H;56tIJA- zdDBDd+N7Q&v>&@HICTTas=6*y80iVT9py7}a^lzfqLzfY>*pAsF|)ISj(i8gaw|77 zZ7s?vUZdmg;7|)q-(rJ4VQue--?aoLwcTA$+<~P@jizU>66XonpFi@jaGo;Ajs zdXzlWfyLh3otKfCYQeOb>H15E6MtYfsJ*pYM`dUA^}>KVtYv3$KL|o@Ghg}SGMB!m zokhlC2J+TsbbTST$y>Si69b?i-{dMzVh0%eAR!Q+b zLu(E`88`<@^5>mHj|BzfI7<&~KSBRt>*&}J%vxVpci`sC4+=VT z^oQqVgrVWoRmT^Vlq}^BF?5CV=e_ixT|=b|L!udYWtaDNA|=0Au*`Qdt;ca;p}0fv zD@+31#8kD(6v zCLKRLj}eA}X@f54jDv4H+gg4@TRhoB5gnFjt3%^ZDWSGV58t~6oi)B51w&z zJYCsL9I=>8B*rjUOxblG!4@F&S2}Kizd0e*JXxa_@u|suvD12-wwW`I8M7QPW@GHP zTjaFLLhvbpGAX9IA_BSCYO{}iVLr|p@6o8wvf6-+SO$|g`DO<1Tj=gjYxKLYLJD@q zO#GAYM(J67Idq3vX$Y(IKQQa5WHjXLW(MkWDU5n3OhPtWkwGDf0R|Ttj{WH0=gig@ zn9ps88}1^?yY%DO3^i+fnaEj!VSP$_@Ti`H5fN>&csC)FvD&^%5wVNSr|cJnf&hFK zPb0dRwPE#0lTmnxs;vh^x=)WrUEvZvz}ZAvBJZ;~qdY4PdoV(Z!1EBPb;f(|h`NMe z2;hq8Z5=epz(m^;_J%cZ9JEO58xj2$*sSe28UwGyadg@;RgYpAeK~x3Kz2}lanl^w zoD01$A~!iH;B3OfYD&u@wH1E6)Wr~hS{8TxLQi1)hW+EF+-{D)$*z1Z70fe5SnS)I zzr5uSue9422ZY|E&GlLoDEMMLp%EB7pl?8}FWnyMHkR}_)sqq4KkK0MtA%?K+gYfO zW#zzc#2IABSIfeZT`qPq_}t6GnDL@D6w$nSb8&l;(KU~#0iZ2}>y$G8!c+^a-5d@u zu0VnOBp$@i4_+wuRCugHAV%W?_BHTI_tCCnA{!`6kQ|NkW#5Qb&Fy`Bd=lA2>2O}u zw6RC6YWm_X=DhtwQ5&iGPpa}d7L&LabH-6Si(>pAw%tFGCiVWtwh>{bEUZ}n?4D1O z6ooKTgxO{`JuKwh8=X)aBpL46&V5q>)vG-%6L!nD*+_hZsd~g|0ZD$jQKTnKedzeC=afCqgG=7@Be*P%;pr@Qh0FX>Wr?SI^PDv7f;00CoUUY7 z01MA7zu6)*k=&jyo0n|uwcr4I1C{k;XyCeF!V&+?#^v+(SGy%kn~t#~+8#>Xo~UgU zuS3ok9jX_G#BSNidcv4=S}+{+tu6GC+>(yW7LeylyF%H4D~?KwMxpMBU`vaYyR5i` z5t=@Wq@(WMyU+W|U#vzU8>A-PIJf!m_2XxE6L@9=XINZxkgRDOjGz6bRh}{dkF&ko zYplqFpI*3Pq8VP}Z_SVyH*)Ft1Z0&Pr9^v1x|HKeE-ALgRT_|KDrfVJ(cpoq9|NX+ zQX?$B@38W!FJq6M;}#Tn=wRZok2ahh1GSE?e&iG-S5e(!bZ^dNUY8Px&??S#gAC3Tr#KB?O{ zlzj6;{IF59{JP$66m1Re7g6HZblO#hcHVSrVb6U~EH>*#*!#jU2oFlyPePtPmx-@2 zX!DFr!@EmIr)sOI*=IEvtSx6ZSgLh!(7~mraa4MA@|EbCl7ta{(T%OG*|ZuY4Djfj ztA(-h^F~a0%;U2Y6=%DgR&A;RZtN`$#jsY;dLz*zTtp&ig^jFEJ8w8a4GpX9lzJJa zK@f!ss$lAE(2kd|)-|NMhV_;Y>Tl<7&;kR%N6EJzJdR9H6PaiTu|c|-RNI{cvjOei%LGrKo^dJT~hdJzL9lI6ZuAGZ+cGb^L|o7uYfm?Tuy&=?pUkt{c;5ucKuO3kK^!AoOE_g zdWGBu*vplCYRkVH}Ya zoK)A*0hqp0MQ33qiPhEG$5+fBSCtnUo{h_j@X9(2-Qb7yRV!5WN%E__*_kPQ;lNW& zd~gansV6Qto?<2&Tm5c&s`AX3c}t%5C=HC6Td6*VW2(1)ik?!bBAB%NY*a-1ifZS* z2e;WOOn*K442>|mm{Qb$F*CapZOIs$bAB>xDXWJySYR3P)Hkfb(LX!sky&YS6 zXt6x1rL->`A>*mEtG)qS0wrzPZBfE#C;>{sD0p6M+jw8>AT&S_WBFMXwC%MW!;rtR z(>eJvC9H>V-#d@NgZ6a<5(ic2-f-!i%i`Kcy|>|$x%V+9wk#Akp5@Z0g<4?zZ3sxR z2JpjpNt86=&|semxM2|sQh|ZmW}`oxz#yuAg|{|oS9|%d0Ht7jqwh^9V78$0KeXzV zjL)aBxNp~ks&2NY$Vu`>>nW8pc%kO%b>i-;B$c6P`&`P74K^wS`=_PQ0Vgt5xz z%R>~P26d*N*F+xZ@~Nwq3~!yCwI!6i%DkfetV`Wve>Dd=xn-fZf^2p@qgOg6GM{ld z0#AA;K3k73ciU05F&j8itZ^L@`6S-;L>0$4^o5G|+{PT~rVdVA&HHgN)|N}VoU81T znlF8lR~Ic!W)a-8&J?hKAdGFn7R{=q-d-1rGd99@!qNc3^c!;w?EvktPuwJ?@W8mm zRF9tRe}Q4T@IWC~P9Wt*R@b9j4F4EE5+{uTQI+)-8Yt%0rj~OM{F{e-qwY=MZ8oFj z1t26|Bd>R(m%^81{*{(rCDkZEi8XzX*JPs(W~~&~d4A;9lc%J z>3`?)^jcuAw0~2lF-ZRh!0}(VmU^4~pHBSu$k6xT_CJCl@xN?F?H(SwN96yKl&4?+ z8yxJjp+08oYl|jshRK$>lgV7Vf8A~HV^-EKwxmSicMDPifKj~-~?-B^9NJ$*vXm% zYFTzA$#-^s`3Ixk2J^mpHOy|QVvB4|K600mdZ|Y3CW85$odAD#RqX0_;g=x+Z!F=? zqY;j-_YgnXe_(K*4SN;+G5JcttV2a>L6{YF(pI$G-M3abDXCT<6ztslF`2k*W;2mD zlU}j85Ad!youRt^IrArp!E*+!1Gzb_z)Q5;FD|CCe-XP==LsW5RF#w(P+02Og?ve3 zSj*yo+6+2UuJx&3RsFY=BP!()Puw_()+i&)wbK!SQjg;(K%rn`=H6 z98M8nB`OwhaSWPe>MY`L1Ih<31fy3otVz{5D{~{HDL+gTT{=Iz{iS&7PDHcC$8qRX zBHN+T#RGDJh%^|N@|bkBgpxneWL5QPNVL=KL@aB1zfvf1=i^! zb{$5=(cyd^IoW=1fU*AZ21;gLO-MOGaj^y99E0%th^;9L4|n&)kMEThxqJ~@875cC z$CrEFF>M#4JWx^3r^S#a^PSFjA{TaS!#TcL3t7#`gAf6>VvKBW8i-30!-kExT zt{$?z5|7mp=NMqID(2M@VHfv)8N^ACdrQHWt~2`;Dx?Ns0wMUSs=oKPnP{$k??v-D zVE&mYz@7E-8^!gG1G4j{vX2Lnj&*s}woBN1R1GRhhw+kmAdGhIIbu{P zz(w=5JG#{K&hMyJjW`#_BkmRUO8tj~=+9bPX^7=x$Ov8Td}Wns9EIlN-4nVHq;*YK>f%oO*WhR$}3|LD+>66oZ%|k*)CD-B{5yK3io~)^@z4(HW zQKqLT`mv?To3WwNl#t>ujitRR?vT8fiZ|?T8K2V&_6d4-G2RWgxMydAh|*qR3nS#M|{1JCJ?tmuZ?;Ob#fC5N_IyHgje ze=mbb?&EHx4JWQPwN?<|ISa&lS04HA+3ERMo;=_o$}r#kD#a?qLfpbOy94mVX^9hZ zq)bnD&})0I^18%WP}Qw?FPfOp!Zs^u&o6wGXlw(cKBEy$Hx+#S8P?Irl_~7?z%+0L z;H~%B+2Hg!wZNlY-iu9P8o8tOKj&Ut1PS8-Iokt%qhzPb>NDT7S-xe>P^P2Sano3KS@03Qtx0S zFs|5KoMus4?3dymcImGpXQopph40pNj{HJ~vYAMQpFNov!A(zJAB`cr2L1lnY17(& zC05#-|3)>H_sVkvcZ{a}#0?|;;Ov1NireZaU30**;O0bSsOTRF!7>O=-|{Bmx}a{1 zx4A}5_Z4jaMDaL|qwqKktFFe>l+gIHluQw`qjGUjB{?_k(Wg)AO#6wZHjB@${kBa< z|lP4H!XE*2SBi9O={aBaO;%yZ47gO~TcxU}St)*QtuzpTUpybYm!_o<^ zf5oA-p)OZXkVhL<1-*a1a)Xe#wzLTxIC98kcUHVOh8k>^z__Oq3M3w;n0 z&|5cU11zb}jvYw=otCbt^U>1h0Yy zv4?TB%`P)m*z@@I(K!b`doSsQRep@2^dRKw&az?v_dm;9wl>gTw>wwa2hEdxb9KWz1 z6^7t=(>T@V#8o3f5)Px}#P7iI7cI>MPpf5QZM-V^2Lld@uqJi{NRx%7ygxfiB$_rS zJ}av>y4s;z80k=x5L5Gh-`@(f>QUXJk;H$_|3!!nUne&`l4|bg64r}ZQM1j<@M2@% zW^3(8Ckb55RP;w6B!h}JA-q&ulb@%sbtp>?NBQNM?&<=j^*h1>(`q9{ZoZ>R2?<4P z>qO-Nt19G01jD}>=O6X{*Nk)3x&O*Ie^}J=itV0po~jni^sVzu-ej7?=u%0gu4L&n zM~9d=?U-%cZG3Zu=Im#hUtwG8NmlMm3Q*nqd|~M>#+EMMI>O#I4I>!|UxEJyQR$Os zZ8HWKE9cyR#4z?n}S6lP*OqW08*GZNe$=o_Qq+C)@UlHY}Y&Po7UR_xQt`suZC zIOgt^b>?;C=>MbaOj1|#T}Y9EV&%I?oAcQ|5u(O}ANWvs5cx`_o%D9{!)AL+#FR$f z=RzNIC$97i9u$<1Zqm+}?jH%cxLF>4uVi~!di>=J$NOKI_*%h?Q6ZiN|+1Oo1HZ)t!mgrO7o)2~QXubj|*pMicLHxL@7 z@A+326#Aq^EE&hDao%t$_9EOC%+G7;;f6AwI+YI|4QUQ~Pw9^Lo)m@;i$p3=nHMAL<=LNugFihWxsxx342c4d7m{$;h5bQC3t zN_`0>MdtSUJR)hlK7M@wp&q0)o3=3)O9C!oFw-w?8=_RJX?iq*@L!9GaVJRR64-B` z(+yA&Vppi|lvQ{=J{B!q#ykIGK6LRz%xE_zp21S%;$oHE_h$G3oo+Iu_$GX~kxjYM^do|-sS<{TK&~~-NV!XSrvgZ|A*NS-8vLZER{sv{`sBpI6lnn480Ly;pXV+dY^L&tW(# zWB(#a&?}C19JI|T1~00^u+QR4USQU;jR~mf$0b++Cp{M*sY*T*c~n@ zN7_g1t6aKi_WkE(d~3GW)Z35scuy*i+y?dTlDf*kg$BcLFKL#&rxB4?GL9@;-nFx< zR|nP(lr#Fl*)C3nV_IY99eV+*I@(%M{D|3ST~)+ZGqXGTQ>*{*+6{6$VnG|j=SM?G zTv&y(jFSbSm)_B;&wSXMmg%b2|6{hgffm~5|2J%Ph^M#=e#$ebQMXSI13Npeot@nw zTfR}mXt}s-rQgdJ{m2RrgsV;_7N4e`UW^vHugTm?G3=GAOYRZuc1Qf({KX{a+qvN- zgXer@#HW$c5mH9e^YcEVufy03q`}leR4v zc;rDxdIk*JbNT#miOXi3%BG_53?XaAn5dlNndvM}elBg;b|-Ckq;ib<8RpOz7F{Q1 zZM45^YZA+EH&C+Rg)sIj?x*&gorL`-btnPOnF9PMK%6zxxc9FQqI8fb`&9wIBbBEmnlH&ZIDL7!tUbcZPz$IBq|=$ z{$?t0thsH|W-)ljI8Wo&H*hZR?UE`lFYiLcs+QC@6AMx63=$x2iGDR+%RxVxl;w;V z8*6Xt2p3-7R8eW}>>My^#!B;t0*4yE02CEZPtWr)RSOG?L+wa<`VdaAsi@gooK9RS zfttR+B51YDm)4?kM4EdP9~B>4#4;kS)l&n9LcgFL(d=L#3*3?c>f=D@VnpOby>l)K zD=>i1H+)JijVmiFTTJiMQNe34#K#Z2&(#!whf5TDRt-iyQ@ESV^@jqFM_|hNhx3k> za?g4H0O@5Kc~wJ{Qe|3d=G-af_vg2TO8PPYKDEFm>D&;;Ton2rAS4b#gkH1n^EO!A zmE}2J31or+0+4ue3)R$H z=@?-QJ5hP)*3OOv@GpD>kA@htZC@l#CZ|~__tq~nwUJ&XQutotU#$3JsZZ6#3@=_R zFpXI55hG&UO>&z*A46C(9Ewj!t_1Ljir*uY0Qht~xx@geWgew? z>ZRPrIS<_sEc~wX?GAXz#an#TY;GRT z0&MnNxTu*`px5!5#Ql-^!^f9Wd92z014W)dKVTmPK;H;(eS(FP1`59!i5LCD!;ciIV*G#AoOwLd`@hG}X;Ds#Wh|94iKr}-oRYm0LPd3yCA%q- zj4d-9+lg%1l9X*|S6PRQ!Jx4X(Wr#MFc@Z%!3;6RnDP5e%Q@%%9`|wYANSrr?)U%i zcb4z>`+2-S@Avch(u}h|ed<17_jrbR;o?!(H}%DFEG*5;6_rVVTuzzK&_3^>sq$_oU4K5Jey+KP&mm-TveTVFhtW-!-kjOw zkvqS1HM0RUX~#=W39r)D4(Ryryn6JkC=5{QD(y349)f4JVQ2n$n^0cMGI(~|-i+EH zoyGzkQ0r1wkCdYP-15KAy>xtQeYSh+P-%Ee?z-Bqki29FB`9UT2ZcXi8#cAmT8wKY z3qIsw!JAv=`3@Bx$xg>jfBWr*)A0oCIlqTXzA}JrzseI|XgP97g)ozT0sAXkot}4w zE;u7NdG1`=KXLa#$d`XW@=yJSPP|OCDmpuu3ZU?m zl&tjM1CLMnlJO#{fJUEjABH=x-EwN&_0de_+AXAQOt-zWd(J$ld2MjbIa<1js*lo~ z)hxXc3rvkQ=e|xStoEU%Bm?I@BlsWw6E5CltMIS<00n;~n0u())d`tMDAWl#tWt>< zUM*4>D3kW0WRh%>(BLJhv!+MlI~6aSLZsyhBWRYN0$s7z-1Q+g`8b=Z_8oU0H+(q# zGOy^&nS0$Ah=IJDn}jZbT+mD~Lb1m*P2nT{(~a5Tqo}j}-{wdbtCc?6=jQ06 znAFrdBTE456^A5ZK9`~NSAD5Xf%zeEp-mO2y)(5(#5%A#kmuDR+)@pK4>jAfv@58} zD7JDhNk7pOz`$6>TcS^DiK%~uR+Hpc4q8a3ngDg>iAzp^sXQ{QOnR3!s>QChM#7rp zcS0RfKQD#wOXircL&az%o;3f(7Sg&V>!>?-V{jZ;Vc8!G2lV;HNAsyF>!p;n32VZIqHM1re zZs;~ty~$7G%CVOxsIB;&-bafH};US}l-KZEi%4^bq1=ey25*|KGpoztz&^F6cnF{ zT;6~d75v)g%JGK5i5cCR!#mVr`;7A_jm2P5l) z^>s42+MbAQ;rx9`x83WzH98w#Cf8yn>I`s~miKusTZltaldL`ANcz^Wb6YQMhQ_QM z=O}t`;2e_m?F@;quW)@a6rGo(YWAwCjd8F*I0C*|{@f-}ivrgYNa$8|ckSAiT1ed5 z{G`*Qxy1y@iEE|XT!R%7-@ppbp6y{*7pio7gRH8vFdv6l{DQ1I7%($cUKuV%Q{Ai= z7lN<1>tA}Xvrxup9i@KVd@SpRR7mjZN{`*I)wHr{)&aGMb+`^eEr}H<>rSm2Sgn6{ z5ISK&)4Uqbc^Uq;MA~kyLnLe#rtKOQXn2o5VxJW8?qaH&yi|PzY^G#MUa*K}_v&uQ zl8q9oLk%RfUSC88+)K{F*9&^5{sDjAQJ;M9>I&(2rMjo#&jSZe@NGRjo@sroF1rfC zhlYk2TUzcuw2;VurHELI*o1%}a>`_kBq~ViEla07Q3_m{bxt8cdq&(yFXth&%=@DB7m5F>hU^WBrdemBxVB@SE1CLS}|Ncbl7p{V8t1cJN}x(PnN`6P^O1!WqW#hKq$;^un%CI zo(|5nmL@!;N6;oABw=75vIv$0FmPe=xg6 zc2J!o^k7qs1WUA|v-o+Y33Qd7LtRm>pGVK`-oIZI>?MMk7$OVc^yK_geU5?2{Ay`w z>EN*h6|iJpJ)Lj3OPd;|Wo58%p5e4>XsMJ>de{E6;7zQua+MKx91@zzTa1x)jt8=1 zXfCmNd12~3g_7jf6zc|_$;Io-Bhg|^uBN8uBAdV+rGI+=*sc~?93GZ4OZ3uL4q>7^ zLoq!wGeg6=^SOZ}0+_Nf-9#f-su=5)DvWR($PS|jeUk2!T4?4Ej+z497TuX=Ah%Xz zv){8nviR;}%?UmWCzX0l>xtX5;O%};ZvYtL1oiAtVC*8F`%(r8C>POh@Xzp)@2mqr zsyDQxNyZat1m0`|wKw!F>MP!PZS`|1g&G$|Gad^KOCEDl=&KN2`}XrWO{WhCHU0 z-k(_C=r651zs>;^< zh-I(ZfB~Gpq72MANwpL&Sv7=|1AZxU1_u&~%Bic#zF|#P$L926YUi|N6k)SRiqry> zapOihUD;KexuoFyLe=IsJci<|MW5u>(#A2ozIXMm4!mbB=WbC8yy{|Py0%y!|Jr(F zUx>-n&bV!rAT^u%l zw0ULYT<8vc1UFhYV7v|C15-BR%u-WTOK^{hOGdHxS&Xfj@w|87cOv?xDRDR>KkC81 znCiFd=2oUTaX;%v2PuA&8G<`mwK-I683g1P?5;JtqWNeYc2J=2hKf?Z9qYgQ41&*l zID&U*+&;%tEzu+f(KvQ9h654Q=6HSl0Vx^Jp*D*oiZv~08+|Y_8B0;01YCk<*pgqU zr^n*JIwi0fSu~gpE~~h(6XJ92f-(K|DE^^w+(16zUI1YQ+=8lEe@^}?Hy`)uQR2Ww zs)R%1WdKHG&0^rhfho6(KOL$@5!TC+=K}%)n3r}!QXDrZkC6vbZDp6B^H@?#*;XecPfLI8uCx9^xr=U*=bC7 zYyaGXoa=LNCVq&4u%@#hMOlB5@V#Fy1TRlLwVSI2h0E$eG9D5~Ja+Qrt?;CG`wMcT zva@&ah__2I7|f#PYks;BHckwRGHhLv-nQ^i$GT87b|C*bhMe0Wwy z#^uz&TFipC=U^fSzw_5Cx5F>185j%+PAhZw;@Kuyyq{p?Z4_;C`Jkiv;?NI?!6nIl z+ik5BSN>Syd`NktU1R*Knl`Hc33{g6W`-&mxv`3>EvVHpRQL}l{9NN>uVjh8!D^h* zZ5ZaQFlId_tR)WQ0<+jeBk9j!n#uInl2kCaIlGs;@-4pk= z`zbGGjs((fjKiLf6ZAk-fZi!m(5Naf1*9cwx!GO#FCR_~*4rxWlhc-mxY%#vR0ehR zim_U9?YA+mR@&5iZJ()4P#>5(xY|oB>wQshi)xo-+LhC=xO+9r)NamMWq-vhHg2Zg zW#kU&BHK3NPg$%}R5i2Gv0NJZ#5pd;vw&Rj`!hbwk>7Li+UO(jrnTE~1EC(jry6bc zaG_%wU6Z=GS5tP87;FMG&W`|n%bjPyzkGU6l7c}V9O}2Aleb{&>|`}RxrQ6-$5z)J zwQQ6tMU)HtH`vwVA~R_TL7_iX1q-QOQ$fx7gZrq}fd%1iUFsh`KRQO-p1LK%S9#Gn zjBTnHa@pz=;Dud@u8}RX#ObE#@z&WlOLt^rs>AJY#hv(Lg9>E9-ci-FPUEolM*kU9 z+D@~R3<~e0kX$WIq_7p%>xx9T#N7ZOAWVp5x1x(bY>Q_-lu$eU0I)aFQrPpf6z>S7 z%p$D>7}goSrwg6t;n%(0neEZ%fUKA(PjIsDcU(v_lgH%DA0qTggZnj0)(g{$jyEhe*Yn;XpQ7Y{V=$dQ?V)^pLxz8P>qOuHKaa|keNTGk@&OVXX!k}8i00H%VtD$0h5|7=BJWZ@nk`}rGWO$0wM*! z!4oU>c&ANOVpR|aud)kfKk)2JKgpT1@QDn^(~_A=n9qAeZ!T<}l3PShd3|>c{${hu zDl?f6;IY|nJ+_G8PLnX3)Z?;Vxw-^f(Rh{(!GewGh~>$}qd8;cJv$+OPK1)!(+}zZ z0go(CjqF$$xj{5>v*is}Ji1tUHG}_7RCywTH%J{2d$k+8W{l&ms%L|ATrDp@6ngZ~ z6W2GF?~hEQ(7d(_h{o)D{h7UjPH~4fnd~-X4ap;4TP_`W|3R*zW`8CplI~6`C$h*i zy)sq<{SU~M?G>1%7FoaORnM^Nr1aDb7&@xdWu$Iw^vv<=nlHW45>Ntt7wnpSD|DS< zZq(1RktQLBV{LsprR81dS8bKt(W8Seo8B$nnQ@J95@O)J58 z86HKD*_7Ty;Vc=q!{cndJ0n_D9VjRrbSeGh{puYuq6EBGEF$~q`~g}2KORiC__V9O zd*)SBU4A9IWN=3t4TI^CkO>!Ctd&f}jCc z^w(ts$L5zHK*jYHC7#+N;8k>}v5y#2Y{_@5{hutkH>QZ0kND(KI20yOXi|dCJpf*Z zPhDy(4Bu%MuYl`otQXkFuz7 zKvcKn5jkp(IPnIMOw7EQpJRWI=O%S}N!_KM5;0-CKeJ4$m>w)1z+KV9=JNQnkyIp9 zo#TmdP)|<{d%1V@-jLoB*;9F2LdK8*hXOaU$(d>kVq8d!m(Y^$h7H~n!_Kr0^z>&2 zVb8Q+r}wXn@Xz6TCJ)pur>!S#hIM&%jlPJ@@$0@ZF|1KhXOmRrvIeod9PCNCb9hJh zFN+}#-2PwAElo-#(PxFGlX&UfxhJWdlkPF3-sg0YvLY5Yy3M8_mxvJekXH>4Tx<9L zk9*g7mXT9XA`xwtd|aeJ`#!uo&fz_h|1?C~c`4GxbyQt@L)d4x2+%K^b6Hd0j6qz!X0 z^W#L6ys+Gyq0fw_5PpJolU_$VC34qZ$;NCLbk@(Jrx#bFL-w>6ELE5OdUD*A)x3;W zX{W@U9*r>m=Fi6G09j@p9Ri$ny>+@GO%hdWX(uCW5gSq}&H@MQ8c<}KXt6{w4piP- z-*)m%y`%*(D_HYkbMI&o%907MxgfM;s%oBBEFHWJ9@#SR$eNumY~Q%hm=uZ2p9+~< zVc(I6PfQ4CZCsnf1GRp|V))Fh#6(T64@x{b0q?S@vUJJ)!p~E5BOt-&)(fV=ILF;8tQ{sI8wJ zl#g0J7xA81Rk*dns;di$g0RW`=EKH>^C)*Kg4 zvDUi}(bPx{Ngti_ab{Kbbm1ztE=qra+RKkY7);~KLMG^gFz;RdAOURpwFHiu_ zt3IC_A*62YFYdVedS01{Q^#52{?gZ1SIW|)cCiy z;&(RxTkOV<{q_GP1^+hDoF{y&A({|*x0BN_F-`s;381ucOfVc+=Mp=9#> zuii;KXoOrvAZ!vr1k1N0O&rqm@#8-e)6IVtM9v?8R`2+%y8y!Hf3h&OGATLk8uc%c CTD3m_ literal 0 HcmV?d00001 diff --git a/src/main/drivers/timer.h b/src/main/drivers/timer.h index 5272c804b8f..ec6b7a672f5 100644 --- a/src/main/drivers/timer.h +++ b/src/main/drivers/timer.h @@ -111,6 +111,8 @@ typedef enum { TIM_USE_MC_CHNFW = (1 << 4), // Deprecated and not used after removal of CHANNEL_FORWARDING feature TIM_USE_FW_MOTOR = (1 << 5), TIM_USE_FW_SERVO = (1 << 6), + TIM_USE_VTOL_SERVO = (TIM_USE_FW_SERVO | TIM_USE_MC_SERVO), + TIM_USE_VTOL_MOTOR = (TIM_USE_FW_MOTOR | TIM_USE_MC_MOTOR), TIM_USE_LED = (1 << 24), TIM_USE_BEEPER = (1 << 25), } timerUsageFlag_e; diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 98c0e878909..0cb6c22f830 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -99,7 +99,7 @@ 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 = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 61 }, { .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 62 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index b77e39c0b88..ea468914406 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -105,14 +105,25 @@ int getThrottleIdleValue(void) static void computeMotorCount(void) { + static bool firstRun = true; + if (!firstRun) { + return; + } motorCount = 0; for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { + bool isMotorUsed = false; + for(int j = 0; j< MAX_MIXER_PROFILE_COUNT; j++){ + if (mixerMotorMixersByIndex(j)[i]->throttle != 0.0f) { + isMotorUsed = true; + } + } // check if done - if (primaryMotorMixer(i)->throttle == 0.0f) { + if (!isMotorUsed) { break; } motorCount++; } + firstRun = false; } bool ifMotorstopFeatureEnabled(void){ diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index d0585a023f9..926cab96a97 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -84,19 +84,19 @@ void mixerConfigInit(void){ } } -static int computeMotorCountByMixerProfileIndex(int index) -{ - int motorCount = 0; - const motorMixer_t* temp_motormixers=mixerMotorMixersByIndex(index)[0]; - for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { - // check if done - if (temp_motormixers[i].throttle == 0.0f) { - break; - } - motorCount++; - } - return motorCount; -} +// static int computeMotorCountByMixerProfileIndex(int index) +// { +// int motorCount = 0; +// const motorMixer_t* temp_motormixers=mixerMotorMixersByIndex(index)[0]; +// for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { +// // check if done +// if (temp_motormixers[i].throttle == 0.0f) { +// break; +// } +// motorCount++; +// } +// return motorCount; +// } // static int computeServoCountByMixerProfileIndex(int index) // { @@ -159,13 +159,13 @@ bool checkMixerProfileHotSwitchAvalibility(void) allow_hot_switch = 0; return false; } + //not necessary when map motor/servos of all mixer profiles on the first boot //do not allow switching if motor or servos counts are different // if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1)) || (computeServoCountByMixerProfileIndex(0) != computeServoCountByMixerProfileIndex(1))) - if ((computeMotorCountByMixerProfileIndex(0) != computeMotorCountByMixerProfileIndex(1))) - { - allow_hot_switch = 0; - return false; - } + // { + // allow_hot_switch = 0; + // return false; + // } allow_hot_switch = 1; return true; } diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index bee9ebee56d..03c7d98807c 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -27,19 +27,19 @@ timerHardware_t timerHardware[] = { #ifdef MATEKF405TE_SD_VTOL //With INAV firmware, DSHOT can not work on S3, S5,S7 because of DMA clash, pls use ONESHOT or MULTISHOT and calibrate ESC PWM range.<-copied from matek website - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S3 D(2,6,0) UP256 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S4 D(2,1,6) UP256 + DEF_TIM(TIM8, CH4, PC9, TIM_USE_VTOL_MOTOR, 0, 0), // S1 D(2,7,7) UP217 + DEF_TIM(TIM8, CH3, PC8, TIM_USE_VTOL_MOTOR, 0, 0), // S2 D(2,2,0) UP217 + DEF_TIM(TIM1, CH3N, PB15, TIM_USE_VTOL_MOTOR, 0, 0), // S3 D(2,6,0) UP256 + DEF_TIM(TIM1, CH1, PA8, TIM_USE_VTOL_MOTOR, 0, 1), // S4 D(2,1,6) UP256 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S5 D(1,7,3) UP173 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 + DEF_TIM(TIM2, CH4, PB11, TIM_USE_VTOL_SERVO, 0, 0), // S5 D(1,7,3) UP173 + DEF_TIM(TIM2, CH3, PB10, TIM_USE_VTOL_SERVO, 0, 0), // S6 D(1,1,3) UP173 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_VTOL_SERVO, 0, 0), // S7 D(1,6,3) UP173 + DEF_TIM(TIM2, CH1, PA15, TIM_USE_VTOL_SERVO, 0, 0), // S8 D(1,5,3) UP173 - DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE - DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S11 D(1,0,2) + DEF_TIM(TIM12, CH1, PB14, TIM_USE_VTOL_SERVO, 0, 0), // S9 DMA NONE + DEF_TIM(TIM13, CH1, PA6, TIM_USE_VTOL_SERVO, 0, 0), // S10 DMA NONE + DEF_TIM(TIM4, CH1, PB6, TIM_USE_VTOL_MOTOR, 0, 0), // S11 D(1,0,2) #else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 From a10d6cab5895f1e40841e879a4a2d34ad78a2b8f Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Tue, 25 Jul 2023 13:07:38 +0100 Subject: [PATCH 25/60] [mixer-profile] [doc] add some clarifications --- docs/MixerProfile.md | 162 ++++++++++++++++++++++++++++--------------- 1 file changed, 108 insertions(+), 54 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 3681fcc05c8..08fcae804cc 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -1,32 +1,51 @@ # MixerProfile -A MixerProfile is a set of motor mixer,servomixer,platform type configuration settings. -Currently Two profiles are supported expect f411 and f722. The default profile is profile `1`. +A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings to enable VTOL transitions. -Switching between profiles requires reboot to take affect by default. But When the conditions are met, It allows inflight profile switching to allow things like vtol build. +Currently two profiles are supported on targets other than F411 and F722 (due to resource constraints on these FC). i.e VTOL transition is not available on F411 and F722 targets. + +By default, switching between profiles requires reboot to take affect. However, when all conditions are met, and a suitable [configuration](#configuration) has been applied, `mixer_profile` also allows in-flight profile [switching](#rc-mode-settings) to allow things like VTOL operation. This is the recommended operating mode. + +Please note that this is an emerging / experimental capability that will require some effort by the pilot to implement. + +## Setup for VTOL + +- For mixer profile switching it is necessary to keep motor and servo PWM mapping consistent between Fixed-Wing (FW) and Multi-rotor (MR) profiles +- Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to change the source code `timerHardware` table to allow a consistent enumeration and thus mapping between MR and FW modes. +- For this reason, a **VTOL specific FC target is required**. This means that the pilot must build a custom target. In future there may be official VTOL FC targets. +- In operation, it is necessary to set the `mixer_profile` and the `pid_profile` separately and to set a [RC mode](#rc-mode-settings) to switch between them. -# Setup for vtol -- Need to keep motor/servo pwm mapping consistent among profiles -- FW and MC have different way to map the pwm mapping, we need to change `timerHardware`` for to make it have same mapping. -- A vtol specific fc target is required. There might more official vtol fc target in the future -- Set mixer_profile, pid_profile separately, and set RC mode to switch them ## FC target -To keep motor/servo pwm mapping consistent and enable hot switching. Here is MATEKF405TE_SD board (folder name `MATEKF405TE`) used for vtol as a example. -The target name for vtol build is MATEKF405TE_SD_VTOL, target folder name is MATEKF405TE. -when it is done, build your target and flash it to your fc,. +In order to keep motor and servo PWM mapping consistent and enable hot switching, the steps below are required. + +The following sections use a MATEKF405TE\_SD board (folder name `MATEKF405TE`) configured for VTOL as a example. + +The target name for VTOL build is `MATEKF405TE_SD_VTOL`, the standard target folder name is `MATEKF405TE`. + +### CMakeLists.text modifications + +#### Adding the VTOL target + +Add the VTOL target definition to `CMakeLists.txt`, i.e. the third line below. -### CMakeLists.txt ```c target_stm32f405xg(MATEKF405TE) target_stm32f405xg(MATEKF405TE_SD) target_stm32f405xg(MATEKF405TE_SD_VTOL) //new target added ``` -### target.c -It is **important** to map all the serial output to `TIM_USE_VTOL_MOTOR` or `TIM_USE_VTOL_SERVO` to ensure same mapping among MC mapping and FW mapping. +### target.c modifications + +Two new enumerations are available to define the motors and servos used for VTOL. + +It is **important** to map all the PWM outputs to `TIM_USE_VTOL_MOTOR` or `TIM_USE_VTOL_SERVO` to ensure consistency between the MR mapping and FW mapping. + +For example, add the new section, encapsulated below by the `#ifdef MATEKF405TE_SD_VTOL` ... `else` section : + ```c timerHardware_t timerHardware[] = { -#ifdef MATEKF405TE_SD_VTOL // Vtol target specific mapping start from there +#ifdef MATEKF405TE_SD_VTOL + // VTOL target specific mapping start from there DEF_TIM(TIM8, CH4, PC9, TIM_USE_VTOL_MOTOR, 0, 0), // S1 for motor DEF_TIM(TIM8, CH3, PC8, TIM_USE_VTOL_MOTOR, 0, 0), // S2 for motor DEF_TIM(TIM1, CH3N, PB15, TIM_USE_VTOL_MOTOR, 0, 0), // S3 for motor @@ -36,24 +55,29 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_VTOL_SERVO, 0, 0), // S6 for servo DEF_TIM(TIM2, CH2, PB3, TIM_USE_VTOL_SERVO, 0, 0), // S7 for servo DEF_TIM(TIM2, CH1, PA15, TIM_USE_VTOL_SERVO, 0, 0), // S8 for servo - + DEF_TIM(TIM12, CH1, PB14, TIM_USE_VTOL_SERVO, 0, 0), // S9 for servo DEF_TIM(TIM13, CH1, PA6, TIM_USE_VTOL_SERVO, 0, 0), // S10 for servo DEF_TIM(TIM4, CH1, PB6, TIM_USE_VTOL_MOTOR, 0, 0), // S11 for motor -#else // Vtol target specific mapping end there - //Non votl target start from here - .........omitted + // VTOL target specific mapping ends here +#else + // Non VOTL target start from here + // .........omitted for brevity #endif DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM - + DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx }; ``` -### target.h -Then define `ENABLE_MIXER_PROFILE_MCFW_HOTSWAP` to enable mixer_profile hot switching once you have set the pwm mapping +Note that using the VTOL enumerations does not affect the normal INAV requirement on the use of discrete timers for motors and servos. + +### target.h modification + +In `target.h`, define `ENABLE_MIXER_PROFILE_MCFW_HOTSWAP` to enable `mixer_profile` hot switching once you have set the `timer.c` PWM mapping: + ```c #ifdef MATEKF405TE_SD_VTOL #define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP //Enable hot swap @@ -61,80 +85,110 @@ Then define `ENABLE_MIXER_PROFILE_MCFW_HOTSWAP` to enable mixer_profile hot swit #endif ``` +Once the target is built, it can be flashed to the FC. + ## Configuration -### Profile switch -Setup the FW mode and MC mode separately in two different mixer profile, -I will use FW mode as mixer_profile 1, and MC as mixer_profile 2 as example. -At current state, inav-configurator do not support mixer_profile, some of the settings have to be done in cli +### Profile Switch + +Setup the FW mode and MR mode separately in two different mixer profiles: + +In this example, FW mode is `mixer_profile` 1 and MR mode is `mixer_profile` 2. + +Currently, the INAV Configurator does not support `mixer_profile`, so some of the settings have to be done in CLI. + +Add `set mixer_pid_profile_linking = ON` in order to enable `pid_profile` auto handling. It will change the `pid profile` index according to the `mixer_profile` index on FC boot and allow `mixer_profile` hot switching (this is recommended usage). + +The following 2 `mixer_profile` sections are added in the CLI: -`set mixer_pid_profile_linking = ON` to enable pid_profile auto handling. It will change the pid_profile index according to mixer_profile index on fc boot and mixer_profile hot switching(recommended) ``` -mixer_profile 1 #switch to mixer_profile by cli +#switch to mixer_profile by cli +mixer_profile 1 set platform_type = AIRPLANE set model_preview_type = 26 -set motorstop_on_low = ON # motor stop feature have been moved to here -set mixer_pid_profile_linking = ON # enable pid_profile auto handling(recommended). +# motor stop feature have been moved to here +set motorstop_on_low = ON +# enable pid_profile auto handling (recommended). +set mixer_pid_profile_linking = ON save ``` -Then finish the airplane setting on mixer_profile 1 +Then finish the aeroplane setting on mixer_profile 1 ``` mixer_profile 2 set platform_type = TRICOPTER set model_preview_type = 1 -set mixer_pid_profile_linking = ON # also enable pid_profile auto handling +# also enable pid_profile auto handling +set mixer_pid_profile_linking = ON save ``` -Then finish the multirotor setting on mixer_profile 2. +Then finish the multi-rotor setting on `mixer_profile` 2. -You can use `MAX` servo input to set a fixed input for the tilting servo. speed setting for `MAX` input is available in cli. +Note that default profile is profile `1`. -It is recommended to have some amount of control surface (elevon/elevator) mapped for stabilization even in MC mode to get improved authority when airspeed is high. +You can use `MAX` servo input to set a fixed input for the tilting servo. Speed setting for `MAX` input is available in the CLI. -**Double check all settings in cli by `diff all` command**, make sure you have correct settings. And see what will change with mixer_profile. For example servo output min/max range will not change. But `smix` and `mmix` will change. +It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilisation even in MR mode to get improved authority when airspeed is high. + +**Double check all settings in CLI with the `diff all` command**; make sure you have set the correct settings. Also check what will change with `mixer_profile`. For example servo output min / max range will not change. But `smix` and `mmix` will change. ### Mixer Transition input -Normally 'transition input' will be useful in MC mode to gain airspeed. -Servo mixer and motor mixer can accept transition mode as input. -It will move accordingly when transition mode is activated. -The use of Transition mode is recommended to enable further features/developments like failsafe support. Map motor to servo output, or servo with logic condition is not recommended -#### servo -38 is the input source for transition input, use this to tilt motor to gain airspeed. +Typically, 'transition input' will be useful in MR mode to gain airspeed. +Both the servo mixer and motor mixer can accept transition mode as an input. +The associated motor or servo will then move accordingly when transition mode is activated. + +The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended + +#### Servo + +38 is the input source for transition input; use this to tilt motor to gain airspeed. + +Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup: -example:Add servo 1 output by +45 in speed of 150 when transition mode is activate for tilted motor setup ``` # rule no; servo index; input source; rate; speed; activate logic function number smix 6 1 38 45 150 -1 ``` -#### motor -the default mmix throttle value is 0.0, It will not show in diff command when throttle value is 0.0(unused), which motor will stop. +#### Motor + +The default `mmix` throttle value is 0.0, It will not show in `diff` command when throttle value is 0.0 (unused); this causes the motor to stop. + - 0.0 Date: Wed, 26 Jul 2023 00:03:49 +0900 Subject: [PATCH 26/60] move ENABLE_STATE(FW_HEADING_USE_YAW) from navigation.c to mixer.c --- src/main/flight/mixer.c | 9 +++++++++ src/main/navigation/navigation.c | 21 --------------------- src/main/navigation/navigation.h | 1 - 3 files changed, 9 insertions(+), 22 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index ea468914406..1f144dc60cc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -182,6 +182,15 @@ void mixerUpdateStateFlags(void) } else { DISABLE_STATE(FLAPERON_AVAILABLE); } + if ( + currentMixerConfig.platformType == PLATFORM_BOAT || + currentMixerConfig.platformType == PLATFORM_ROVER || + navConfig()->fw.useFwNavYawControl + ) { + ENABLE_STATE(FW_HEADING_USE_YAW); + } else { + DISABLE_STATE(FW_HEADING_USE_YAW); + } } void nullMotorRateLimiting(const float dT) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 436c3c3796e..780122e724c 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4177,18 +4177,6 @@ void navigationUsePIDs(void) ); } -void navigationInitYawControl(void){ - if ( - currentMixerConfig.platformType == PLATFORM_BOAT || - currentMixerConfig.platformType == PLATFORM_ROVER || - navConfig()->fw.useFwNavYawControl - ) { - ENABLE_STATE(FW_HEADING_USE_YAW); - } else { - DISABLE_STATE(FW_HEADING_USE_YAW); - } -} - void navigationInit(void) { /* Initial state */ @@ -4223,15 +4211,6 @@ void navigationInit(void) /* Use system config */ navigationUsePIDs(); - if ( - currentMixerConfig.platformType == PLATFORM_BOAT || - currentMixerConfig.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 diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b6ff19f7116..cf4289e14a1 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -479,7 +479,6 @@ typedef struct { } navSystemStatus_t; void navigationUsePIDs(void); -void navigationInitYawControl(void); void navigationInit(void); /* Position estimator update functions */ From 167db6ec5913ec115aeda5b5e6a22f43e12b99ae Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 27 Jul 2023 12:09:26 +0900 Subject: [PATCH 27/60] add sannity check in Platform specific RC modes, Add sanity check in mixer_profile switching --- docs/MixerProfile.md | 2 ++ src/main/fc/fc_core.c | 4 ++-- src/main/flight/mixer_profile.c | 7 +++++-- src/main/flight/pid.c | 2 +- src/main/flight/pid_autotune.c | 2 +- src/main/navigation/navigation.c | 6 +++--- 6 files changed, 14 insertions(+), 9 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 08fcae804cc..e586cfc4017 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -139,6 +139,7 @@ It is recommended to have some amount of control surface (elevon / elevator) map Typically, 'transition input' will be useful in MR mode to gain airspeed. Both the servo mixer and motor mixer can accept transition mode as an input. The associated motor or servo will then move accordingly when transition mode is activated. +Transition input is disabled when navigation mode is activate The use of Transition Mode is recommended to enable further features and future developments like fail-safe support. Mapping motor to servo output, or servo with logic conditions is **not** recommended @@ -171,6 +172,7 @@ mmix 4 -1.200 0.000 0.000 0.000 ### RC mode settings It is recommend that the pilot uses a RC mode switch to activate modes or switch profiles. +Profile files Switching is not available until the runtime sensor calibration is done. Switching is NOT available when navigation mode is activate or position controller is activate, including altitude hold. `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. Once successfully set, you can see the profiles / model preview etc. will switch accordingly when you view the relevant INAV Configurator tabs. Checking these tabs in the INAV Configurator will help make the setup easier. diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b5bb2fc3227..df194954c4d 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -697,14 +697,14 @@ void processRx(timeUs_t currentTimeUs) #if defined(USE_MAG) if (sensors(SENSOR_ACC) || sensors(SENSOR_MAG)) { - if (IS_RC_MODE_ACTIVE(BOXHEADFREE)) { + if (IS_RC_MODE_ACTIVE(BOXHEADFREE) && STATE(MULTIROTOR)) { if (!FLIGHT_MODE(HEADFREE_MODE)) { ENABLE_FLIGHT_MODE(HEADFREE_MODE); } } else { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } - if (IS_RC_MODE_ACTIVE(BOXHEADADJ)) { + if (IS_RC_MODE_ACTIVE(BOXHEADADJ) && STATE(MULTIROTOR)) { headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); // acquire new heading } } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 926cab96a97..12fe15a1eee 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -14,6 +14,8 @@ #include "common/axis.h" #include "flight/pid.h" #include "flight/servos.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "fc/fc_core.h" #include "fc/config.h" @@ -172,7 +174,8 @@ bool checkMixerProfileHotSwitchAvalibility(void) void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - isInMixerTransition = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); + 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))); + isInMixerTransition = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!navBoxModesEnabled) && ((posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); outputProfileHotSwitch((int) IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); } @@ -201,7 +204,7 @@ bool outputProfileHotSwitch(int profile_index) } // 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){ + if (navBoxModesEnabled || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || posControl.navState != NAV_STATE_IDLE){ // LOG_INFO(PWM, "mixer switch failed, navModesEnabled"); return false; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index eb9abb4b55e..13a7c45d788 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1067,7 +1067,7 @@ void FAST_CODE pidController(float dT) return; } - bool canUseFpvCameraMix = true; + bool canUseFpvCameraMix = STATE(MULTIROTOR); uint8_t headingHoldState = getHeadingHoldState(); // In case Yaw override is active, we engage the Heading Hold state diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index d9688f1e829..6b59f931172 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -130,7 +130,7 @@ void autotuneStart(void) void autotuneUpdateState(void) { - if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && ARMING_FLAG(ARMED)) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE) && STATE(AIRPLANE) && ARMING_FLAG(ARMED)) { if (!FLIGHT_MODE(AUTO_TUNE)) { autotuneStart(); ENABLE_FLIGHT_MODE(AUTO_TUNE); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 780122e724c..091a1245186 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3789,14 +3789,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // CRUISE has priority over COURSE_HOLD and AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) { if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } // PH has priority over COURSE_HOLD // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) { if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } @@ -4373,7 +4373,7 @@ bool navigationRTHAllowsLanding(void) bool isNavLaunchEnabled(void) { - return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH); + return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE); } bool abortLaunchAllowed(void) From 5242ad2b35219a387b566e3c8404a5ede214d7b2 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 29 Jul 2023 02:05:04 +0900 Subject: [PATCH 28/60] mixer profile sitl support --- .gitignore | 3 ++- src/main/target/SITL/target.h | 3 +++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index ea13b9f1789..f23f0dfd9f4 100644 --- a/.gitignore +++ b/.gitignore @@ -15,11 +15,12 @@ startup_stm32f10x_md_gcc.s .vscode/ cov-int* /build/ +/build_SITL/ /obj/ /patches/ /tools/ /downloads/ -/debug/ +/debug/ /release/ # script-generated files diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 5ec93c583c2..5c829f6e7cd 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -69,6 +69,9 @@ #define USE_GPS_FAKE #define USE_RANGEFINDER_FAKE #define USE_RX_SIM +#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP +#undef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 #undef USE_DASHBOARD From 1d34800388d4501d5a430c3b880dd1b7f5807c1d Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 30 Jul 2023 22:36:24 +0900 Subject: [PATCH 29/60] initinal failsafe support for mixer profile --- src/main/fc/fc_core.c | 2 +- src/main/fc/settings.yaml | 24 +++ src/main/flight/failsafe.c | 49 ++++- src/main/flight/failsafe.h | 10 + src/main/flight/mixer.c | 2 +- src/main/flight/mixer_profile.c | 244 ++++++++++++++++++----- src/main/flight/mixer_profile.h | 30 ++- src/main/flight/servos.c | 2 +- src/main/io/osd.c | 2 + src/main/io/osd.h | 1 + src/main/io/osd_dji_hd.c | 2 + src/main/navigation/navigation.c | 40 +++- src/main/navigation/navigation.h | 4 + src/main/navigation/navigation_private.h | 1 + 14 files changed, 347 insertions(+), 66 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index df194954c4d..9eea10e4aa5 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -638,7 +638,7 @@ void processRx(timeUs_t currentTimeUs) bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) { + if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mixerATRequiresAngleMode()) && sensors(SENSOR_ACC)) { // bumpless transfer to Level mode canUseHorizonMode = false; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 9064ef63ad0..01265e60c21 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1173,6 +1173,30 @@ groups: default_value: OFF field: mixer_config.PIDProfileLinking type: bool + - name: mixer_switch_on_fs_rth + description: "If set to on, mixer_profile will switch on failsafe RTH" + default_value: OFF + field: mixer_config.switchOnFSRTH + type: bool + - name: mixer_switch_on_fs_land + description: "If set to on, mixer_profile will switch on failsafe Landing" + default_value: OFF + field: mixer_config.switchOnFSLand + type: bool + - name: mixer_switch_on_fs_stab_timer + description: "If swith mixer_profile on failsafe is required, Wait for this many decisecond(0.1s) before using mixerTransition input for servo and motor mixer" + default_value: 0 + field: mixer_config.switchOnFSStabilizationTimer + min: 0 + max: 200 + - name: mixer_switch_on_fs_trans_timer + description: "If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch" + default_value: 0 + field: mixer_config.switchOnFSTransitionTimer + min: 0 + max: 200 + + - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index ab48fba2c6e..71f5a74ec58 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -43,6 +43,7 @@ #include "fc/settings.h" #include "flight/failsafe.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -430,8 +431,14 @@ void failsafeUpdateState(void) switch (failsafeState.activeProcedure) { case FAILSAFE_PROCEDURE_AUTO_LANDING: // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level). - failsafeActivate(FAILSAFE_LANDING); - activateForcedEmergLanding(); + if(mixerATUpdateState(FAILSAFE_LANDING)) + { + failsafeActivate(FAILSAFE_LANDING); + activateForcedEmergLanding(); + } + else{ + failsafeActivate(FAILSAFE_MIXER_SWITCHING); + } break; case FAILSAFE_PROCEDURE_DROP_IT: @@ -441,9 +448,16 @@ void failsafeUpdateState(void) break; case FAILSAFE_PROCEDURE_RTH: - // Proceed to handling & monitoring RTH navigation - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); + if(mixerATUpdateState(FAILSAFE_RETURN_TO_HOME)) + { + // Proceed to handling & monitoring RTH navigation + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); + } + else{ + failsafeActivate(FAILSAFE_MIXER_SWITCHING); + activateForcedAltHold(); + } break; case FAILSAFE_PROCEDURE_NONE: default: @@ -466,6 +480,31 @@ void failsafeUpdateState(void) } break; + case FAILSAFE_MIXER_SWITCHING: + //enters when mixer switching is required + if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { + mixerATUpdateState(FAILSAFE_RX_LOSS_RECOVERED); + failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; + reprocessState = true; + } else { + if (armed) { + beeperMode = BEEPER_RX_LOST; + } + if (mixerATUpdateState(FAILSAFE_MIXER_SWITCHING)){ + failsafeActivate(FAILSAFE_RX_LOSS_DETECTED); //throw back to failsafe_rx_loss_detected to perform designated procedure + } + else + { + failsafeState.phase = FAILSAFE_MIXER_SWITCHING; //wait + } + if (!armed) { + failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData + failsafeState.phase = FAILSAFE_LANDED; + reprocessState = true; + } + } + break; + case FAILSAFE_RETURN_TO_HOME: if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { abortForcedRTH(); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 7cd88acd591..015cd040451 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -97,6 +97,10 @@ typedef enum { * and transition into FAILSAFE_RX_LOSS_MONITORING immediately. This is * the first recovery phase enabled when failsafe_procedure = DROP. */ + FAILSAFE_MIXER_SWITCHING, + /* Failsafe is executing MIXER_PROFILE_SWITCHING. This phase is triggered + if a MIXER_PROFILE_SWITCHING is required by the failsafe process + */ FAILSAFE_RX_LOSS_MONITORING, /* This phase will wait until the RX connection is * working for some time and if and only if switch arming @@ -134,6 +138,12 @@ typedef enum { EMERG_LAND_HAS_LANDED // Emergency landing is active and has landed. } emergLandState_e; +typedef enum { + ALTHOLD_IDLE = 0, // Altitude hold is waiting + ALTHOLD_IN_PROGRESS // Altitude hold is activated +} altHoldState_e; + + typedef struct failsafeState_s { int16_t events; bool monitoring; // Flag that failsafe is monitoring RC link diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 1f144dc60cc..9ba9a764417 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -618,7 +618,7 @@ void FAST_CODE mixTable(void) motor[i] = motorZeroCommand; } //spin stopped motors only in mixer transition mode - if (isInMixerTransition && currentMixer[i].throttle <= -1.05f && currentMixer[i].throttle >= -2.0f && (!feature(FEATURE_REVERSIBLE_MOTORS))) { + if (isMixerTransitionMixing && 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 12fe15a1eee..cdfdf148d56 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -14,6 +14,7 @@ #include "common/axis.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/failsafe.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -30,59 +31,69 @@ mixerConfig_t currentMixerConfig; int currentMixerProfileIndex; -bool isInMixerTransition; +bool isMixerTransitionMixing; +bool isMixerTransitionMixing_requested; +mixerProfileAT_t mixerProfileAT; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); void pgResetFn_mixerProfiles(mixerProfile_t *instance) { - for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) { + for (int i = 0; i < MAX_MIXER_PROFILE_COUNT; i++) + { RESET_CONFIG(mixerProfile_t, &instance[i], - .mixer_config = { - .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, - .platformType = SETTING_PLATFORM_TYPE_DEFAULT, - .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, - .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, - .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT - } - ); - for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { + .mixer_config = { + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .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, + .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, + .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, + .switchOnFSRTH = SETTING_MIXER_SWITCH_ON_FS_RTH_DEFAULT, + .switchOnFSLand = SETTING_MIXER_SWITCH_ON_FS_LAND_DEFAULT, + .switchOnFSStabilizationTimer = SETTING_MIXER_SWITCH_ON_FS_STAB_TIMER_DEFAULT, + .switchOnFSTransitionTimer = SETTING_MIXER_SWITCH_ON_FS_TRANS_TIMER_DEFAULT, + }); + for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) + { RESET_CONFIG(motorMixer_t, &instance[i].MotorMixers[j], - .throttle=0, - .roll=0, - .pitch=0, - .yaw=0 - ); + .throttle = 0, + .roll = 0, + .pitch = 0, + .yaw = 0); } - for (int j = 0; j < MAX_SERVO_RULES; j++) { + for (int j = 0; j < MAX_SERVO_RULES; j++) + { RESET_CONFIG(servoMixer_t, &instance[i].ServoMixers[j], - .targetChannel = 0, - .inputSource = 0, - .rate = 0, - .speed = 0 + .targetChannel = 0, + .inputSource = 0, + .rate = 0, + .speed = 0 #ifdef USE_PROGRAMMING_FRAMEWORK - ,.conditionId = -1, + , + .conditionId = -1, #endif ); } } } -void mixerConfigInit(void){ - currentMixerProfileIndex=getConfigMixerProfile(); - currentMixerConfig=*mixerConfig(); +void mixerConfigInit(void) +{ + currentMixerProfileIndex = getConfigMixerProfile(); + currentMixerConfig = *mixerConfig(); servosInit(); mixerUpdateStateFlags(); mixerInit(); - if(currentMixerConfig.PIDProfileLinking){ + if (currentMixerConfig.PIDProfileLinking) + { LOG_INFO(PWM, "mixer switch pidInit"); setConfigProfile(getConfigMixerProfile()); pidInit(); pidInitFilters(); schedulePidGainsUpdate(); - navigationUsePIDs(); //set navigation pid gains + navigationUsePIDs(); // set navigation pid gains } } @@ -129,12 +140,119 @@ void mixerConfigInit(void){ // } // } +bool mixerATRequiresAngleMode(void) +{ + return mixerProfileAT.phase == MIXERAT_PHASE_TRANSITIONING; +} + +void setMixerProfileAT(void) +{ + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; + mixerProfileAT.transitionStartTime = millis(); + mixerProfileAT.transitionStabEndTime = millis() + (timeMs_t)mixerConfig()->switchOnFSStabilizationTimer * 100; + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStabEndTime + (timeMs_t)mixerConfig()->switchOnFSTransitionTimer * 100; + activateForcedAltHold(); +} + +void performMixerProfileAT(int nextProfileIndex) +{ + abortForcedAltHold(); + isMixerTransitionMixing_requested = false; + outputProfileHotSwitch(nextProfileIndex); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; +} + +void abortMixerProfileAT(void) +{ + abortForcedAltHold(); + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; +} + +bool mixerATUpdateState(failsafePhase_e required_fs_phase) +{ + //set mixer profile automated transition according to failsafe phase + //on non vtol setups , behave as normal + if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) + { + return true; + } + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) + { + return true; + } + int nextProfileIndex = 0; + bool reprocessState=false; + do + { + nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; + switch (mixerProfileAT.phase) + { + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + if ((required_fs_phase == FAILSAFE_RETURN_TO_HOME) && mixerConfig()->switchOnFSRTH) + { + if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSRTH)//check next mixer_profile setting is valid + { + mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + } + } + else if ((required_fs_phase == FAILSAFE_LANDING) && mixerConfig()->switchOnFSLand) + { + if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSLand)//check next mixer_profile setting is valid + { + mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; + } + } + else + { + //return true if mixerAT condition is met or setting is not valid + return true; + } + break; + case MIXERAT_PHASE_TRANSITION_INITIALIZE: + setMixerProfileAT(); + reprocessState = true; + break; + case MIXERAT_PHASE_TRANSITIONING: + if (required_fs_phase==FAILSAFE_RX_LOSS_RECOVERED) + { + abortMixerProfileAT(); + reprocessState = true; + } + else + { + if (millis() < mixerProfileAT.transitionStabEndTime) + { + isMixerTransitionMixing_requested = false; + } + else{ + isMixerTransitionMixing_requested = true; + } + if (millis() > mixerProfileAT.transitionTransEndTime) + { + performMixerProfileAT(nextProfileIndex); + reprocessState = true; + //transition is done + } + } + break; + default: + break; + } + } + while (reprocessState); + return true; +} + 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) + // 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; } @@ -161,26 +279,44 @@ bool checkMixerProfileHotSwitchAvalibility(void) allow_hot_switch = 0; return false; } - //not necessary when map motor/servos of all mixer profiles on the first boot - //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; - // } + // not necessary when map motor/servos of all mixer profiles on the first boot + // 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) { +bool isNavBoxModesEnabled(void) +{ + return 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))); +} + +void outputProfileUpdateTask(timeUs_t currentTimeUs) +{ UNUSED(currentTimeUs); - 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))); - isInMixerTransition = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!navBoxModesEnabled) && ((posControl.navState == NAV_STATE_IDLE) || (posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); - outputProfileHotSwitch((int) IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); + // transition mode input for servo mix and motor mix + if (failsafePhase() == FAILSAFE_IDLE) + { + isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input + } + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + + if (failsafePhase() == FAILSAFE_IDLE) + { + // do not allow switching when user activated navigation mode + if (!isNavBoxModesEnabled()) + { + outputProfileHotSwitch((int)IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); + } + } } -//switch mixerprofile without reboot +// switch mixerprofile without reboot bool outputProfileHotSwitch(int profile_index) { static bool allow_hot_switch = true; @@ -199,24 +335,26 @@ bool outputProfileHotSwitch(int 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 + 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 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 (navBoxModesEnabled || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || posControl.navState != NAV_STATE_IDLE){ - // LOG_INFO(PWM, "mixer switch failed, navModesEnabled"); + if (!checkMixerProfileHotSwitchAvalibility()) + { + // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); return false; } - if (!checkMixerProfileHotSwitchAvalibility()){ - // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); + if (posControl.navState != NAV_STATE_IDLE) + { + // LOG_INFO(PWM, "mixer switch failed, navState != NAV_STATE_IDLE"); return false; } - if (!setConfigMixerProfile(profile_index)){ + if (!setConfigMixerProfile(profile_index)) + { // LOG_INFO(PWM, "mixer switch failed to set config"); return false; } - stopMotorsNoDelay(); + stopMotorsNoDelay(); // not necessary, but just in case something goes wrong. But will a short period of stop command cause problem? mixerConfigInit(); return true; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 25d4e793625..d145b949ce1 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -1,7 +1,7 @@ #pragma once #include "config/parameter_group.h" - +#include "flight/failsafe.h" #include "flight/mixer.h" #include "flight/servos.h" @@ -17,6 +17,10 @@ typedef struct mixerConfig_s { uint8_t outputMode; bool motorstopOnLow; bool PIDProfileLinking; + bool switchOnFSRTH; + bool switchOnFSLand; + int16_t switchOnFSStabilizationTimer; + int16_t switchOnFSTransitionTimer; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; @@ -26,9 +30,31 @@ typedef struct mixerProfile_s { PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); + +//mixerProfile Automated Transition PHASE +typedef enum { + MIXERAT_PHASE_IDLE, + MIXERAT_PHASE_TRANSITION_INITIALIZE, + MIXERAT_PHASE_TRANSITIONING, + MIXERAT_PHASE_DONE, +} mixerProfileATState_t; + +typedef struct mixerProfileAT_s { + mixerProfileATState_t phase; + bool transitionInputMixing; + timeMs_t transitionStartTime; + timeMs_t transitionStabEndTime; + timeMs_t transitionTransEndTime; + bool lastTransitionInputMixing; + bool lastMixerProfile; +} mixerProfileAT_t; +extern mixerProfileAT_t mixerProfileAT; +bool mixerATRequiresAngleMode(void); +bool mixerATUpdateState(failsafePhase_e required_fs_phase); + extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; -extern bool isInMixerTransition; +extern bool isMixerTransitionMixing; #define mixerConfig() (&(mixerProfiles(systemConfig()->current_mixer_profile_index)->mixer_config)) #define mixerConfigMutable() ((mixerConfig_t *) mixerConfig()) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4b421d41180..e3105456703 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -321,7 +321,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[INPUT_MIXER_TRANSITION] = isInMixerTransition * 500; //fixed value + input[INPUT_MIXER_TRANSITION] = isMixerTransitionMixing * 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/io/osd.c b/src/main/io/osd.c index 63312af46c2..b0be9743859 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -945,6 +945,8 @@ static const char * osdFailsafePhaseMessage(void) case FAILSAFE_LANDING: // This should be considered an emergengy landing return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); + case FAILSAFE_MIXER_SWITCHING: + return OSD_MESSAGE_STR(OSD_MSG_MIXER_SWITCHING_FS); case FAILSAFE_RX_LOSS_MONITORING: // Only reachable from FAILSAFE_LANDED, which performs // a disarm. Since aircraft has been disarmed, we no diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 2dad5ceb819..ab608caf6f7 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -88,6 +88,7 @@ #define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" +#define OSD_MSG_MIXER_SWITCHING_FS "(FS MIXER SWITCHING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" #define OSD_MSG_STARTING_RTH "STARTING RTH" #define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index e79ddc0dfd3..72cfc781aa2 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -558,6 +558,8 @@ static char * osdFailsafePhaseMessage(void) case FAILSAFE_LANDING: // This should be considered an emergengy landing return OSD_MESSAGE_STR("(EMRGY LANDING)"); + case FAILSAFE_MIXER_SWITCHING: + return OSD_MESSAGE_STR("(MIXER SWITCHING)"); case FAILSAFE_RX_LOSS_MONITORING: // Only reachable from FAILSAFE_LANDED, which performs // a disarm. Since aircraft has been disarmed, we no diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 091a1245186..7ffa8c3388e 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3740,6 +3740,11 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } + // Failsafe_Altitude hold for vtol transition (can override MANUAL) + if (posControl.flags.forcedAltHoldActivated && canActivateAltHold) { + return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; + } + // Failsafe_RTH (can override MANUAL) if (posControl.flags.forcedRTHActivated) { // If we request forced RTH - attempt to activate it no matter what @@ -3789,14 +3794,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // CRUISE has priority over COURSE_HOLD and AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } // PH has priority over COURSE_HOLD // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } @@ -4193,6 +4198,7 @@ void navigationInit(void) posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; + posControl.flags.forcedAltHoldActivated = false; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; @@ -4316,6 +4322,34 @@ emergLandState_e getStateOfForcedEmergLanding(void) } } + +/*----------------------------------------------------------- + * Ability to mixer_profile(vtol) switch on external event + *-----------------------------------------------------------*/ +void activateForcedAltHold(void) +{ + posControl.flags.forcedAltHoldActivated = true; + navProcessFSMEvents(selectNavEventFromBoxModeInput()); +} + +void abortForcedAltHold(void) +{ + // Disable emergency landing and make sure we back out of navigation mode to IDLE + // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update + posControl.flags.forcedAltHoldActivated = false; + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); +} + +altHoldState_e getStateOfForcedAltHold(void) +{ + /* If forced emergency landing activated and in EMERG state */ + if (posControl.flags.forcedAltHoldActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { + return ALTHOLD_IN_PROGRESS; + } else { + return ALTHOLD_IDLE; + } +} + bool isWaypointMissionRTHActive(void) { return (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) && IS_RC_MODE_ACTIVE(BOXNAVWP) && @@ -4373,7 +4407,7 @@ bool navigationRTHAllowsLanding(void) bool isNavLaunchEnabled(void) { - return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE); + return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH); } bool abortLaunchAllowed(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index cf4289e14a1..2bae5f8a729 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -588,6 +588,10 @@ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); emergLandState_e getStateOfForcedEmergLanding(void); +void activateForcedAltHold(void); +void abortForcedAltHold(void); +altHoldState_e getStateOfForcedAltHold(void); + /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); bool navigationIsControllingThrottle(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 0598467d28a..096fac54260 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -103,6 +103,7 @@ typedef struct navigationFlags_s { // Failsafe actions bool forcedRTHActivated; bool forcedEmergLandingActivated; + bool forcedAltHoldActivated; /* Landing detector */ bool resetLandingDetector; From e46ccfe7d5536bce377e869e31f597c9fdea1e6b Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 31 Jul 2023 00:17:10 +0900 Subject: [PATCH 30/60] minor bug fix on failsafe mixertransition --- src/main/flight/failsafe.c | 1 - src/main/flight/mixer_profile.c | 36 ++++++++++++++++++--------------- 2 files changed, 20 insertions(+), 17 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 71f5a74ec58..e244392ffec 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -456,7 +456,6 @@ void failsafeUpdateState(void) } else{ failsafeActivate(FAILSAFE_MIXER_SWITCHING); - activateForcedAltHold(); } break; case FAILSAFE_PROCEDURE_NONE: diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index cdfdf148d56..8207bb0835e 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -147,10 +147,17 @@ bool mixerATRequiresAngleMode(void) void setMixerProfileAT(void) { - mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; mixerProfileAT.transitionStartTime = millis(); - mixerProfileAT.transitionStabEndTime = millis() + (timeMs_t)mixerConfig()->switchOnFSStabilizationTimer * 100; - mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStabEndTime + (timeMs_t)mixerConfig()->switchOnFSTransitionTimer * 100; + if (isMixerTransitionMixing && STATE(MULTIROTOR)) + { + mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime; + } + else + { + mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchOnFSStabilizationTimer * 100; + } + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStabEndTime + (timeMs_t)currentMixerConfig.switchOnFSTransitionTimer * 100; + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; activateForcedAltHold(); } @@ -171,6 +178,7 @@ void abortMixerProfileAT(void) bool mixerATUpdateState(failsafePhase_e required_fs_phase) { + //return true if mixerAT condition is met or setting is not valid //set mixer profile automated transition according to failsafe phase //on non vtol setups , behave as normal if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) @@ -182,15 +190,17 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) return true; } int nextProfileIndex = 0; - bool reprocessState=false; + bool reprocessState; do { + reprocessState=false; nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; switch (mixerProfileAT.phase) { case MIXERAT_PHASE_IDLE: + // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); //check if mixerAT is required - if ((required_fs_phase == FAILSAFE_RETURN_TO_HOME) && mixerConfig()->switchOnFSRTH) + if ((required_fs_phase == FAILSAFE_RETURN_TO_HOME) && currentMixerConfig.switchOnFSRTH) { if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSRTH)//check next mixer_profile setting is valid { @@ -198,7 +208,7 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) reprocessState = true; } } - else if ((required_fs_phase == FAILSAFE_LANDING) && mixerConfig()->switchOnFSLand) + else if ((required_fs_phase == FAILSAFE_LANDING) && currentMixerConfig.switchOnFSLand) { if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSLand)//check next mixer_profile setting is valid { @@ -206,29 +216,22 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) reprocessState = true; } } - else - { - //return true if mixerAT condition is met or setting is not valid - return true; - } break; case MIXERAT_PHASE_TRANSITION_INITIALIZE: + // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); setMixerProfileAT(); reprocessState = true; break; case MIXERAT_PHASE_TRANSITIONING: + // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); if (required_fs_phase==FAILSAFE_RX_LOSS_RECOVERED) { abortMixerProfileAT(); - reprocessState = true; } else { - if (millis() < mixerProfileAT.transitionStabEndTime) + if (millis() > mixerProfileAT.transitionStabEndTime) { - isMixerTransitionMixing_requested = false; - } - else{ isMixerTransitionMixing_requested = true; } if (millis() > mixerProfileAT.transitionTransEndTime) @@ -237,6 +240,7 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) reprocessState = true; //transition is done } + return false; } break; default: From 310bb097a5bdc533fc6ebf4a1ddc5f37a5d03e63 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 31 Jul 2023 03:12:25 +0900 Subject: [PATCH 31/60] modification on airmodes --- src/main/fc/fc_core.c | 4 ++-- src/main/flight/mixer_profile.c | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9eea10e4aa5..a7e1cf53522 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -737,7 +737,7 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { + if (STATE(AIRMODE_ACTIVE)) { if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { ENABLE_STATE(ANTI_WINDUP); } @@ -756,7 +756,7 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER_ONCE) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { + if (STATE(AIRMODE_ACTIVE)) { if ((rollPitchStatus == CENTERED) && !STATE(ANTI_WINDUP_DEACTIVATED)) { ENABLE_STATE(ANTI_WINDUP); } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 8207bb0835e..1875cb4bfc0 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -92,6 +92,7 @@ void mixerConfigInit(void) setConfigProfile(getConfigMixerProfile()); pidInit(); pidInitFilters(); + pidResetErrorAccumulators(); schedulePidGainsUpdate(); navigationUsePIDs(); // set navigation pid gains } From 384439a809f10f4bc9e0cb8f3ab3375b29eea68f Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 31 Jul 2023 23:25:00 +0900 Subject: [PATCH 32/60] automated transition --- src/main/flight/failsafe.c | 13 +-- src/main/flight/mixer_profile.c | 66 ++++++------- src/main/flight/mixer_profile.h | 10 +- src/main/navigation/navigation.c | 115 ++++++++++++++++++++--- src/main/navigation/navigation.h | 6 +- src/main/navigation/navigation_private.h | 9 +- 6 files changed, 156 insertions(+), 63 deletions(-) diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index e244392ffec..2fa27cff14c 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -431,7 +431,7 @@ void failsafeUpdateState(void) switch (failsafeState.activeProcedure) { case FAILSAFE_PROCEDURE_AUTO_LANDING: // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level). - if(mixerATUpdateState(FAILSAFE_LANDING)) + if(mixerATUpdateState(MIXERAT_REQUEST_LAND)) { failsafeActivate(FAILSAFE_LANDING); activateForcedEmergLanding(); @@ -448,7 +448,7 @@ void failsafeUpdateState(void) break; case FAILSAFE_PROCEDURE_RTH: - if(mixerATUpdateState(FAILSAFE_RETURN_TO_HOME)) + if(mixerATUpdateState(MIXERAT_REQUEST_RTH)) { // Proceed to handling & monitoring RTH navigation failsafeActivate(FAILSAFE_RETURN_TO_HOME); @@ -482,19 +482,16 @@ void failsafeUpdateState(void) case FAILSAFE_MIXER_SWITCHING: //enters when mixer switching is required if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { - mixerATUpdateState(FAILSAFE_RX_LOSS_RECOVERED); + mixerATUpdateState(MIXERAT_REQUEST_ABORT); failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; reprocessState = true; } else { if (armed) { beeperMode = BEEPER_RX_LOST; } - if (mixerATUpdateState(FAILSAFE_MIXER_SWITCHING)){ + if (mixerATUpdateState(MIXERAT_REQUEST_NONE)){ failsafeActivate(FAILSAFE_RX_LOSS_DETECTED); //throw back to failsafe_rx_loss_detected to perform designated procedure - } - else - { - failsafeState.phase = FAILSAFE_MIXER_SWITCHING; //wait + reprocessState = true; } if (!armed) { failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 1875cb4bfc0..779a6c7f112 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -143,41 +143,31 @@ void mixerConfigInit(void) bool mixerATRequiresAngleMode(void) { - return mixerProfileAT.phase == MIXERAT_PHASE_TRANSITIONING; + return (mixerProfileAT.phase == MIXERAT_PHASE_TRANSITIONING) || (mixerProfileAT.phase == MIXERAT_PHASE_STAB_AND_CLIMB); } void setMixerProfileAT(void) { mixerProfileAT.transitionStartTime = millis(); - if (isMixerTransitionMixing && STATE(MULTIROTOR)) - { - mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime; - } - else - { - mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchOnFSStabilizationTimer * 100; - } + mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchOnFSStabilizationTimer * 100; mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStabEndTime + (timeMs_t)currentMixerConfig.switchOnFSTransitionTimer * 100; - mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; - activateForcedAltHold(); + activateMIXERATHelper(); } void performMixerProfileAT(int nextProfileIndex) { - abortForcedAltHold(); + abortMIXERATHelper(); isMixerTransitionMixing_requested = false; outputProfileHotSwitch(nextProfileIndex); - mixerProfileAT.phase = MIXERAT_PHASE_IDLE; } void abortMixerProfileAT(void) { - abortForcedAltHold(); + abortMIXERATHelper(); isMixerTransitionMixing_requested = false; - mixerProfileAT.phase = MIXERAT_PHASE_IDLE; } -bool mixerATUpdateState(failsafePhase_e required_fs_phase) +bool mixerATUpdateState(mixerProfileATRequest_t required_action) { //return true if mixerAT condition is met or setting is not valid //set mixer profile automated transition according to failsafe phase @@ -196,12 +186,16 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) { reprocessState=false; nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; + if (required_action==MIXERAT_REQUEST_ABORT){ + abortMixerProfileAT(); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + } switch (mixerProfileAT.phase) { case MIXERAT_PHASE_IDLE: // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); //check if mixerAT is required - if ((required_fs_phase == FAILSAFE_RETURN_TO_HOME) && currentMixerConfig.switchOnFSRTH) + if ((required_action == MIXERAT_REQUEST_RTH) && currentMixerConfig.switchOnFSRTH && STATE(MULTIROTOR)) { if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSRTH)//check next mixer_profile setting is valid { @@ -209,7 +203,7 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) reprocessState = true; } } - else if ((required_fs_phase == FAILSAFE_LANDING) && currentMixerConfig.switchOnFSLand) + else if ((required_action == MIXERAT_REQUEST_LAND) && currentMixerConfig.switchOnFSLand && STATE(AIRPLANE)) { if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSLand)//check next mixer_profile setting is valid { @@ -221,28 +215,26 @@ bool mixerATUpdateState(failsafePhase_e required_fs_phase) case MIXERAT_PHASE_TRANSITION_INITIALIZE: // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); setMixerProfileAT(); + mixerProfileAT.phase = MIXERAT_PHASE_STAB_AND_CLIMB; reprocessState = true; break; - case MIXERAT_PHASE_TRANSITIONING: - // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); - if (required_fs_phase==FAILSAFE_RX_LOSS_RECOVERED) - { - abortMixerProfileAT(); + case MIXERAT_PHASE_STAB_AND_CLIMB: + isMixerTransitionMixing_requested = false; + if (millis() > mixerProfileAT.transitionStabEndTime){ + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; + reprocessState = true; } - else - { - if (millis() > mixerProfileAT.transitionStabEndTime) - { - isMixerTransitionMixing_requested = true; - } - if (millis() > mixerProfileAT.transitionTransEndTime) - { - performMixerProfileAT(nextProfileIndex); - reprocessState = true; - //transition is done - } - return false; + return false; + break; + case MIXERAT_PHASE_TRANSITIONING: + isMixerTransitionMixing_requested = true; + if (millis() > mixerProfileAT.transitionTransEndTime){ + performMixerProfileAT(nextProfileIndex); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + reprocessState = true; + //transition is done } + return false; break; default: break; @@ -309,7 +301,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) { isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) ||(posControl.navState == NAV_STATE_MIXERAT_IN_PROGRESS)); if (failsafePhase() == FAILSAFE_IDLE) { diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index d145b949ce1..22eb4841850 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -31,10 +31,18 @@ typedef struct mixerProfile_s { PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); +typedef enum { + MIXERAT_REQUEST_NONE, //no request, stats checking only + MIXERAT_REQUEST_RTH, + MIXERAT_REQUEST_LAND, + MIXERAT_REQUEST_ABORT, +} mixerProfileATRequest_t; + //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, + MIXERAT_PHASE_STAB_AND_CLIMB, MIXERAT_PHASE_TRANSITIONING, MIXERAT_PHASE_DONE, } mixerProfileATState_t; @@ -50,7 +58,7 @@ typedef struct mixerProfileAT_s { } mixerProfileAT_t; extern mixerProfileAT_t mixerProfileAT; bool mixerATRequiresAngleMode(void); -bool mixerATUpdateState(failsafePhase_e required_fs_phase); +bool mixerATUpdateState(mixerProfileATRequest_t required_action); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7ffa8c3388e..70b739b5cc8 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -314,6 +314,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_FINIS static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -334,6 +336,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -370,6 +373,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -406,6 +410,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, /** CRUISE_HOLD mode ************************************************/ @@ -442,6 +447,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -464,6 +470,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -501,6 +508,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -523,6 +531,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -562,6 +571,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -582,6 +592,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -602,6 +613,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -623,6 +635,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -642,6 +655,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -660,6 +674,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -691,6 +706,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -747,6 +763,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -771,6 +788,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -792,6 +810,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -813,6 +832,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -847,6 +867,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -866,6 +887,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -884,6 +906,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -947,6 +970,43 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, + + /** MIXER AUTOMATED TRANSITION mode, alternated althod ***************************************************/ + [NAV_STATE_MIXERAT_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE, + .timeoutMs = 0, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_MIXERAT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } + }, + + [NAV_STATE_MIXERAT_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + } + }, }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1864,6 +1924,35 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_NONE; } +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState) +{ + UNUSED(previousState); + resetGCSFlags(); + + // Prepare altitude controller + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset + + return NAV_FSM_EVENT_SUCCESS; +} + +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState) +{ + UNUSED(previousState); + + // Climb to safe altitude and turn to correct direction + if ( (STATE(MULTIROTOR)) && (mixerProfileAT.phase==MIXERAT_PHASE_STAB_AND_CLIMB)) { + updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + // fpVector3_t tmp_pos = navGetCurrentActualPositionAndVelocity()->pos; + // tmp_pos.z = tmp_pos.z + navConfig()->general.max_auto_climb_rate; + // setDesiredPosition(&tmp_pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + } else { + // climb to perform the transition + //set the desired position to the current position to enable althold only + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + } + return NAV_FSM_EVENT_NONE; +} + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -3740,9 +3829,9 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // Failsafe_Altitude hold for vtol transition (can override MANUAL) - if (posControl.flags.forcedAltHoldActivated && canActivateAltHold) { - return NAV_FSM_EVENT_SWITCH_TO_ALTHOLD; + // Altitude hold for vtol transition (can override MANUAL) + if (posControl.flags.mixerATHelperActivated && canActivateAltHold) { + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } // Failsafe_RTH (can override MANUAL) @@ -3794,14 +3883,14 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } // CRUISE has priority over COURSE_HOLD and AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE) && STATE(AIRPLANE)) { if ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold)) return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } // PH has priority over COURSE_HOLD // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD) && STATE(AIRPLANE)) { if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) { return NAV_FSM_EVENT_SWITCH_TO_CRUISE; } @@ -4198,7 +4287,7 @@ void navigationInit(void) posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; - posControl.flags.forcedAltHoldActivated = false; + posControl.flags.mixerATHelperActivated = false; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; @@ -4326,24 +4415,24 @@ emergLandState_e getStateOfForcedEmergLanding(void) /*----------------------------------------------------------- * Ability to mixer_profile(vtol) switch on external event *-----------------------------------------------------------*/ -void activateForcedAltHold(void) +void activateMIXERATHelper(void) { - posControl.flags.forcedAltHoldActivated = true; + posControl.flags.mixerATHelperActivated = true; navProcessFSMEvents(selectNavEventFromBoxModeInput()); } -void abortForcedAltHold(void) +void abortMIXERATHelper(void) { // Disable emergency landing and make sure we back out of navigation mode to IDLE // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update - posControl.flags.forcedAltHoldActivated = false; + posControl.flags.mixerATHelperActivated = false; navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } -altHoldState_e getStateOfForcedAltHold(void) +altHoldState_e getStateOfMIXERATHelper(void) { /* If forced emergency landing activated and in EMERG state */ - if (posControl.flags.forcedAltHoldActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { + if (posControl.flags.mixerATHelperActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { return ALTHOLD_IN_PROGRESS; } else { return ALTHOLD_IDLE; @@ -4407,7 +4496,7 @@ bool navigationRTHAllowsLanding(void) bool isNavLaunchEnabled(void) { - return IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH); + return (IS_RC_MODE_ACTIVE(BOXNAVLAUNCH) || feature(FEATURE_FW_LAUNCH)) && STATE(AIRPLANE); } bool abortLaunchAllowed(void) diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 2bae5f8a729..a9b23e9b32a 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -588,9 +588,9 @@ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); emergLandState_e getStateOfForcedEmergLanding(void); -void activateForcedAltHold(void); -void abortForcedAltHold(void); -altHoldState_e getStateOfForcedAltHold(void); +void activateMIXERATHelper(void); +void abortMIXERATHelper(void); +altHoldState_e getStateOfMIXERATHelper(void); /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 096fac54260..78ecef6f1f9 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -103,7 +103,7 @@ typedef struct navigationFlags_s { // Failsafe actions bool forcedRTHActivated; bool forcedEmergLandingActivated; - bool forcedAltHoldActivated; + bool mixerATHelperActivated; /* Landing detector */ bool resetLandingDetector; @@ -156,6 +156,7 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, NAV_FSM_EVENT_SWITCH_TO_CRUISE, NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, + NAV_FSM_EVENT_SWITCH_TO_MIXERAT, NAV_FSM_EVENT_STATE_SPECIFIC_1, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_2, // State-specific event @@ -227,6 +228,9 @@ typedef enum { NAV_PERSISTENT_ID_UNUSED_4 = 37, // was NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME NAV_PERSISTENT_ID_RTH_TRACKBACK = 38, + NAV_PERSISTENT_ID_MIXERAT_INITIALIZE = 39, + NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS = 40, + } navigationPersistentId_e; typedef enum { @@ -274,6 +278,9 @@ typedef enum { NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, + NAV_STATE_MIXERAT_INITIALIZE, + NAV_STATE_MIXERAT_IN_PROGRESS, + NAV_STATE_COUNT, } navigationFSMState_t; From 7c8b9976b71bec193c8bcd4ed70e1daa7ad82567 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 2 Aug 2023 00:44:22 +0900 Subject: [PATCH 33/60] automatic mixer profile switching on rth and land --- src/main/fc/fc_core.c | 2 +- src/main/fc/settings.yaml | 35 +++-- src/main/flight/failsafe.c | 45 +----- src/main/flight/failsafe.h | 10 -- src/main/flight/mixer_profile.c | 158 +++++++-------------- src/main/flight/mixer_profile.h | 28 ++-- src/main/io/osd.c | 2 - src/main/io/osd.h | 1 - src/main/io/osd_dji_hd.c | 2 - src/main/navigation/navigation.c | 168 ++++++++++++++--------- src/main/navigation/navigation.h | 6 +- src/main/navigation/navigation_private.h | 6 +- 12 files changed, 194 insertions(+), 269 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index a7e1cf53522..e6358080aa9 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -638,7 +638,7 @@ void processRx(timeUs_t currentTimeUs) bool canUseHorizonMode = true; - if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode() || mixerATRequiresAngleMode()) && sensors(SENSOR_ACC)) { + if ((IS_RC_MODE_ACTIVE(BOXANGLE) || failsafeRequiresAngleMode() || navigationRequiresAngleMode()) && sensors(SENSOR_ACC)) { // bumpless transfer to Level mode canUseHorizonMode = false; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 01265e60c21..bf6aeb40cb5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -190,6 +190,9 @@ tables: - name: nav_fw_wp_turn_smoothing values: ["OFF", "ON", "ON-CUT"] enum: wpFwTurnSmoothing_e + - name: mixer_switch_on_event + values: ["OFF", "ON", "ON_FS_ONLY"] + enum: mixerProfileSwitchOnEvent_e constants: RPYL_PID_MIN: 0 @@ -1173,26 +1176,22 @@ groups: default_value: OFF field: mixer_config.PIDProfileLinking type: bool - - name: mixer_switch_on_fs_rth - description: "If set to on, mixer_profile will switch on failsafe RTH" - default_value: OFF - field: mixer_config.switchOnFSRTH - type: bool - - name: mixer_switch_on_fs_land - description: "If set to on, mixer_profile will switch on failsafe Landing" - default_value: OFF - field: mixer_config.switchOnFSLand - type: bool - - name: mixer_switch_on_fs_stab_timer - description: "If swith mixer_profile on failsafe is required, Wait for this many decisecond(0.1s) before using mixerTransition input for servo and motor mixer" - default_value: 0 - field: mixer_config.switchOnFSStabilizationTimer - min: 0 - max: 200 - - name: mixer_switch_on_fs_trans_timer + - name: mixer_switch_on_rth + description: "If set to on, mixer_profile will switch when it is heading home" + default_value: "OFF" + field: mixer_config.switchOnRTH + table: mixer_switch_on_event + type: uint8_t + - name: mixer_switch_on_land + description: "If set to on, mixer_profile will switch when Landing" + default_value: "OFF" + field: mixer_config.switchOnLand + table: mixer_switch_on_event + type: uint8_t + - name: mixer_switch_trans_timer description: "If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch" default_value: 0 - field: mixer_config.switchOnFSTransitionTimer + field: mixer_config.switchTransitionTimer min: 0 max: 200 diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 2fa27cff14c..ab48fba2c6e 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -43,7 +43,6 @@ #include "fc/settings.h" #include "flight/failsafe.h" -#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/pid.h" @@ -431,14 +430,8 @@ void failsafeUpdateState(void) switch (failsafeState.activeProcedure) { case FAILSAFE_PROCEDURE_AUTO_LANDING: // Use Emergency Landing if Nav defined (otherwise stabilize and set Throttle to specified level). - if(mixerATUpdateState(MIXERAT_REQUEST_LAND)) - { - failsafeActivate(FAILSAFE_LANDING); - activateForcedEmergLanding(); - } - else{ - failsafeActivate(FAILSAFE_MIXER_SWITCHING); - } + failsafeActivate(FAILSAFE_LANDING); + activateForcedEmergLanding(); break; case FAILSAFE_PROCEDURE_DROP_IT: @@ -448,15 +441,9 @@ void failsafeUpdateState(void) break; case FAILSAFE_PROCEDURE_RTH: - if(mixerATUpdateState(MIXERAT_REQUEST_RTH)) - { - // Proceed to handling & monitoring RTH navigation - failsafeActivate(FAILSAFE_RETURN_TO_HOME); - activateForcedRTH(); - } - else{ - failsafeActivate(FAILSAFE_MIXER_SWITCHING); - } + // Proceed to handling & monitoring RTH navigation + failsafeActivate(FAILSAFE_RETURN_TO_HOME); + activateForcedRTH(); break; case FAILSAFE_PROCEDURE_NONE: default: @@ -479,28 +466,6 @@ void failsafeUpdateState(void) } break; - case FAILSAFE_MIXER_SWITCHING: - //enters when mixer switching is required - if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { - mixerATUpdateState(MIXERAT_REQUEST_ABORT); - failsafeState.phase = FAILSAFE_RX_LOSS_RECOVERED; - reprocessState = true; - } else { - if (armed) { - beeperMode = BEEPER_RX_LOST; - } - if (mixerATUpdateState(MIXERAT_REQUEST_NONE)){ - failsafeActivate(FAILSAFE_RX_LOSS_DETECTED); //throw back to failsafe_rx_loss_detected to perform designated procedure - reprocessState = true; - } - if (!armed) { - failsafeState.receivingRxDataPeriodPreset = PERIOD_OF_30_SECONDS; // require 30 seconds of valid rxData - failsafeState.phase = FAILSAFE_LANDED; - reprocessState = true; - } - } - break; - case FAILSAFE_RETURN_TO_HOME: if (receivingRxDataAndNotFailsafeMode && sticksAreMoving) { abortForcedRTH(); diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 015cd040451..7cd88acd591 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -97,10 +97,6 @@ typedef enum { * and transition into FAILSAFE_RX_LOSS_MONITORING immediately. This is * the first recovery phase enabled when failsafe_procedure = DROP. */ - FAILSAFE_MIXER_SWITCHING, - /* Failsafe is executing MIXER_PROFILE_SWITCHING. This phase is triggered - if a MIXER_PROFILE_SWITCHING is required by the failsafe process - */ FAILSAFE_RX_LOSS_MONITORING, /* This phase will wait until the RX connection is * working for some time and if and only if switch arming @@ -138,12 +134,6 @@ typedef enum { EMERG_LAND_HAS_LANDED // Emergency landing is active and has landed. } emergLandState_e; -typedef enum { - ALTHOLD_IDLE = 0, // Altitude hold is waiting - ALTHOLD_IN_PROGRESS // Altitude hold is activated -} altHoldState_e; - - typedef struct failsafeState_s { int16_t events; bool monitoring; // Flag that failsafe is monitoring RC link diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 779a6c7f112..f2b6708727f 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -34,6 +34,7 @@ int currentMixerProfileIndex; bool isMixerTransitionMixing; bool isMixerTransitionMixing_requested; mixerProfileAT_t mixerProfileAT; +int nextProfileIndex; PG_REGISTER_ARRAY_WITH_RESET_FN(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles, PG_MIXER_PROFILE, 1); @@ -50,10 +51,9 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .outputMode = SETTING_OUTPUT_MODE_DEFAULT, .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, - .switchOnFSRTH = SETTING_MIXER_SWITCH_ON_FS_RTH_DEFAULT, - .switchOnFSLand = SETTING_MIXER_SWITCH_ON_FS_LAND_DEFAULT, - .switchOnFSStabilizationTimer = SETTING_MIXER_SWITCH_ON_FS_STAB_TIMER_DEFAULT, - .switchOnFSTransitionTimer = SETTING_MIXER_SWITCH_ON_FS_TRANS_TIMER_DEFAULT, + .switchOnRTH = SETTING_MIXER_SWITCH_ON_RTH_DEFAULT, + .switchOnLand = SETTING_MIXER_SWITCH_ON_LAND_DEFAULT, + .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) { @@ -83,12 +83,13 @@ void mixerConfigInit(void) { currentMixerProfileIndex = getConfigMixerProfile(); currentMixerConfig = *mixerConfig(); + nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; servosInit(); mixerUpdateStateFlags(); mixerInit(); if (currentMixerConfig.PIDProfileLinking) { - LOG_INFO(PWM, "mixer switch pidInit"); + // LOG_INFO(PWM, "mixer switch pidInit"); setConfigProfile(getConfigMixerProfile()); pidInit(); pidInitFilters(); @@ -98,77 +99,14 @@ void mixerConfigInit(void) } } -// static int computeMotorCountByMixerProfileIndex(int index) -// { -// int motorCount = 0; -// const motorMixer_t* temp_motormixers=mixerMotorMixersByIndex(index)[0]; -// for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { -// // check if done -// if (temp_motormixers[i].throttle == 0.0f) { -// break; -// } -// motorCount++; -// } -// return motorCount; -// } - -// static int computeServoCountByMixerProfileIndex(int index) -// { -// int servoRuleCount = 0; -// int minServoIndex = 255; -// int maxServoIndex = 0; - -// const servoMixer_t* temp_servomixers=mixerServoMixersByIndex(index)[0]; -// for (int i = 0; i < MAX_SERVO_RULES; i++) { -// if (temp_servomixers[i].rate == 0) -// break; - -// if (temp_servomixers[i].targetChannel < minServoIndex) { -// minServoIndex = temp_servomixers[i].targetChannel; -// } - -// if (temp_servomixers[i].targetChannel > maxServoIndex) { -// maxServoIndex = temp_servomixers[i].targetChannel; -// } -// // LOG_INFO(PWM, "i:%d, minServoIndex:%d, maxServoIndex:%d",i,minServoIndex,maxServoIndex); -// servoRuleCount++; -// } -// if (servoRuleCount) { -// return 1 + maxServoIndex - minServoIndex; -// } -// else { -// return 0; -// } -// } - -bool mixerATRequiresAngleMode(void) -{ - return (mixerProfileAT.phase == MIXERAT_PHASE_TRANSITIONING) || (mixerProfileAT.phase == MIXERAT_PHASE_STAB_AND_CLIMB); -} - void setMixerProfileAT(void) { mixerProfileAT.transitionStartTime = millis(); - mixerProfileAT.transitionStabEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchOnFSStabilizationTimer * 100; - mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStabEndTime + (timeMs_t)currentMixerConfig.switchOnFSTransitionTimer * 100; - activateMIXERATHelper(); -} - -void performMixerProfileAT(int nextProfileIndex) -{ - abortMIXERATHelper(); - isMixerTransitionMixing_requested = false; - outputProfileHotSwitch(nextProfileIndex); + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; } -void abortMixerProfileAT(void) +bool checkMixerATRequired(mixerProfileATRequest_e required_action) { - abortMIXERATHelper(); - isMixerTransitionMixing_requested = false; -} - -bool mixerATUpdateState(mixerProfileATRequest_t required_action) -{ //return true if mixerAT condition is met or setting is not valid //set mixer profile automated transition according to failsafe phase //on non vtol setups , behave as normal @@ -180,56 +118,59 @@ bool mixerATUpdateState(mixerProfileATRequest_t required_action) { return true; } - int nextProfileIndex = 0; + + if ((required_action == MIXERAT_REQUEST_RTH) && (currentMixerConfig.switchOnRTH!=MIXERAT_ON_EVENT_OFF) && STATE(MULTIROTOR)) + { + if ((currentMixerConfig.switchOnRTH==MIXERAT_ON_EVENT_ON_FS_ONLY) && (!FLIGHT_MODE(FAILSAFE_MODE))) + { + return false; + } + //check next mixer_profile setting is valid + return mixerConfigByIndex(nextProfileIndex)->switchOnRTH == MIXERAT_ON_EVENT_OFF ? true:false; + + } + else if ((required_action == MIXERAT_REQUEST_LAND) && (currentMixerConfig.switchOnLand!=MIXERAT_ON_EVENT_OFF) && STATE(AIRPLANE)) + { + if ((currentMixerConfig.switchOnLand==MIXERAT_ON_EVENT_ON_FS_ONLY) && (!FLIGHT_MODE(FAILSAFE_MODE))) + { + return false; + } + //check next mixer_profile setting is valid + return mixerConfigByIndex(nextProfileIndex)->switchOnLand == MIXERAT_ON_EVENT_OFF ? true:false; + } + return false; +} + +bool mixerATUpdateState(mixerProfileATRequest_e required_action) +{ bool reprocessState; do { reprocessState=false; - nextProfileIndex = (currentMixerProfileIndex + 1) % MAX_MIXER_PROFILE_COUNT; if (required_action==MIXERAT_REQUEST_ABORT){ - abortMixerProfileAT(); + isMixerTransitionMixing_requested = false; mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + return true; } - switch (mixerProfileAT.phase) - { + switch (mixerProfileAT.phase){ case MIXERAT_PHASE_IDLE: - // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); //check if mixerAT is required - if ((required_action == MIXERAT_REQUEST_RTH) && currentMixerConfig.switchOnFSRTH && STATE(MULTIROTOR)) - { - if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSRTH)//check next mixer_profile setting is valid - { - mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; - reprocessState = true; - } - } - else if ((required_action == MIXERAT_REQUEST_LAND) && currentMixerConfig.switchOnFSLand && STATE(AIRPLANE)) - { - if(!mixerConfigByIndex(nextProfileIndex)->switchOnFSLand)//check next mixer_profile setting is valid - { - mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; - reprocessState = true; - } + if (checkMixerATRequired(required_action)){ + mixerProfileAT.phase=MIXERAT_PHASE_TRANSITION_INITIALIZE; + reprocessState = true; } break; case MIXERAT_PHASE_TRANSITION_INITIALIZE: // LOG_INFO(PWM, "MIXERAT_PHASE_IDLE"); setMixerProfileAT(); - mixerProfileAT.phase = MIXERAT_PHASE_STAB_AND_CLIMB; + mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; reprocessState = true; break; - case MIXERAT_PHASE_STAB_AND_CLIMB: - isMixerTransitionMixing_requested = false; - if (millis() > mixerProfileAT.transitionStabEndTime){ - mixerProfileAT.phase = MIXERAT_PHASE_TRANSITIONING; - reprocessState = true; - } - return false; - break; case MIXERAT_PHASE_TRANSITIONING: isMixerTransitionMixing_requested = true; if (millis() > mixerProfileAT.transitionTransEndTime){ - performMixerProfileAT(nextProfileIndex); + isMixerTransitionMixing_requested = false; + outputProfileHotSwitch(nextProfileIndex); mixerProfileAT.phase = MIXERAT_PHASE_IDLE; reprocessState = true; //transition is done @@ -295,22 +236,17 @@ bool isNavBoxModesEnabled(void) void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - + bool nav_mixerAT_inuse = (posControl.navState == NAV_STATE_MIXERAT_IN_PROGRESS || posControl.navState == NAV_STATE_MIXERAT_ABORT); // transition mode input for servo mix and motor mix - if (failsafePhase() == FAILSAFE_IDLE) + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!nav_mixerAT_inuse)) { - isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input - } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) ||(posControl.navState == NAV_STATE_MIXERAT_IN_PROGRESS)); - - if (failsafePhase() == FAILSAFE_IDLE) - { - // do not allow switching when user activated navigation mode if (!isNavBoxModesEnabled()) { outputProfileHotSwitch((int)IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); } + isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input } + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || nav_mixerAT_inuse); } // switch mixerprofile without reboot @@ -341,7 +277,7 @@ bool outputProfileHotSwitch(int profile_index) // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); return false; } - if (posControl.navState != NAV_STATE_IDLE) + if ((posControl.navState != NAV_STATE_IDLE) && (posControl.navState != NAV_STATE_MIXERAT_IN_PROGRESS)) { // LOG_INFO(PWM, "mixer switch failed, navState != NAV_STATE_IDLE"); return false; diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 22eb4841850..b6016e72864 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -9,6 +9,12 @@ #define MAX_MIXER_PROFILE_COUNT 2 #endif +typedef enum { + MIXERAT_ON_EVENT_OFF, //no request, stats checking only + MIXERAT_ON_EVENT_ON, + MIXERAT_ON_EVENT_ON_FS_ONLY, +} mixerProfileSwitchOnEvent_e; + typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; @@ -17,10 +23,9 @@ typedef struct mixerConfig_s { uint8_t outputMode; bool motorstopOnLow; bool PIDProfileLinking; - bool switchOnFSRTH; - bool switchOnFSLand; - int16_t switchOnFSStabilizationTimer; - int16_t switchOnFSTransitionTimer; + mixerProfileSwitchOnEvent_e switchOnRTH; + mixerProfileSwitchOnEvent_e switchOnLand; + int16_t switchTransitionTimer; } mixerConfig_t; typedef struct mixerProfile_s { mixerConfig_t mixer_config; @@ -29,36 +34,31 @@ typedef struct mixerProfile_s { } mixerProfile_t; PG_DECLARE_ARRAY(mixerProfile_t, MAX_MIXER_PROFILE_COUNT, mixerProfiles); - - typedef enum { MIXERAT_REQUEST_NONE, //no request, stats checking only MIXERAT_REQUEST_RTH, MIXERAT_REQUEST_LAND, MIXERAT_REQUEST_ABORT, -} mixerProfileATRequest_t; +} mixerProfileATRequest_e; //mixerProfile Automated Transition PHASE typedef enum { MIXERAT_PHASE_IDLE, MIXERAT_PHASE_TRANSITION_INITIALIZE, - MIXERAT_PHASE_STAB_AND_CLIMB, MIXERAT_PHASE_TRANSITIONING, MIXERAT_PHASE_DONE, -} mixerProfileATState_t; +} mixerProfileATState_e; typedef struct mixerProfileAT_s { - mixerProfileATState_t phase; + mixerProfileATState_e phase; bool transitionInputMixing; timeMs_t transitionStartTime; timeMs_t transitionStabEndTime; timeMs_t transitionTransEndTime; - bool lastTransitionInputMixing; - bool lastMixerProfile; } mixerProfileAT_t; extern mixerProfileAT_t mixerProfileAT; -bool mixerATRequiresAngleMode(void); -bool mixerATUpdateState(mixerProfileATRequest_t required_action); +bool checkMixerATRequired(mixerProfileATRequest_e required_action); +bool mixerATUpdateState(mixerProfileATRequest_e required_action); extern mixerConfig_t currentMixerConfig; extern int currentMixerProfileIndex; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b0be9743859..63312af46c2 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -945,8 +945,6 @@ static const char * osdFailsafePhaseMessage(void) case FAILSAFE_LANDING: // This should be considered an emergengy landing return OSD_MESSAGE_STR(OSD_MSG_EMERG_LANDING_FS); - case FAILSAFE_MIXER_SWITCHING: - return OSD_MESSAGE_STR(OSD_MSG_MIXER_SWITCHING_FS); case FAILSAFE_RX_LOSS_MONITORING: // Only reachable from FAILSAFE_LANDED, which performs // a disarm. Since aircraft has been disarmed, we no diff --git a/src/main/io/osd.h b/src/main/io/osd.h index ab608caf6f7..2dad5ceb819 100644 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -88,7 +88,6 @@ #define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" -#define OSD_MSG_MIXER_SWITCHING_FS "(FS MIXER SWITCHING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" #define OSD_MSG_STARTING_RTH "STARTING RTH" #define OSD_MSG_RTH_CLIMB "ADJUSTING RTH ALTITUDE" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 72cfc781aa2..e79ddc0dfd3 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -558,8 +558,6 @@ static char * osdFailsafePhaseMessage(void) case FAILSAFE_LANDING: // This should be considered an emergengy landing return OSD_MESSAGE_STR("(EMRGY LANDING)"); - case FAILSAFE_MIXER_SWITCHING: - return OSD_MESSAGE_STR("(MIXER SWITCHING)"); case FAILSAFE_RX_LOSS_MONITORING: // Only reachable from FAILSAFE_LANDED, which performs // a disarm. Since aircraft has been disarmed, we no diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 70b739b5cc8..84a0658a5b4 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -316,6 +316,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -373,7 +374,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -410,7 +410,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, /** CRUISE_HOLD mode ************************************************/ @@ -447,7 +446,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -470,7 +468,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -508,7 +505,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -531,7 +527,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -571,7 +566,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -592,7 +586,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -635,7 +628,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -655,7 +647,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -706,7 +697,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -763,7 +753,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -788,7 +777,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -810,7 +798,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -867,7 +854,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -887,7 +873,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -906,7 +891,6 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_MIXERAT] = NAV_STATE_MIXERAT_INITIALIZE, } }, @@ -976,7 +960,7 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_MIXERAT_INITIALIZE, .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE, .timeoutMs = 0, - .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT, .mapToFlightModes = NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, @@ -991,20 +975,29 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .persistentId = NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS, .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS, .timeoutMs = 10, - .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE_FW | NAV_REQUIRE_THRTILT, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT | NAV_MIXERAT, .mapToFlightModes = NAV_ALTHOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, + } + }, + [NAV_STATE_MIXERAT_ABORT] = { + .persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, + .timeoutMs = 10, + .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, + .mapToFlightModes = NAV_ALTHOLD_MODE, + .mwState = MW_NAV_STATE_NONE, + .mwError = MW_NAV_ERROR_NONE, + .onEvent = { + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + } }, }; @@ -1415,6 +1408,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (checkMixerATRequired(MIXERAT_REQUEST_RTH)){ + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } + if (navConfig()->general.flags.rth_use_linear_descent && navConfig()->general.rth_home_altitude > 0) { // Check linear descent status uint32_t homeDistance = calculateDistanceToDestination(&posControl.rthState.homePosition.pos); @@ -1519,6 +1516,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() || !validateRTHSanityChecker()) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + + if (checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; + } float descentVelLimited = 0; int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z; @@ -1924,35 +1925,70 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_IN_PROGRESS(navi return NAV_FSM_EVENT_NONE; } +navigationFSMState_t navMixerATPendingState = NAV_STATE_IDLE; static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_INITIALIZE(navigationFSMState_t previousState) { - UNUSED(previousState); - resetGCSFlags(); - - // Prepare altitude controller - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); // This will reset surface offset + const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + // Prepare altitude controller if idle, RTH or WP modes active or surface mode status changed + if (!(prevFlags & NAV_CTL_ALT) || (prevFlags & NAV_AUTO_RTH) || (prevFlags & NAV_AUTO_WP)) { + resetAltitudeController(false); + setupAltitudeController(); + } + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + navMixerATPendingState = previousState; return NAV_FSM_EVENT_SUCCESS; } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_IN_PROGRESS(navigationFSMState_t previousState) { UNUSED(previousState); - - // Climb to safe altitude and turn to correct direction - if ( (STATE(MULTIROTOR)) && (mixerProfileAT.phase==MIXERAT_PHASE_STAB_AND_CLIMB)) { - updateClimbRateToAltitudeController(navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); - // fpVector3_t tmp_pos = navGetCurrentActualPositionAndVelocity()->pos; - // tmp_pos.z = tmp_pos.z + navConfig()->general.max_auto_climb_rate; - // setDesiredPosition(&tmp_pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); - } else { - // climb to perform the transition - //set the desired position to the current position to enable althold only - setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + mixerProfileATRequest_e required_action; + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + required_action = MIXERAT_REQUEST_RTH; + break; + case NAV_STATE_RTH_LANDING: + required_action = MIXERAT_REQUEST_LAND; + break; + default: + required_action = MIXERAT_REQUEST_NONE; + break; + } + if (mixerATUpdateState(required_action)){ + // MixerAT is done, switch to next state + resetPositionController(); + resetAltitudeController(false); // Make sure surface tracking is not enabled uses global altitude, not AGL + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + switch (navMixerATPendingState) + { + case NAV_STATE_RTH_HEAD_HOME: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME; + break; + case NAV_STATE_RTH_LANDING: + setupAltitudeController(); + return NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING; + break; + default: + return NAV_FSM_EVENT_SWITCH_TO_IDLE; + break; + } } + + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, posControl.actualState.yaw, NAV_POS_UPDATE_Z); + return NAV_FSM_EVENT_NONE; } +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState) +{ + UNUSED(previousState); + mixerATUpdateState(MIXERAT_REQUEST_ABORT); + return NAV_FSM_EVENT_SUCCESS; +} + static navigationFSMState_t navSetNewFSMState(navigationFSMState_t newState) { navigationFSMState_t previousState; @@ -4366,7 +4402,7 @@ void abortForcedRTH(void) rthState_e getStateOfForcedRTH(void) { /* If forced RTH activated and in AUTO_RTH or EMERG state */ - if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG))) { + if (posControl.flags.forcedRTHActivated && (navGetStateFlags(posControl.navState) & (NAV_AUTO_RTH | NAV_CTL_EMERG | NAV_MIXERAT))) { if (posControl.navState == NAV_STATE_RTH_FINISHED || posControl.navState == NAV_STATE_EMERGENCY_LANDING_FINISHED) { return RTH_HAS_LANDED; } @@ -4415,29 +4451,29 @@ emergLandState_e getStateOfForcedEmergLanding(void) /*----------------------------------------------------------- * Ability to mixer_profile(vtol) switch on external event *-----------------------------------------------------------*/ -void activateMIXERATHelper(void) -{ - posControl.flags.mixerATHelperActivated = true; - navProcessFSMEvents(selectNavEventFromBoxModeInput()); -} - -void abortMIXERATHelper(void) -{ - // Disable emergency landing and make sure we back out of navigation mode to IDLE - // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update - posControl.flags.mixerATHelperActivated = false; - navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); -} - -altHoldState_e getStateOfMIXERATHelper(void) -{ - /* If forced emergency landing activated and in EMERG state */ - if (posControl.flags.mixerATHelperActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { - return ALTHOLD_IN_PROGRESS; - } else { - return ALTHOLD_IDLE; - } -} +// void activateMIXERATHelper(void) +// { +// posControl.flags.mixerATHelperActivated = true; +// navProcessFSMEvents(selectNavEventFromBoxModeInput()); +// } + +// void abortMIXERATHelper(void) +// { +// // Disable emergency landing and make sure we back out of navigation mode to IDLE +// // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update +// posControl.flags.mixerATHelperActivated = false; +// navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); +// } + +// altHoldState_e getStateOfMIXERATHelper(void) +// { +// /* If forced emergency landing activated and in EMERG state */ +// if (posControl.flags.mixerATHelperActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { +// return ALTHOLD_IN_PROGRESS; +// } else { +// return ALTHOLD_IDLE; +// } +// } bool isWaypointMissionRTHActive(void) { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index a9b23e9b32a..b90cb3e5219 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -588,9 +588,9 @@ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); emergLandState_e getStateOfForcedEmergLanding(void); -void activateMIXERATHelper(void); -void abortMIXERATHelper(void); -altHoldState_e getStateOfMIXERATHelper(void); +// void activateMIXERATHelper(void); +// void abortMIXERATHelper(void); +// altHoldState_e getStateOfMIXERATHelper(void); /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 78ecef6f1f9..1b1506d0cdb 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -164,6 +164,7 @@ typedef enum { NAV_FSM_EVENT_STATE_SPECIFIC_4, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_5, // State-specific event NAV_FSM_EVENT_STATE_SPECIFIC_6, // State-specific event + NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND = NAV_FSM_EVENT_STATE_SPECIFIC_1, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED = NAV_FSM_EVENT_STATE_SPECIFIC_2, @@ -230,7 +231,7 @@ typedef enum { NAV_PERSISTENT_ID_MIXERAT_INITIALIZE = 39, NAV_PERSISTENT_ID_MIXERAT_IN_PROGRESS = 40, - + NAV_PERSISTENT_ID_MIXERAT_ABORT = 41, } navigationPersistentId_e; typedef enum { @@ -280,6 +281,7 @@ typedef enum { NAV_STATE_MIXERAT_INITIALIZE, NAV_STATE_MIXERAT_IN_PROGRESS, + NAV_STATE_MIXERAT_ABORT, NAV_STATE_COUNT, } navigationFSMState_t; @@ -310,6 +312,8 @@ typedef enum { /* Additional flags */ NAV_CTL_LAND = (1 << 14), NAV_AUTO_WP_DONE = (1 << 15), //Waypoint mission reached the last waypoint and is idling + + NAV_MIXERAT = (1 << 16), //MIXERAT in progress } navigationFSMStateFlags_t; typedef struct { From 271c7988c8cfa1fbd07ba6c94f07a52db73a84e5 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 2 Aug 2023 02:38:44 +0900 Subject: [PATCH 34/60] minor fix --- src/main/flight/mixer_profile.c | 6 +++--- src/main/navigation/navigation.c | 19 ++++++++++--------- src/main/navigation/navigation.h | 4 ---- src/main/navigation/navigation_private.h | 1 - 4 files changed, 13 insertions(+), 17 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index f2b6708727f..90cdfcca166 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -112,11 +112,11 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) //on non vtol setups , behave as normal if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) { - return true; + return false; } if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) { - return true; + return false; } if ((required_action == MIXERAT_REQUEST_RTH) && (currentMixerConfig.switchOnRTH!=MIXERAT_ON_EVENT_OFF) && STATE(MULTIROTOR)) @@ -246,7 +246,7 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) } isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || nav_mixerAT_inuse); + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || nav_mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); } // switch mixerprofile without reboot diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 84a0658a5b4..c63504c8072 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -68,6 +68,7 @@ #define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt) #define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend) #define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set +#define MR_RTH_LAND_MARGIN_CM 2000 // pause landing if this amount of cm *before* remaining to the home point (2D distance) // Planes: #define FW_RTH_CLIMB_OVERSHOOT_CM 100 @@ -1408,7 +1409,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } - if (checkMixerATRequired(MIXERAT_REQUEST_RTH)){ + if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){ return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; } @@ -1522,10 +1523,16 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF } float descentVelLimited = 0; - int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z; + fpVector3_t tmpHomePos = posControl.rthState.homeTmpWaypoint; + uint32_t remaning_distance = calculateDistanceToDestination(&tmpHomePos); + + int32_t landingElevation = posControl.rthState.homeTmpWaypoint.z; + if(STATE(MULTIROTOR) && (remaning_distance>MR_RTH_LAND_MARGIN_CM)){ + descentVelLimited = navConfig()->general.land_minalt_vspd; + } // A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed - if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { + else if ((posControl.flags.estAglStatus == EST_TRUSTED) && posControl.actualState.agl.pos.z < 50.0f) { // land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown // Do not allow descent velocity slower than -30cm/s so the landing detector works. descentVelLimited = navConfig()->general.land_minalt_vspd; @@ -3865,11 +3872,6 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) } } - // Altitude hold for vtol transition (can override MANUAL) - if (posControl.flags.mixerATHelperActivated && canActivateAltHold) { - return NAV_FSM_EVENT_SWITCH_TO_MIXERAT; - } - // Failsafe_RTH (can override MANUAL) if (posControl.flags.forcedRTHActivated) { // If we request forced RTH - attempt to activate it no matter what @@ -4323,7 +4325,6 @@ void navigationInit(void) posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; - posControl.flags.mixerATHelperActivated = false; posControl.waypointCount = 0; posControl.activeWaypointIndex = 0; posControl.waypointListValid = false; diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index b90cb3e5219..cf4289e14a1 100644 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -588,10 +588,6 @@ void activateForcedEmergLanding(void); void abortForcedEmergLanding(void); emergLandState_e getStateOfForcedEmergLanding(void); -// void activateMIXERATHelper(void); -// void abortMIXERATHelper(void); -// altHoldState_e getStateOfMIXERATHelper(void); - /* Getter functions which return data about the state of the navigation system */ bool navigationInAutomaticThrottleMode(void); bool navigationIsControllingThrottle(void); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 1b1506d0cdb..073fc59c99a 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -103,7 +103,6 @@ typedef struct navigationFlags_s { // Failsafe actions bool forcedRTHActivated; bool forcedEmergLandingActivated; - bool mixerATHelperActivated; /* Landing detector */ bool resetLandingDetector; From 1bdca4e02c09fff1a30ebc43fdaa429eb1486260 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 2 Aug 2023 12:08:35 +0900 Subject: [PATCH 35/60] some clean up for mixer profile --- src/main/drivers/pwm_mapping.c | 4 ---- src/main/fc/settings.h | 3 --- src/main/flight/mixer.c | 7 +------ src/main/flight/mixer_profile.c | 12 ++---------- src/main/target/MATEKF405/CMakeLists.txt | 1 - src/main/target/MATEKF405/target.c | 16 ---------------- src/main/target/MATEKF405/target.h | 4 ---- 7 files changed, 3 insertions(+), 44 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index 653404a4e28..a40faf965cc 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -266,10 +266,6 @@ void pwmBuildTimerOutputList(timMotorServoHardware_t * timOutputs, bool isMixerU else if (timHw->usageFlags & TIM_USE_MC_MOTOR) { type = MAP_TO_MOTOR_OUTPUT; } - else if (timHw->usageFlags & TIM_USE_MC_SERVO){ - type = MAP_TO_SERVO_OUTPUT; - } - } else { // Fixed wing or HELI (one/two motors and a lot of servos if (timHw->usageFlags & TIM_USE_FW_SERVO) { diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index ae7da853172..7fdfd66bf25 100644 --- a/src/main/fc/settings.h +++ b/src/main/fc/settings.h @@ -43,9 +43,6 @@ typedef enum { MODE_LOOKUP = (1 << SETTING_MODE_OFFSET), // 0x40 } setting_mode_e; -// #define SETTING_TYPE_MASK (0x0F) -// #define SETTING_SECTION_MASK (0x30) -// #define SETTING_MODE_MASK (0xC0) #define SETTING_TYPE_MASK (0x07) #define SETTING_SECTION_MASK (0x38) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 9ba9a764417..e3d255e491d 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -446,14 +446,9 @@ void writeAllMotors(int16_t mc) writeMotors(); } - -void stopMotorsNoDelay(void) -{ - writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand); -} void stopMotors(void) { - stopMotorsNoDelay(); + writeAllMotors(feature(FEATURE_REVERSIBLE_MOTORS) ? reversibleMotorsConfig()->neutral : motorConfig()->mincommand); delay(50); // give the timers and ESCs a chance to react. } diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 90cdfcca166..e264012e8c7 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -107,8 +107,7 @@ void setMixerProfileAT(void) bool checkMixerATRequired(mixerProfileATRequest_e required_action) { - //return true if mixerAT condition is met or setting is not valid - //set mixer profile automated transition according to failsafe phase + //return false if mixerAT condition is not required or setting is not valid //on non vtol setups , behave as normal if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) { @@ -217,13 +216,6 @@ bool checkMixerProfileHotSwitchAvalibility(void) allow_hot_switch = 0; return false; } - // not necessary when map motor/servos of all mixer profiles on the first boot - // 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; } @@ -287,7 +279,7 @@ bool outputProfileHotSwitch(int profile_index) // LOG_INFO(PWM, "mixer switch failed to set config"); return false; } - stopMotorsNoDelay(); // not necessary, but just in case something goes wrong. But will a short period of stop command cause problem? + // stopMotorsNoDelay(); // not necessary, but just in case something goes wrong. But will a short period of stop command cause problem? mixerConfigInit(); return true; } diff --git a/src/main/target/MATEKF405/CMakeLists.txt b/src/main/target/MATEKF405/CMakeLists.txt index 36d193a945b..c4f839c1c1d 100644 --- a/src/main/target/MATEKF405/CMakeLists.txt +++ b/src/main/target/MATEKF405/CMakeLists.txt @@ -1,4 +1,3 @@ target_stm32f405xg(MATEKF405) target_stm32f405xg(MATEKF405_SERVOS6) target_stm32f405xg(MATEKF405OSD) -target_stm32f405xg(MATEKF405VTOL) diff --git a/src/main/target/MATEKF405/target.c b/src/main/target/MATEKF405/target.c index 3f4d7618340..40ebf626e5f 100644 --- a/src/main/target/MATEKF405/target.c +++ b/src/main/target/MATEKF405/target.c @@ -23,21 +23,6 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM -#ifdef MATEKF405VTOL//development purpose, TODO: remove it in the release - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 - DEF_TIM(TIM8, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S2 UP(2,1) - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 1), // S3 UP(2,1) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S4 UP(2,1) - - DEF_TIM(TIM2, CH1, PA15, TIM_USE_LED , 0, 0), // S5 UP(1,7) - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S6 UP(2,5) - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,7)!S5 UP(2,6) - - DEF_TIM(TIM5, CH3, PA2, TIM_USE_PWM, 0, 0), // TX2 - - DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // TX4 UP(1,6) D(1,2)!S1 - DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // RX4 UP(1,6) D(1,4) -#else #ifdef MATEKF405_SERVOS6 DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 @@ -61,7 +46,6 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH1, PA0, TIM_USE_PWM, 0, 0), // TX4 UP(1,6) D(1,2)!S1 DEF_TIM(TIM5, CH2, PA1, TIM_USE_PWM, 0, 0), // RX4 UP(1,6) D(1,4) -#endif }; const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index e9aff02df4b..fa2af63384c 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -195,7 +195,3 @@ #define USE_ESC_SENSOR #define MAX_PWM_OUTPUT_PORTS 6 - -#ifdef MATEKF405VTOL -#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP -#endif \ No newline at end of file From c9ff9cd099a88570ee9fda857982092deb47e281 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 2 Aug 2023 18:00:52 +0900 Subject: [PATCH 36/60] updates --- src/main/fc/settings.yaml | 13 ++++--------- src/main/flight/mixer_profile.c | 22 +++++++--------------- src/main/flight/mixer_profile.h | 10 ++-------- src/main/navigation/navigation.c | 16 ++++++++++++---- 4 files changed, 25 insertions(+), 36 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bf6aeb40cb5..56f2a7027f4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -190,9 +190,6 @@ tables: - name: nav_fw_wp_turn_smoothing values: ["OFF", "ON", "ON-CUT"] enum: wpFwTurnSmoothing_e - - name: mixer_switch_on_event - values: ["OFF", "ON", "ON_FS_ONLY"] - enum: mixerProfileSwitchOnEvent_e constants: RPYL_PID_MIN: 0 @@ -1178,16 +1175,14 @@ groups: type: bool - name: mixer_switch_on_rth description: "If set to on, mixer_profile will switch when it is heading home" - default_value: "OFF" + default_value: OFF field: mixer_config.switchOnRTH - table: mixer_switch_on_event - type: uint8_t + type: bool - name: mixer_switch_on_land description: "If set to on, mixer_profile will switch when Landing" - default_value: "OFF" + default_value: OFF field: mixer_config.switchOnLand - table: mixer_switch_on_event - type: uint8_t + type: bool - name: mixer_switch_trans_timer description: "If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch" default_value: 0 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index e264012e8c7..044cb027abd 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -93,7 +93,7 @@ void mixerConfigInit(void) setConfigProfile(getConfigMixerProfile()); pidInit(); pidInitFilters(); - pidResetErrorAccumulators(); + // pidResetErrorAccumulators(); schedulePidGainsUpdate(); navigationUsePIDs(); // set navigation pid gains } @@ -118,24 +118,16 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) return false; } - if ((required_action == MIXERAT_REQUEST_RTH) && (currentMixerConfig.switchOnRTH!=MIXERAT_ON_EVENT_OFF) && STATE(MULTIROTOR)) + if ((required_action == MIXERAT_REQUEST_RTH) && (currentMixerConfig.switchOnRTH) && STATE(MULTIROTOR)) { - if ((currentMixerConfig.switchOnRTH==MIXERAT_ON_EVENT_ON_FS_ONLY) && (!FLIGHT_MODE(FAILSAFE_MODE))) - { - return false; - } - //check next mixer_profile setting is valid - return mixerConfigByIndex(nextProfileIndex)->switchOnRTH == MIXERAT_ON_EVENT_OFF ? true:false; + //check next mixer_profile setting is valid, need to be false + return mixerConfigByIndex(nextProfileIndex)->switchOnRTH? false:true; } - else if ((required_action == MIXERAT_REQUEST_LAND) && (currentMixerConfig.switchOnLand!=MIXERAT_ON_EVENT_OFF) && STATE(AIRPLANE)) + else if ((required_action == MIXERAT_REQUEST_LAND) && (currentMixerConfig.switchOnLand) && STATE(AIRPLANE)) { - if ((currentMixerConfig.switchOnLand==MIXERAT_ON_EVENT_ON_FS_ONLY) && (!FLIGHT_MODE(FAILSAFE_MODE))) - { - return false; - } - //check next mixer_profile setting is valid - return mixerConfigByIndex(nextProfileIndex)->switchOnLand == MIXERAT_ON_EVENT_OFF ? true:false; + //check next mixer_profile setting is valid, need to be false + return mixerConfigByIndex(nextProfileIndex)->switchOnLand? false:true; } return false; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index b6016e72864..89d85ae7ab0 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -9,12 +9,6 @@ #define MAX_MIXER_PROFILE_COUNT 2 #endif -typedef enum { - MIXERAT_ON_EVENT_OFF, //no request, stats checking only - MIXERAT_ON_EVENT_ON, - MIXERAT_ON_EVENT_ON_FS_ONLY, -} mixerProfileSwitchOnEvent_e; - typedef struct mixerConfig_s { int8_t motorDirectionInverted; uint8_t platformType; @@ -23,8 +17,8 @@ typedef struct mixerConfig_s { uint8_t outputMode; bool motorstopOnLow; bool PIDProfileLinking; - mixerProfileSwitchOnEvent_e switchOnRTH; - mixerProfileSwitchOnEvent_e switchOnLand; + bool switchOnRTH; + bool switchOnLand; int16_t switchTransitionTimer; } mixerConfig_t; typedef struct mixerProfile_s { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index c63504c8072..1bed5e11adb 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -982,14 +982,22 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwError = MW_NAV_ERROR_NONE, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_MIXERAT_ABORT, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_MIXERAT_ABORT, } }, [NAV_STATE_MIXERAT_ABORT] = { .persistentId = NAV_PERSISTENT_ID_MIXERAT_ABORT, - .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, + .onEntry = navOnEnteringState_NAV_STATE_MIXERAT_ABORT, //will not switch to its pending state .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_REQUIRE_THRTILT, .mapToFlightModes = NAV_ALTHOLD_MODE, From 1188ff79fe762a5ee1d3d63e9773b5cacd1a7d02 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 2 Aug 2023 19:49:02 +0900 Subject: [PATCH 37/60] fixes --- docs/Settings.md | 30 ++++++++++++++++++++++++++++++ src/main/flight/mixer.c | 2 +- src/main/flight/mixer_profile.h | 4 ++-- src/main/flight/servos.c | 2 +- src/main/navigation/navigation.c | 10 +--------- 5 files changed, 35 insertions(+), 13 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index cce1b160066..afa1329b646 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2552,6 +2552,36 @@ If enabled, pid profile index will follow mixer profile index --- +### mixer_switch_on_land + +If set to on, mixer_profile will switch when Landing + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_switch_on_rth + +If set to on, mixer_profile will switch when it is heading home + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + +### mixer_switch_trans_timer + +If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch + +| Default | Min | Max | +| --- | --- | --- | +| 0 | 0 | 200 | + +--- + ### mode_range_logic_operator Control how Mode selection works in flight modes. If you example have Angle mode configured on two different Aux channels, this controls if you need both activated ( AND ) or if you only need one activated ( OR ) to active angle mode. diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index e3d255e491d..ab8a701bc74 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -113,7 +113,7 @@ static void computeMotorCount(void) for (int i = 0; i < MAX_SUPPORTED_MOTORS; i++) { bool isMotorUsed = false; for(int j = 0; j< MAX_MIXER_PROFILE_COUNT; j++){ - if (mixerMotorMixersByIndex(j)[i]->throttle != 0.0f) { + if (mixerMotorMixersByIndex(j)[i].throttle != 0.0f) { isMotorUsed = true; } } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index 89d85ae7ab0..cda32f57a2c 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -70,8 +70,8 @@ static inline const mixerProfile_t* mixerProfiles_CopyArray_by_index(int _index) #define customServoMixers_CopyArray() (mixerProfiles_CopyArray_by_index(systemConfig()->current_mixer_profile_index)->ServoMixers) #define mixerConfigByIndex(index) (&(mixerProfiles(index)->mixer_config)) -#define mixerMotorMixersByIndex(index) (&(mixerProfiles(index)->MotorMixers)) -#define mixerServoMixersByIndex(index) (&(mixerProfiles(index)->ServoMixers)) +#define mixerMotorMixersByIndex(index) (mixerProfiles(index)->MotorMixers) +#define mixerServoMixersByIndex(index) (mixerProfiles(index)->ServoMixers) bool outputProfileHotSwitch(int profile_index); bool checkMixerProfileHotSwitchAvalibility(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index e3105456703..1ccaefd4627 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -199,7 +199,7 @@ void loadCustomServoMixer(void) memset(currentServoMixer, 0, sizeof(currentServoMixer)); for (int j = 0; j < MAX_MIXER_PROFILE_COUNT; j++) { - const servoMixer_t* tmp_customServoMixers = mixerServoMixersByIndex(j)[0]; + const servoMixer_t* tmp_customServoMixers = &mixerServoMixersByIndex(j)[0]; // load custom mixer into currentServoMixer for (int i = 0; i < MAX_SERVO_RULES; i++) { // check if done diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 1bed5e11adb..de41be47672 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -982,17 +982,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwError = MW_NAV_ERROR_NONE, .onEvent = { [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_MIXERAT_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, [NAV_FSM_EVENT_SWITCH_TO_RTH_HEAD_HOME] = NAV_STATE_RTH_HEAD_HOME, //switch to its pending state [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_LANDING, //switch to its pending state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_MIXERAT_ABORT, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_MIXERAT_ABORT, } }, [NAV_STATE_MIXERAT_ABORT] = { From 258a88ce154a4b0670e644f2953283c9152387f3 Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 3 Aug 2023 20:31:34 +0900 Subject: [PATCH 38/60] no loiter waypoint for vtol --- src/main/flight/servos.c | 23 +--------------------- src/main/navigation/navigation_fixedwing.c | 5 +++++ 2 files changed, 6 insertions(+), 22 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 1ccaefd4627..4d4bb497d19 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -70,27 +70,10 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT ); -// PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); - -// void pgResetFn_customServoMixers(servoMixer_t *instance) -// { -// for (int i = 0; i < MAX_SERVO_RULES; i++) { -// RESET_CONFIG(servoMixer_t, &instance[i], -// .targetChannel = 0, -// .inputSource = 0, -// .rate = 0, -// .speed = 0 -// #ifdef USE_PROGRAMMING_FRAMEWORK -// ,.conditionId = -1 -// #endif -// ); -// } -// } void Reset_servoMixers(servoMixer_t *instance) { - for (int i = 0; i < MAX_SERVO_RULES; i++) - { + for (int i = 0; i < MAX_SERVO_RULES; i++){ RESET_CONFIG(servoMixer_t, &instance[i], .targetChannel = 0, .inputSource = 0, @@ -171,10 +154,6 @@ void servosInit(void) servoOutputEnabled = true; mixerUsesServos = true; } - else { - servoOutputEnabled = false; - mixerUsesServos = false; - } for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) { servoComputeScalingFactors(i); diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index b82b4c58013..95ee2ca7ddf 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -39,6 +39,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "fc/config.h" #include "fc/controlrate_profile.h" @@ -289,6 +290,10 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) needToCalculateCircularLoiter = isNavHoldPositionActive() && (distanceToActualTarget <= (navLoiterRadius / TAN_15DEG)) && (distanceToActualTarget > 50.0f); + //if vtol landing is required, fly straight to homepoint + if ((posControl.navState == NAV_STATE_RTH_HEAD_HOME) && navigationRTHAllowsLanding() && checkMixerATRequired(MIXERAT_REQUEST_LAND)){ + needToCalculateCircularLoiter = false; + } /* WP turn smoothing with 2 options, 1: pass through WP, 2: cut inside turn missing WP * Works for turns > 30 degs and < 160 degs. From f96fd6402aa06a1c9bf933e6cc4b0cb94d546b14 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 4 Aug 2023 19:14:37 +0900 Subject: [PATCH 39/60] refactoring on pid i term --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 13a7c45d788..fc7fa132fe3 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -799,7 +799,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD)); return itermErrorRate * itermRelaxFactor; } } @@ -1037,7 +1037,7 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); + shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; @@ -1135,8 +1135,8 @@ void FAST_CODE pidController(float dT) pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } - // Prevent strong Iterm accumulation during stick inputs - antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); + // Prevent Iterm accumulation when motors are near its saturation + antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) / motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits From 75bd05a089bc463985b99f2ec17b49ec04c5f512 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 5 Aug 2023 14:09:31 +0900 Subject: [PATCH 40/60] refactoring on pid iterm --- src/main/fc/settings.yaml | 14 +++++----- src/main/flight/pid.c | 57 +++++++++++++++++---------------------- src/main/flight/pid.h | 6 +---- 3 files changed, 33 insertions(+), 44 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 56f2a7027f4..c50860fd573 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1851,12 +1851,6 @@ groups: default_value: 0 min: 0 max: 200 - - name: fw_iterm_throw_limit - description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." - default_value: 165 - field: fixedWingItermThrowLimit - min: FW_ITERM_THROW_LIMIT_MIN - max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." default_value: 1500 @@ -1900,11 +1894,17 @@ groups: min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup - description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)" + description: "Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)" default_value: 50 field: itermWindupPointPercent min: 0 max: 90 + - name: pid_iterm_limit_percent + description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely." + default_value: 33 + field: pidItermLimitPercent + min: 0 + max: 200 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fc7fa132fe3..63d976267c8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -79,7 +79,6 @@ typedef struct { // Rate integrator float errorGyroIf; - float errorGyroIfLimit; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -101,7 +100,6 @@ typedef struct { #endif uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; - bool itermLimitActive; bool itermFreezeActive; pt3Filter_t rateTargetFilter; @@ -264,8 +262,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, + .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT, - .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, @@ -380,14 +378,12 @@ void pidResetErrorAccumulators(void) // Reset R/P/Y integrator for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; - pidState[axis].errorGyroIfLimit = 0.0f; } } void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) { pidState[axis].errorGyroIf -= delta; - pidState[axis].errorGyroIfLimit -= delta; } float getTotalRateTarget(void) @@ -622,7 +618,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); if (!angleErrorDeg) { pidState->errorGyroIf = 0.0f; - pidState->errorGyroIfLimit = 0.0f; + // pidState->errorGyroIfLimit = 0.0f; } } @@ -728,14 +724,6 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d return(newDTerm); } -static void applyItermLimiting(pidState_t *pidState) { - if (pidState->itermLimitActive) { - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); - } else - { - pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); - } -} static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { UNUSED(pidState); @@ -759,10 +747,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } - applyItermLimiting(pidState); - - if (pidProfile()->fixedWingItermThrowLimit != 0) { - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); } axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -834,11 +821,19 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid itermErrorRate *= iTermAntigravityGain; #endif - pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) - + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + if (!pidState->itermFreezeActive) { + pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + } + + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + // Don't grow I-term if motors are at their limit - applyItermLimiting(pidState); + // applyItermLimiting(pidState); //merged with itermFreezeActive axisPID[axis] = newOutputLimited; @@ -1030,9 +1025,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void checkItermLimitingActive(pidState_t *pidState) +bool checkItermLimitingActive(pidState_t *pidState) { - bool shouldActivate; + bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else @@ -1040,25 +1035,24 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } - pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; + return STATE(ANTI_WINDUP) || shouldActivate; } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { + bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ - pidState->itermFreezeActive = true; + shouldActivate |= true; } else { - pidState->itermFreezeActive = false; + shouldActivate |= false; } - } else - { - pidState->itermFreezeActive = false; } - + shouldActivate |=checkItermLimitingActive(pidState); + pidState->itermFreezeActive = shouldActivate; } void FAST_CODE pidController(float dT) @@ -1136,14 +1130,13 @@ void FAST_CODE pidController(float dT) } // Prevent Iterm accumulation when motors are near its saturation - antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) / motorItermWindupPoint, 0.0f, 1.0f); + antiWindupScaler = constrainf((1.0f - MAX(2*motorItermWindupPoint,1) * getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control - checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); pidControllerApplyFn(&pidState[axis], axis, dT); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e7d3981a07b..8e2931c539c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -31,10 +31,6 @@ #define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90 -#define FW_ITERM_THROW_LIMIT_DEFAULT 165 -#define FW_ITERM_THROW_LIMIT_MIN 0 -#define FW_ITERM_THROW_LIMIT_MAX 500 - #define AXIS_ACCEL_MIN_LIMIT 50 #define HEADING_HOLD_ERROR_LPF_FREQ 2 @@ -121,9 +117,9 @@ typedef struct pidProfile_s { uint16_t pidSumLimit; uint16_t pidSumLimitYaw; + uint16_t pidItermLimitPercent; // Airplane-specific parameters - uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. From 461497eff687da4fee324bbd0d9e30bf25d888e8 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 5 Aug 2023 14:38:23 +0900 Subject: [PATCH 41/60] refactoring on pid motorItermWindupPoint --- src/main/flight/pid.c | 13 ++++++++----- 1 file changed, 8 insertions(+), 5 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 20333071b0b..9126bb5dcb4 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -148,6 +148,7 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration; static EXTENDED_FASTRAM uint8_t yawLpfHz; static EXTENDED_FASTRAM float motorItermWindupPoint; +static EXTENDED_FASTRAM float motorItermWindupPoint_inv; static EXTENDED_FASTRAM float antiWindupScaler; #ifdef USE_ANTIGRAVITY static EXTENDED_FASTRAM float iTermAntigravityGain; @@ -799,7 +800,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD)); return itermErrorRate * itermRelaxFactor; } } @@ -1037,7 +1038,7 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); + shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; @@ -1138,8 +1139,8 @@ void FAST_CODE pidController(float dT) pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } - // Prevent strong Iterm accumulation during stick inputs - antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f); + // Prevent Iterm accumulation when motors are near its saturation + antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) * motorItermWindupPoint_inv, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits @@ -1181,7 +1182,9 @@ void pidInit(void) itermRelax = pidProfile()->iterm_relax; yawLpfHz = pidProfile()->yaw_lpf_hz; - motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f)); + motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); + motorItermWindupPoint_inv = 1.0f / motorItermWindupPoint; + #ifdef USE_D_BOOST dBoostMin = pidProfile()->dBoostMin; From 2f75a764d96719cf29c168f0493631286143c753 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 5 Aug 2023 14:09:31 +0900 Subject: [PATCH 42/60] refactoring on pid iterm --- src/main/fc/settings.yaml | 14 +++++----- src/main/flight/pid.c | 55 +++++++++++++++++---------------------- src/main/flight/pid.h | 6 +---- 3 files changed, 32 insertions(+), 43 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a0904af6f32..11a37c1ba6b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1821,12 +1821,6 @@ groups: default_value: 0 min: 0 max: 200 - - name: fw_iterm_throw_limit - description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." - default_value: 165 - field: fixedWingItermThrowLimit - min: FW_ITERM_THROW_LIMIT_MIN - max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." default_value: 1500 @@ -1870,11 +1864,17 @@ groups: min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup - description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)" + description: "Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)" default_value: 50 field: itermWindupPointPercent min: 0 max: 90 + - name: pid_iterm_limit_percent + description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely." + default_value: 33 + field: pidItermLimitPercent + min: 0 + max: 200 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 9126bb5dcb4..969b2417eb8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -78,7 +78,6 @@ typedef struct { // Rate integrator float errorGyroIf; - float errorGyroIfLimit; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -100,7 +99,6 @@ typedef struct { #endif uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; - bool itermLimitActive; bool itermFreezeActive; pt3Filter_t rateTargetFilter; @@ -264,8 +262,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, + .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT, - .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, @@ -380,14 +378,12 @@ void pidResetErrorAccumulators(void) // Reset R/P/Y integrator for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; - pidState[axis].errorGyroIfLimit = 0.0f; } } void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) { pidState[axis].errorGyroIf -= delta; - pidState[axis].errorGyroIfLimit -= delta; } float getTotalRateTarget(void) @@ -622,7 +618,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); if (!angleErrorDeg) { pidState->errorGyroIf = 0.0f; - pidState->errorGyroIfLimit = 0.0f; + // pidState->errorGyroIfLimit = 0.0f; } } @@ -728,14 +724,6 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d return(newDTerm); } -static void applyItermLimiting(pidState_t *pidState) { - if (pidState->itermLimitActive) { - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); - } else - { - pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); - } -} static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { UNUSED(pidState); @@ -760,10 +748,9 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } - applyItermLimiting(pidState); - - if (pidProfile()->fixedWingItermThrowLimit != 0) { - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); } axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -835,11 +822,19 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid itermErrorRate *= iTermAntigravityGain; #endif - pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) - + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + if (!pidState->itermFreezeActive) { + pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + } + + if (pidProfile()->pidItermLimitPercent != 0){ + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + } + // Don't grow I-term if motors are at their limit - applyItermLimiting(pidState); + // applyItermLimiting(pidState); //merged with itermFreezeActive axisPID[axis] = newOutputLimited; @@ -1031,9 +1026,9 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -void checkItermLimitingActive(pidState_t *pidState) +bool checkItermLimitingActive(pidState_t *pidState) { - bool shouldActivate; + bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else @@ -1041,25 +1036,24 @@ void checkItermLimitingActive(pidState_t *pidState) shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent } - pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; + return STATE(ANTI_WINDUP) || shouldActivate; } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { + bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ - pidState->itermFreezeActive = true; + shouldActivate |= true; } else { - pidState->itermFreezeActive = false; + shouldActivate |= false; } - } else - { - pidState->itermFreezeActive = false; } - + shouldActivate |=checkItermLimitingActive(pidState); + pidState->itermFreezeActive = shouldActivate; } void FAST_CODE pidController(float dT) @@ -1147,7 +1141,6 @@ void FAST_CODE pidController(float dT) pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control - checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index de0e3126b7c..7c61b7cec21 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -31,10 +31,6 @@ #define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90 -#define FW_ITERM_THROW_LIMIT_DEFAULT 165 -#define FW_ITERM_THROW_LIMIT_MIN 0 -#define FW_ITERM_THROW_LIMIT_MAX 500 - #define AXIS_ACCEL_MIN_LIMIT 50 #define HEADING_HOLD_ERROR_LPF_FREQ 2 @@ -121,9 +117,9 @@ typedef struct pidProfile_s { uint16_t pidSumLimit; uint16_t pidSumLimitYaw; + uint16_t pidItermLimitPercent; // Airplane-specific parameters - uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. From eac42ce6dfe0ee532fdd37b05cf76e35e9da233c Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 5 Aug 2023 14:43:07 +0900 Subject: [PATCH 43/60] update docs --- docs/Settings.md | 22 +++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index dda075cee64..413480886e3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1222,16 +1222,6 @@ Iterm is not allowed to grow when stick position is above threshold. This solves --- -### fw_iterm_throw_limit - -Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. - -| Default | Min | Max | -| --- | --- | --- | -| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | - ---- - ### fw_level_pitch_gain I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations @@ -1914,7 +1904,7 @@ Calculated 1G of Acc axis Z to use in INS ### iterm_windup -Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) +Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached) | Default | Min | Max | | --- | --- | --- | @@ -4732,6 +4722,16 @@ Output function assignment mode. AUTO assigns outputs according to the default m --- +### pid_iterm_limit_percent + +Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely. + +| Default | Min | Max | +| --- | --- | --- | +| 33 | 0 | 200 | + +--- + ### pid_type Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` From 57bba67f86306c48bd10513fd3201e8ff9a5006e Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 5 Aug 2023 15:11:16 +0900 Subject: [PATCH 44/60] minor fix --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 969b2417eb8..76784a14608 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -749,7 +749,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh } if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); } @@ -828,7 +828,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid } if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; + float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); } From ecad9fe7494200d69056a9a9f8f593f5dd51a2d0 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 7 Aug 2023 22:32:23 +0900 Subject: [PATCH 45/60] pid windup backcalc fix --- src/main/common/maths.h | 1 + src/main/flight/pid.c | 7 ++++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index cc3d1bc5176..fc8dbe33295 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -91,6 +91,7 @@ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) +#define SIGN(a) ((a >= 0) ? 1 : -1) #define _ABS_II(x, var) \ ( __extension__ ({ \ diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 76784a14608..639b1e838c9 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -816,6 +816,11 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); + float backCalc = newOutputLimited - newOutput;//back-calculation anti-windup + if (SIGN(backCalc) == SIGN(pidState->errorGyroIf)) { + //back calculation anti-windup can only shrink integrator, will not push it to the opposite direction + backCalc = 0.0f; + } float itermErrorRate = applyItermRelax(axis, rateTarget, rateError); #ifdef USE_ANTIGRAVITY @@ -824,7 +829,7 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid if (!pidState->itermFreezeActive) { pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) - + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); + + (backCalc * pidState->kT * antiWindupScaler * dT); } if (pidProfile()->pidItermLimitPercent != 0){ From 5008f1b7a63c8204fbe31ab322542ac06521c361 Mon Sep 17 00:00:00 2001 From: shota Date: Mon, 7 Aug 2023 22:52:49 +0900 Subject: [PATCH 46/60] revert commit on pid --- src/main/fc/settings.yaml | 14 ++++----- src/main/flight/pid.c | 63 ++++++++++++++++++++++----------------- src/main/flight/pid.h | 6 +++- 3 files changed, 47 insertions(+), 36 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c50860fd573..56f2a7027f4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1851,6 +1851,12 @@ groups: default_value: 0 min: 0 max: 200 + - name: fw_iterm_throw_limit + description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." + default_value: 165 + field: fixedWingItermThrowLimit + min: FW_ITERM_THROW_LIMIT_MIN + max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." default_value: 1500 @@ -1894,17 +1900,11 @@ groups: min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup - description: "Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)" + description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)" default_value: 50 field: itermWindupPointPercent min: 0 max: 90 - - name: pid_iterm_limit_percent - description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely." - default_value: 33 - field: pidItermLimitPercent - min: 0 - max: 200 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 63d976267c8..13a7c45d788 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -79,6 +79,7 @@ typedef struct { // Rate integrator float errorGyroIf; + float errorGyroIfLimit; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -100,6 +101,7 @@ typedef struct { #endif uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; + bool itermLimitActive; bool itermFreezeActive; pt3Filter_t rateTargetFilter; @@ -262,8 +264,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, - .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT, + .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, @@ -378,12 +380,14 @@ void pidResetErrorAccumulators(void) // Reset R/P/Y integrator for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; + pidState[axis].errorGyroIfLimit = 0.0f; } } void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) { pidState[axis].errorGyroIf -= delta; + pidState[axis].errorGyroIfLimit -= delta; } float getTotalRateTarget(void) @@ -618,7 +622,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); if (!angleErrorDeg) { pidState->errorGyroIf = 0.0f; - // pidState->errorGyroIfLimit = 0.0f; + pidState->errorGyroIfLimit = 0.0f; } } @@ -724,6 +728,14 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d return(newDTerm); } +static void applyItermLimiting(pidState_t *pidState) { + if (pidState->itermLimitActive) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); + } else + { + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); + } +} static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT) { UNUSED(pidState); @@ -747,9 +759,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } - if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + applyItermLimiting(pidState); + + if (pidProfile()->fixedWingItermThrowLimit != 0) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -786,7 +799,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD)); + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); return itermErrorRate * itermRelaxFactor; } } @@ -821,19 +834,11 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid itermErrorRate *= iTermAntigravityGain; #endif - if (!pidState->itermFreezeActive) { - pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) - + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); - } - - if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent / 100.0f; - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); - } - + pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); // Don't grow I-term if motors are at their limit - // applyItermLimiting(pidState); //merged with itermFreezeActive + applyItermLimiting(pidState); axisPID[axis] = newOutputLimited; @@ -1025,34 +1030,35 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -bool checkItermLimitingActive(pidState_t *pidState) +void checkItermLimitingActive(pidState_t *pidState) { - bool shouldActivate=false; + bool shouldActivate; if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent + shouldActivate = mixerIsOutputSaturated(); } - return STATE(ANTI_WINDUP) || shouldActivate; + pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ - shouldActivate |= true; + pidState->itermFreezeActive = true; } else { - shouldActivate |= false; + pidState->itermFreezeActive = false; } + } else + { + pidState->itermFreezeActive = false; } - shouldActivate |=checkItermLimitingActive(pidState); - pidState->itermFreezeActive = shouldActivate; + } void FAST_CODE pidController(float dT) @@ -1129,14 +1135,15 @@ void FAST_CODE pidController(float dT) pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } - // Prevent Iterm accumulation when motors are near its saturation - antiWindupScaler = constrainf((1.0f - MAX(2*motorItermWindupPoint,1) * getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); + // Prevent strong Iterm accumulation during stick inputs + antiWindupScaler = constrainf((1.0f - getMotorMixRange()) / motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control + checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); pidControllerApplyFn(&pidState[axis], axis, dT); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8e2931c539c..e7d3981a07b 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -31,6 +31,10 @@ #define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90 +#define FW_ITERM_THROW_LIMIT_DEFAULT 165 +#define FW_ITERM_THROW_LIMIT_MIN 0 +#define FW_ITERM_THROW_LIMIT_MAX 500 + #define AXIS_ACCEL_MIN_LIMIT 50 #define HEADING_HOLD_ERROR_LPF_FREQ 2 @@ -117,9 +121,9 @@ typedef struct pidProfile_s { uint16_t pidSumLimit; uint16_t pidSumLimitYaw; - uint16_t pidItermLimitPercent; // Airplane-specific parameters + uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. From 454387f24882fece82ffc86533c5b5f0c256127a Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 9 Aug 2023 15:02:46 +0900 Subject: [PATCH 47/60] updates on documents for mixer_profile --- docs/MixerProfile.md | 22 ++++++++++++++++++++-- docs/Settings.md | 10 +++++----- src/main/fc/settings.yaml | 10 +++++----- 3 files changed, 30 insertions(+), 12 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index e586cfc4017..b30753e631a 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -147,12 +147,13 @@ The use of Transition Mode is recommended to enable further features and future 38 is the input source for transition input; use this to tilt motor to gain airspeed. -Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup: +Example: Increase servo 1 output by +45 with speed of maximum when transition mode is activated for tilted motor setup: ``` # rule no; servo index; input source; rate; speed; activate logic function number -smix 6 1 38 45 150 -1 +smix 6 1 38 45 0 -1 ``` +Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. A faster tilting servo speed or more forwarded tilting servo position on transition input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. #### Motor @@ -186,6 +187,23 @@ Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input It is also possible to set it as 4 state switch by adding FW(profile1) with transition on. +### Automated Transition +This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing. +Set `mixer_switch_on_rth` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode, for example, 50 ds. And set `mixer_switch_on_land` to `ON` in mixer_profile for FW mode to let the model land in MC mode. +``` +mixer_profile 2 +set mixer_switch_on_rth = ON +set mixer_switch_trans_timer = 50 +mixer_profile 1 +set mixer_switch_on_land = ON +save +``` + +`ON` for a mixer_profile\`s `mixer_switch_on_rth` or `mixer_switch_on_land` means to schedule a Automated Transition when RTH head home or RTH Land is requested by navigation controller. We need to schedule a Automated Transition in MC mode when it is heading home, So set `mixer_switch_on_rth` to `ON` for MC mixer_profile. We do not need a Automated Transition in FW mode when it is heading home, So set `mixer_switch_on_rth` to `OFF` for FW mixer_profile. + +When `mixer_switch_on_rth`:`OFF` and `mixer_switch_on_land`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. + + ## Happy flying Remember that this is currently an emerging capability: diff --git a/docs/Settings.md b/docs/Settings.md index 7c6479ff9c7..229b0b716a8 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2534,7 +2534,7 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly ### mixer_pid_profile_linking -If enabled, pid profile index will follow mixer profile index +If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup. | Default | Min | Max | | --- | --- | --- | @@ -2544,7 +2544,7 @@ If enabled, pid profile index will follow mixer profile index ### mixer_switch_on_land -If set to on, mixer_profile will switch when Landing +If set to on, This mixer_profile will try to switch to another mixer_profile when RTH landing is requested, Only applies if this mixer_profile is a AIRPLANE platform_type. | Default | Min | Max | | --- | --- | --- | @@ -2554,7 +2554,7 @@ If set to on, mixer_profile will switch when Landing ### mixer_switch_on_rth -If set to on, mixer_profile will switch when it is heading home +If set to on, This mixer_profile will try to switch to another mixer_profile when RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius, Only applies if this mixer_profile is a MULTIROTOR or TRICOPTER platform_type. | Default | Min | Max | | --- | --- | --- | @@ -2564,7 +2564,7 @@ If set to on, mixer_profile will switch when it is heading home ### mixer_switch_trans_timer -If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch +If switch another mixer_profile is scheduled by mixer_switch_on_rth or mixer_switch_on_land. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. | Default | Min | Max | | --- | --- | --- | @@ -2634,7 +2634,7 @@ Output frequency (in Hz) for motor pins. Applies only to brushed motors. ### motorstop_on_low -If enabled, motor will stop when throttle is low +If enabled, motor will stop when throttle is low on this mixer_profile | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index c50860fd573..80540cc3968 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1164,27 +1164,27 @@ groups: field: mixer_config.outputMode table: output_mode - name: motorstop_on_low - description: "If enabled, motor will stop when throttle is low" + description: "If enabled, motor will stop when throttle is low on this mixer_profile" default_value: OFF field: mixer_config.motorstopOnLow type: bool - name: mixer_pid_profile_linking - description: "If enabled, pid profile index will follow mixer profile index" + description: "If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup." default_value: OFF field: mixer_config.PIDProfileLinking type: bool - name: mixer_switch_on_rth - description: "If set to on, mixer_profile will switch when it is heading home" + description: "If set to on, This mixer_profile will try to switch to another mixer_profile when RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius, Only applies if this mixer_profile is a MULTIROTOR or TRICOPTER platform_type." default_value: OFF field: mixer_config.switchOnRTH type: bool - name: mixer_switch_on_land - description: "If set to on, mixer_profile will switch when Landing" + description: "If set to on, This mixer_profile will try to switch to another mixer_profile when RTH landing is requested, Only applies if this mixer_profile is a AIRPLANE platform_type." default_value: OFF field: mixer_config.switchOnLand type: bool - name: mixer_switch_trans_timer - description: "If swith mixer_profile on failsafe is required, Activate MixerTransion mode for this many decisecond(0.1s) before the actual mixer_profile switch" + description: "If switch another mixer_profile is scheduled by mixer_switch_on_rth or mixer_switch_on_land. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 From 6ac1bec25e33e9dbf912eeca5221423f76478b8d Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 12 Aug 2023 15:50:10 +0900 Subject: [PATCH 48/60] update the docs --- docs/MixerProfile.md | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index b30753e631a..db31e647a53 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -130,7 +130,7 @@ Note that default profile is profile `1`. You can use `MAX` servo input to set a fixed input for the tilting servo. Speed setting for `MAX` input is available in the CLI. -It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilisation even in MR mode to get improved authority when airspeed is high. +It is recommended to have some amount of control surface (elevon / elevator) mapped for stabilization even in MR mode to get improved authority when airspeed is high. **Double check all settings in CLI with the `diff all` command**; make sure you have set the correct settings. Also check what will change with `mixer_profile`. For example servo output min / max range will not change. But `smix` and `mmix` will change. @@ -147,13 +147,13 @@ The use of Transition Mode is recommended to enable further features and future 38 is the input source for transition input; use this to tilt motor to gain airspeed. -Example: Increase servo 1 output by +45 with speed of maximum when transition mode is activated for tilted motor setup: +Example: Increase servo 1 output by +45 with speed of 150 when transition mode is activated for tilted motor setup: ``` # rule no; servo index; input source; rate; speed; activate logic function number -smix 6 1 38 45 0 -1 +smix 6 1 38 45 150 -1 ``` -Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. A faster tilting servo speed or more forwarded tilting servo position on transition input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. +Please note there will be a time window that tilting motors is providing up lift but rear motor isn't. Result in a sudden pitch raise on the entering of the mode. More forward tilting servo position on transition input(you can use 'speed' in servo rules to slowly move to this position), A faster tilting servo speed on `MAX` servo input will reduce the time window. OR lower the throttle on the entering of the FW mode to mitigate the effect. #### Motor @@ -210,5 +210,5 @@ Remember that this is currently an emerging capability: * Test every thing on bench first. * Then try MR or FW mode separately see if there are any problems. -* Try it somewhere you can recover your model in case of fail-safe. Fail-safe behaviour is unknown at the current stage of development. -* Use the INAV Discord for help and setup questions; use the Github Issues for reporting bugs and unexpected behaviours. For reporting on Github, a CLI `diff all`, a DVR and a Blackbox log of the incident will assist investigation. +* Try it somewhere you can recover your model in case of fail-safe. Fail-safe behavior is unknown at the current stage of development. +* Use the INAV Discord for help and setup questions; use the Github Issues for reporting bugs and unexpected behaviors. For reporting on Github, a CLI `diff all`, a DVR and a Blackbox log of the incident will assist investigation. From c51f15f3ad0d84b9caa3911d67bb0ce59db74ea5 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 12 Aug 2023 17:46:17 +0900 Subject: [PATCH 49/60] some cleanup --- src/main/navigation/navigation.c | 35 -------------------------------- 1 file changed, 35 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index de41be47672..3f57bcdb566 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -4448,34 +4448,6 @@ emergLandState_e getStateOfForcedEmergLanding(void) } } - -/*----------------------------------------------------------- - * Ability to mixer_profile(vtol) switch on external event - *-----------------------------------------------------------*/ -// void activateMIXERATHelper(void) -// { -// posControl.flags.mixerATHelperActivated = true; -// navProcessFSMEvents(selectNavEventFromBoxModeInput()); -// } - -// void abortMIXERATHelper(void) -// { -// // Disable emergency landing and make sure we back out of navigation mode to IDLE -// // If any navigation mode was active prior to emergency landing it will be re-enabled with next RX update -// posControl.flags.mixerATHelperActivated = false; -// navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); -// } - -// altHoldState_e getStateOfMIXERATHelper(void) -// { -// /* If forced emergency landing activated and in EMERG state */ -// if (posControl.flags.mixerATHelperActivated && (navGetStateFlags(posControl.navState) & NAV_CTL_ALT)) { -// return ALTHOLD_IN_PROGRESS; -// } else { -// return ALTHOLD_IDLE; -// } -// } - bool isWaypointMissionRTHActive(void) { return (navGetStateFlags(posControl.navState) & NAV_AUTO_RTH) && IS_RC_MODE_ACTIVE(BOXNAVWP) && @@ -4487,13 +4459,6 @@ 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(); From 1d0d5be21ef932b0848f1e04074aba427bcc9f23 Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 1 Sep 2023 00:51:09 +0900 Subject: [PATCH 50/60] refactoring --- src/main/flight/servos.c | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4d4bb497d19..fc0147ca3aa 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -104,7 +104,6 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; -static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer static bool servoOutputEnabled; static bool mixerUsesServos; @@ -194,7 +193,7 @@ void loadCustomServoMixer(void) } memcpy(¤tServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t)); - currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex; + currentServoMixer[servoRuleCount].rate = (j==currentMixerProfileIndex) ? currentServoMixer[servoRuleCount].rate : 0; //set rate to 0 if not active profile servoRuleCount++; } } @@ -353,9 +352,6 @@ void servoMixer(float dT) inputRaw = 0; } #endif - if (!currentServoMixerActivative[i]) { - inputRaw = 0; - } /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -438,7 +434,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} + if (currentServoMixer[i].rate==0) {continue;} // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; @@ -461,7 +457,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} + if (currentServoMixer[i].rate==0) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -474,7 +470,7 @@ void processServoAutotrimMode(void) if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} + if (currentServoMixer[i].rate==0) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -508,7 +504,7 @@ void processServoAutotrimMode(void) if (trimState == AUTOTRIM_SAVE_PENDING) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (!currentServoMixerActivative[i]) {continue;} + if (currentServoMixer[i].rate==0) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { From 8161ef8dfbdf3f700bfed030aeaf42960208db3e Mon Sep 17 00:00:00 2001 From: shota Date: Thu, 7 Sep 2023 23:32:09 +0900 Subject: [PATCH 51/60] updates on mixer_profile --- src/main/fc/settings.yaml | 13 ++++--------- src/main/flight/mixer_profile.c | 24 +++++++++++------------- src/main/flight/mixer_profile.h | 3 +-- 3 files changed, 16 insertions(+), 24 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 06c351324a3..a7622fe0c84 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1179,18 +1179,13 @@ groups: default_value: OFF field: mixer_config.PIDProfileLinking type: bool - - name: mixer_switch_on_rth - description: "If set to on, This mixer_profile will try to switch to another mixer_profile when RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius, Only applies if this mixer_profile is a MULTIROTOR or TRICOPTER platform_type." + - name: mixer_automated_switch + description: "If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type" default_value: OFF - field: mixer_config.switchOnRTH - type: bool - - name: mixer_switch_on_land - description: "If set to on, This mixer_profile will try to switch to another mixer_profile when RTH landing is requested, Only applies if this mixer_profile is a AIRPLANE platform_type." - default_value: OFF - field: mixer_config.switchOnLand + field: mixer_config.automated_switch type: bool - name: mixer_switch_trans_timer - description: "If switch another mixer_profile is scheduled by mixer_switch_on_rth or mixer_switch_on_land. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." + description: "If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch." default_value: 0 field: mixer_config.switchTransitionTimer min: 0 diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 044cb027abd..d600c533ebf 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -51,8 +51,7 @@ void pgResetFn_mixerProfiles(mixerProfile_t *instance) .outputMode = SETTING_OUTPUT_MODE_DEFAULT, .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, - .switchOnRTH = SETTING_MIXER_SWITCH_ON_RTH_DEFAULT, - .switchOnLand = SETTING_MIXER_SWITCH_ON_LAND_DEFAULT, + .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, .switchTransitionTimer = SETTING_MIXER_SWITCH_TRANS_TIMER_DEFAULT, }); for (int j = 0; j < MAX_SUPPORTED_MOTORS; j++) @@ -93,7 +92,7 @@ void mixerConfigInit(void) setConfigProfile(getConfigMixerProfile()); pidInit(); pidInitFilters(); - // pidResetErrorAccumulators(); + pidResetErrorAccumulators(); //should be safer to reset error accumulators schedulePidGainsUpdate(); navigationUsePIDs(); // set navigation pid gains } @@ -118,16 +117,15 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) return false; } - if ((required_action == MIXERAT_REQUEST_RTH) && (currentMixerConfig.switchOnRTH) && STATE(MULTIROTOR)) - { - //check next mixer_profile setting is valid, need to be false - return mixerConfigByIndex(nextProfileIndex)->switchOnRTH? false:true; - - } - else if ((required_action == MIXERAT_REQUEST_LAND) && (currentMixerConfig.switchOnLand) && STATE(AIRPLANE)) - { - //check next mixer_profile setting is valid, need to be false - return mixerConfigByIndex(nextProfileIndex)->switchOnLand? false:true; + if(currentMixerConfig.automated_switch){ + if ((required_action == MIXERAT_REQUEST_RTH) && STATE(MULTIROTOR)) + { + return true; + } + if ((required_action == MIXERAT_REQUEST_LAND) && STATE(AIRPLANE)) + { + return true; + } } return false; } diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index cda32f57a2c..c833651c214 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -17,8 +17,7 @@ typedef struct mixerConfig_s { uint8_t outputMode; bool motorstopOnLow; bool PIDProfileLinking; - bool switchOnRTH; - bool switchOnLand; + bool automated_switch; int16_t switchTransitionTimer; } mixerConfig_t; typedef struct mixerProfile_s { From 454a2898e58cbfe208bec3976068e8e2bae77bce Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 8 Sep 2023 00:02:58 +0900 Subject: [PATCH 52/60] add option to use tpa on yaw --- src/main/fc/controlrate_profile.c | 1 + src/main/fc/controlrate_profile_config_struct.h | 1 + src/main/fc/settings.yaml | 7 ++++++- src/main/flight/pid.c | 2 +- 4 files changed, 9 insertions(+), 2 deletions(-) diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 18f5bd343b6..c8b144e136c 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -44,6 +44,7 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .rcMid8 = SETTING_THR_MID_DEFAULT, .rcExpo8 = SETTING_THR_EXPO_DEFAULT, .dynPID = SETTING_TPA_RATE_DEFAULT, + .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT }, diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index e07c328560c..e4038537603 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -29,6 +29,7 @@ typedef struct controlRateConfig_s { uint8_t rcMid8; uint8_t rcExpo8; uint8_t dynPID; + bool dynPID_on_YAW; uint16_t pa_breakpoint; // Breakpoint where TPA is activated uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter } throttle; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 11a37c1ba6b..df5ecf76271 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1244,7 +1244,7 @@ groups: min: 0 max: 100 - name: tpa_rate - description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." + description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." default_value: 0 field: throttle.dynPID min: 0 @@ -1255,6 +1255,11 @@ groups: field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX + - name: tpa_on_yaw + description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors." + type: bool + field: throttle.dynPID_on_YAW + default_value: OFF - name: fw_tpa_time_constant description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details." default_value: 1500 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 639b1e838c9..06f7b92c80c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -527,7 +527,7 @@ void updatePIDCoefficients(void) pidState[axis].kT = 0.0f; } else { - const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor; + const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; From 5cf1665335ed315bff9a80b76afe01d9b29e95cf Mon Sep 17 00:00:00 2001 From: shota Date: Fri, 8 Sep 2023 00:20:05 +0900 Subject: [PATCH 53/60] update the documents for mixer_profile --- docs/MixerProfile.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index db31e647a53..0127da6774d 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -189,19 +189,19 @@ It is also possible to set it as 4 state switch by adding FW(profile1) with tran ### Automated Transition This feature is mainly for RTH in a failsafe event. When set properly, model will use the FW mode to fly home efficiently, And land in the MC mode for easier landing. -Set `mixer_switch_on_rth` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode, for example, 50 ds. And set `mixer_switch_on_land` to `ON` in mixer_profile for FW mode to let the model land in MC mode. +Set `mixer_automated_switch` to `ON` in mixer_profile for MC mode. Set `mixer_switch_trans_timer` in mixer_profile for MC mode for the time required to gain airspeed for your model before entering to FW mode, for example, 50 ds. Finally set `mixer_automated_switch` to `ON` in mixer_profile for FW mode to let the model land in MC mode. ``` mixer_profile 2 -set mixer_switch_on_rth = ON +set mixer_automated_switch = ON set mixer_switch_trans_timer = 50 mixer_profile 1 -set mixer_switch_on_land = ON +set mixer_automated_switch = ON save ``` -`ON` for a mixer_profile\`s `mixer_switch_on_rth` or `mixer_switch_on_land` means to schedule a Automated Transition when RTH head home or RTH Land is requested by navigation controller. We need to schedule a Automated Transition in MC mode when it is heading home, So set `mixer_switch_on_rth` to `ON` for MC mixer_profile. We do not need a Automated Transition in FW mode when it is heading home, So set `mixer_switch_on_rth` to `OFF` for FW mixer_profile. +`ON` for a mixer_profile\`s `mixer_automated_switch` means to schedule a Automated Transition when RTH head home(applies for MC mixer_profile) or RTH Land(applies for FW mixer_profile) is requested by navigation controller. -When `mixer_switch_on_rth`:`OFF` and `mixer_switch_on_land`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. +When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Model will not perform automated transition at all. ## Happy flying From e4ced70de73c4c90233ead12cd75dbbe7b5092f9 Mon Sep 17 00:00:00 2001 From: shota Date: Sun, 10 Sep 2023 19:09:09 +0900 Subject: [PATCH 54/60] update with timer overide --- docs/MixerProfile.md | 84 +++------------------ docs/Screenshots/timer_outputs.png | Bin 0 -> 17744 bytes docs/Settings.md | 32 ++++---- src/main/drivers/pwm_mapping.c | 14 ++++ src/main/drivers/pwm_mapping.h | 1 + src/main/fc/fc_init.c | 1 - src/main/flight/mixer_profile.c | 8 +- src/main/target/MATEKF405TE/CMakeLists.txt | 1 - src/main/target/MATEKF405TE/target.c | 18 +---- src/main/target/MATEKF405TE/target.h | 5 -- src/main/target/SITL/target.h | 1 - 11 files changed, 46 insertions(+), 119 deletions(-) create mode 100644 docs/Screenshots/timer_outputs.png diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index 0127da6774d..c0628638934 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -11,83 +11,18 @@ Please note that this is an emerging / experimental capability that will require ## Setup for VTOL - For mixer profile switching it is necessary to keep motor and servo PWM mapping consistent between Fixed-Wing (FW) and Multi-rotor (MR) profiles -- Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to change the source code `timerHardware` table to allow a consistent enumeration and thus mapping between MR and FW modes. -- For this reason, a **VTOL specific FC target is required**. This means that the pilot must build a custom target. In future there may be official VTOL FC targets. +- Traditionally, FW and MR have had different enumerations to define the PWM mappings. For VTOL operation it is necessary to set the `timer_output_mode` overrides to allow a consistent enumeration and thus mapping between MR and FW modes. +- A VTOL specific FC target was required in the early stage of the development, but thanks to `timer_output_mode` overrides, It is not needed anymore. - In operation, it is necessary to set the `mixer_profile` and the `pid_profile` separately and to set a [RC mode](#rc-mode-settings) to switch between them. -## FC target - -In order to keep motor and servo PWM mapping consistent and enable hot switching, the steps below are required. - -The following sections use a MATEKF405TE\_SD board (folder name `MATEKF405TE`) configured for VTOL as a example. - -The target name for VTOL build is `MATEKF405TE_SD_VTOL`, the standard target folder name is `MATEKF405TE`. - -### CMakeLists.text modifications - -#### Adding the VTOL target - -Add the VTOL target definition to `CMakeLists.txt`, i.e. the third line below. - -```c -target_stm32f405xg(MATEKF405TE) -target_stm32f405xg(MATEKF405TE_SD) -target_stm32f405xg(MATEKF405TE_SD_VTOL) //new target added -``` -### target.c modifications - -Two new enumerations are available to define the motors and servos used for VTOL. - -It is **important** to map all the PWM outputs to `TIM_USE_VTOL_MOTOR` or `TIM_USE_VTOL_SERVO` to ensure consistency between the MR mapping and FW mapping. - -For example, add the new section, encapsulated below by the `#ifdef MATEKF405TE_SD_VTOL` ... `else` section : - -```c -timerHardware_t timerHardware[] = { -#ifdef MATEKF405TE_SD_VTOL - // VTOL target specific mapping start from there - DEF_TIM(TIM8, CH4, PC9, TIM_USE_VTOL_MOTOR, 0, 0), // S1 for motor - DEF_TIM(TIM8, CH3, PC8, TIM_USE_VTOL_MOTOR, 0, 0), // S2 for motor - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_VTOL_MOTOR, 0, 0), // S3 for motor - DEF_TIM(TIM1, CH1, PA8, TIM_USE_VTOL_MOTOR, 0, 1), // S4 for motor - - DEF_TIM(TIM2, CH4, PB11, TIM_USE_VTOL_SERVO, 0, 0), // S5 for servo - DEF_TIM(TIM2, CH3, PB10, TIM_USE_VTOL_SERVO, 0, 0), // S6 for servo - DEF_TIM(TIM2, CH2, PB3, TIM_USE_VTOL_SERVO, 0, 0), // S7 for servo - DEF_TIM(TIM2, CH1, PA15, TIM_USE_VTOL_SERVO, 0, 0), // S8 for servo - - DEF_TIM(TIM12, CH1, PB14, TIM_USE_VTOL_SERVO, 0, 0), // S9 for servo - DEF_TIM(TIM13, CH1, PA6, TIM_USE_VTOL_SERVO, 0, 0), // S10 for servo - DEF_TIM(TIM4, CH1, PB6, TIM_USE_VTOL_MOTOR, 0, 0), // S11 for motor - // VTOL target specific mapping ends here -#else - // Non VOTL target start from here - // .........omitted for brevity -#endif - DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) - DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM - - DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 - DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 softserial1_Tx -}; -``` - -Note that using the VTOL enumerations does not affect the normal INAV requirement on the use of discrete timers for motors and servos. - -### target.h modification - -In `target.h`, define `ENABLE_MIXER_PROFILE_MCFW_HOTSWAP` to enable `mixer_profile` hot switching once you have set the `timer.c` PWM mapping: - -```c -#ifdef MATEKF405TE_SD_VTOL -#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP //Enable hot swap -#define MATEKF405TE_SD //Define the original target name keep its original configuration such as USBD_PRODUCT_STRING -#endif -``` +## Configuration +### Timer overrides +![Alt text](Screenshots/timer_outputs.png) -Once the target is built, it can be flashed to the FC. +Set Output mode to `AUTO`. And set all servo/motor Timer outputs to `MOTORS` or `SERVOS`. This will result in a consistent mapping between MR and FW modes. +A Sanity check on timer overrides settings could potentially block Profile Switch for safety reasons. -## Configuration +For SITL builds, is not necessary to set timer overrides. ### Profile Switch @@ -95,7 +30,7 @@ Setup the FW mode and MR mode separately in two different mixer profiles: In this example, FW mode is `mixer_profile` 1 and MR mode is `mixer_profile` 2. -Currently, the INAV Configurator does not support `mixer_profile`, so some of the settings have to be done in CLI. +Currently, the INAV Configurator does not fully support `mixer_profile`, so some of the settings have to be done in CLI. Add `set mixer_pid_profile_linking = ON` in order to enable `pid_profile` auto handling. It will change the `pid profile` index according to the `mixer_profile` index on FC boot and allow `mixer_profile` hot switching (this is recommended usage). @@ -209,6 +144,7 @@ When `mixer_automated_switch`:`OFF` is set for all mixer_profiles(defaults). Mod Remember that this is currently an emerging capability: * Test every thing on bench first. +* Remove the props and try `MIXER PROFILE 2`, `MIXER TRANSITION` RC modes while arming. * Then try MR or FW mode separately see if there are any problems. * Try it somewhere you can recover your model in case of fail-safe. Fail-safe behavior is unknown at the current stage of development. * Use the INAV Discord for help and setup questions; use the Github Issues for reporting bugs and unexpected behaviors. For reporting on Github, a CLI `diff all`, a DVR and a Blackbox log of the incident will assist investigation. diff --git a/docs/Screenshots/timer_outputs.png b/docs/Screenshots/timer_outputs.png new file mode 100644 index 0000000000000000000000000000000000000000..3675f64631e5495221e91abe3ce6b0e89fed10a4 GIT binary patch literal 17744 zcmeIaXH-*dgDxCUq$mi|JBW&aH0d2tdhaC^0i}f^y%!Y_snQ|zUIS7>hbUb-1c-ou z^b&d}v~T-7?^*NCnwhg^=9_ia`Eh<&62jhhcJ_VUSG)6BLrtELfQA4B0ud@IywU=J zu$e$0EGXU`;2A-lvpC=%ELSag8BoO#-3IXBw)G3u7a&ko%-u_K9N;njI|V~m5QwPb z&mXKFr{Xst(5n^2S1)wDOt)w6`{^8K?A~||+%gh^N!8-S$8EhGDe%yDdqJQQuKF40 zhHMx52LDSo@zTyjRz(HWv@DW0_;W4Tu2QFT*5S4+cJTb8CfWWhhoErkY=XOEi7p`{ zPZIF-8YXQXBNBYutcM=)#WO#858InNkE{W2rifF z+aS<8LM-6#hG-Br2xKe|!UBORSb~6efFh9wUWCSD0)f7{-zEluev4w`0WYWj@4p=N z5mYU&9z~tUdvmyk$*U>g2sGBu<(<4%-*yrFd9q#)n|+qz~sPY0v?G{ibT7U&yJ$tSF3qrqoi^c=*uYSr`h1z=-@O$*9u zv>*_ZObkRiRK4ZJ%j7HLi*yh13&Dy>eHYe^Lq;*+$DoHtEqLw+V)KbF=oDG6UWy?oTr1M zLI^=Q%WglD#``xqVsoyT=@7({wm{2)$zY@H2a|~BtGHt60U*%Gybpu)5p`6Ss3P~Z zy}X<7J3mZW^6~Uw{W|oG^-Fy&grPbG<7B(F<%jMb@NoyDziG^zYt*qr^gNKyN_=hu_zbVMpJ9is zK*4T3+~}h+HT-4mtI{3%!G?48B7du_tb^d4E3cfSEds`^7);lP$C%5OW;az|%-6cB zby@H06*KqU&IL>)siWi#im$d;%J^<_S7^?P(OLJP&+39A&0N4e;8v&*ZU)8qB|2ta zM4Qf$9ZK(f7Hw_WrBLmAH$8hmt%_O0Enwsv&5VH+S2^bdVD$B#Qq#xM&0U0;UeDfJ z@q@cU&jvc}XHRw!V@~NF21B)qn3ftM5NL?gno{~%2kTs}rEVqLfpAM>n@bz^YLm6EO>uP6Y_T^4?UBEHNf$-qdA1nec1CsQS(9qQUvWhqBw?ILz z6Nb3NZ-3>cmQ>n8@{o18y^~q~vh}Kx&CZ{OBU@$X{Z8Lz3?K~kyT)T$ZZ7U@goxx= zjH&gL4F+~h0rMoCa!4|0{jl%4xzgr*I0M(^2EArw;!c()|{2F{D}vm@orgvmS>KAMbwdqv;{) zriIFkZHdu3bt!+hGRJ^rK|&#I?o4XTP^XdqX=2!B9HQ;T&0e?NmH1FEFlU8tj<|vh zD*Bpa!His^V)bg_W{-UCT$rX*&emj&IFM92oGvHZj`fph^ZwA^@Ic6OQzY4(S9Duor!=)oh%eqt+M&7+0OeQ&^oJP(X$~@0Ae_Nq_6?( z8F~5N0huPp)^PvDP0tJg;7D^z^{@Y6R?3I}X%sW54~)K9ijkU(WsyJXk-FA&aBvvX z1J0F+)&ub_EPZ=_3)4>7mL0*>6!r&5eG0%>oxw2?+-(N4bkBg%tZUWVNe|RUP@e

oBr zCgVi0Zpa*sD`7wXF|?%ApvH)=ZENctm#}H&F6F?;>Bm<5cu>$?&3X0jkBZi(!LIx} z@ti)E>Jc?nxO4N_)wYS#Yy1wo0-LH)NL~GVbTSrHv^fSyxmmMbU1GMMuIJL z|6PxpYJbHFdt+b~xuBh{g}SVcUGkKkCgixSq{|iNpl<)gUs=2jT*k38daq{wXo~%O z|BgUiO!by_0sN{FHt9Kqq!BT?rbV;R_VaINB|~0bxDDN#J^TotK1CDy>?}~9oG%Yt zwbiw((LK?dao#pP&);Fg%kUlg@I%EPA;Y+k-dV3 z952rl zGD=I`YdX(`dgiRVXVvYF87@F|{>rc=Ns}@{DK1;Y++(Q8!;a>Z9&`H+rQH>^t;oXK15u-PSu9Fmy zNVvmj3ms%ecos1*Ef`qJ-(TAP0LRoe;58$)i99>yZ(K0}SXMTE15 zNZzc^&iveHgFI~43|#o^ki6r0LgGbmQkrF3wSo7Ibz`62ZrzZGuC}s2)N1?r&ij>< zdHA7-p*8&2cJ{`trmgB5ohvN!CXgGo+QU%OZrU?%L~Azjo%ir4F4YKAKXcrO${auV zZ5SDe1sa0*Q%a`?wyJYH8$FQndr5xdQ(qAkFIAqB!_jS=qpWIH4Zqe$DIvVoS!FV2 z4`+nnRcyN2)je!oD*BJWLIX2}CGI_UuGlS(EJir_1RUOkBME4We#}LV@DBxEgtu8- zId|0;ZW`63xu-j)PM&WGGg-f8X~$}9tk5hGQq^3gd|4enz zB|6Oo)|TiBqtS28@qBm?RiP~pD(Lg~uFzG2g7>vRSwx2?^C)Uhah-?W`CiRr~GMI=u#KU?P0+pM{^@vF3a zv?+@A5i2vt^`S}!f!3rv?WEPv4T6`I-bMijv`{nK5OR*M&jP=xG#-s~Woef}-NX-8 z9b)*6%OZON7g&G(j(`QWX02aVo_?oH1MlHtm=@bG8(FI4Dr;X|J+ut>+{j2o zO?!JsS|a9~#jMDa9pd;EmW)fOq0NojZR9~*mk-~zhlB|!#;K}T<(B)*YyY9;x>Knz zD&BK`oy&OV#s>$eJD_`1S9H?pv`cT3IVOJ9fQf7lC)Y{7B=irk)J0)3Qk8gf9-4k% z?0q(SvSQ_6tk1}PWDl7PO-9*`oRwhgT3*BiHz=i^Q`?!=YaS0h?f-E~GvPNTYK`_S zGHH>e6%=J5<7_^*+je|Aa0zX*h#5FCJ&dMtNYydN4T0-MxSb~4Ei_v6+5NH$K} zETftI7Imi;Q_{3N*_RkP8YQhP{c#x)xCd|YbxwVLBAzp~>UX^$(weNB!)k3Z!SKKbU0qvg(>v{b zaylJxwXDAWIqo}ZV5FsmdHN{dyZvowCZ1_YD&O#4MZ)(bw;9iJ5Q*2fYZb*~c}zEb z;THb@#_9NeJw#zvdvTFg9BDAP}?K2St65CZxp z9qZP&Se`Wrc4{pcAAf&ZU3ob1Ln5*1%66lwt1HLCaOpLSq2WgNE!TC4Ris-T7U*5{ z<{AlcdQ{RW?Y3ydmFl1*X;p;+o0D>%D}H6xq@SAtSjiX^+7@JqaW%3O13EgpJ|Ze zFd-qKsPu{TO9So1gGGnkXb191UJf!hDDaA36Zf(54c*~!2xEelxh?ybxpXFL+5rIp zHmN50xLKx^p-CF^b6KlpF6uU|^3&0FSMjn?2YT0&cz4kS`#YfD_Kl3vb-JY*-t3vr z^#pJdM_DK~a0t=~ikW6@?}HB4hEX{$Kc(y8);LU6zg6R?lAAq`EG;Y3y=uSjuD*%( z@!RmoaKkiN-rT(lm#ii_K}l(^{IA1$|9&SCbXy3M zkQNUc|2%cjU)1EF_K8FU$Ffl8fI}xXYgq-wU)E|=P^-blL$kp9kR!14_S88VZ^Uimk_$4GJ+8!+b&}8l`rz9tzQZ@@A1|JYd zVVc1L0W+rCo`UcTWB}}H?jt*to`!87Hdjf34xGv2(mF3|*72^SmMs}aTfOQ+m`+z2 z$ey=p@Ysswm~fw2NYYx!y{14o8_e7szZe`ss0&2pINU{TPfYJU?aM2xz3IrC#&lfB zZ2Jag!!o#BKH4#~s1I)&R=V&(m&ztMu1EH#>A~Fd9T4}Q;ZGAiTZ>`e*N%SEXACP& z8g(>v<+O=0j4zroxYykRVIumm9u+r@8LyaR*};5ujT1TMySYjQC(rUfscqb%MIH#?fypMBGM^+fm@cU!f>+((2 zEckegOzeOjW*t_7D3c`N&b1hRlX=0ugHJ*$jQ>97$ToVOqHM7~wn6z}( zqj<_v-K(XG#J16<_kmw>@$dVo*T>KlEIyt**|abTj5nKTqb(>Y(3`2G{iO8CEThsI ze0cQoaEh|6C=E(!D0CeCIn|_}60+-sFf9#`YoyL`Km;_Zy}ULrfy0dMUVAk6F_w;h zMg;ab2JUt*`8Yak34fArl)8*|ti9H^lXJZkX*v(p#2A{$Cy3G+v#L^8E)?!B+<{$O zMICU%PyFX9KO?yGSE5H6hJQ$u)qV}gS5lc*SLS!=kbz0qdt!kwpPRAXfHVA79&ejJ zJjRHlSItne3&mzrq!|-mPlk(1e={S5HoL1PmlnVI-hX@U;n1kAAuESF<-lCB5Eb!}(4G~H(OO^WhL3{QrOnQ7AP zVP?_6 zih5qW;s6z4mA8i2b-!+w_~qLwM<-g&7o~iUf$xRohnd>>0HyP<9uA75$0)*@i}ErU zn$$(BV<^<1n_ z=sCf5RhaC;Myz9R>+?mz96hgIcfGujLP-0(s}ro|JQ6ywS4aI;F+!v#!%jGj)%95w zb7$0E)y?^ZjBK&zxz6di?-6&6oQMR88C%+Fv!Naruno zmrToL4y=<2MbfQoka_a6OVE+gv9S?!#W0+0-JV^ZEky-7z!>KW%#5Khe6?a;mM0zD zu6j`s?Yp@mr5&HtY+&3+r~Li?!=BJD*r0}iR?wJE(KnjFi*Y$G5v{~B=ya1K%5c1e z)_KX%?rZxCy225G#b)gLveC|Nk99Lg^?FN0AGk-yYD-(QJP$r{KjWE>&@j8yPaJZV zW6!0Vz!XuSm(s3s*itsFb?8|0Iz3CcR2@bZdLK?m!PkS2Zxkhn$gIuBgB7RSEGvl+ z$9GLI@G26Vs?jyK>c_@dak&Wys=`z$p1|Sgu0U-SCSZ_#aM0MLkF>zb)V4OQ`3e63 zG-$I)tbIh1elfDRQ^ZQaYrK7`GOa~B7cDD;Q_>oUP!JA2I}q znXz81fde|ag~C>N`y%R6d+}Ask^PElrRKue&g`OVe}U}Dkph$eE+fwn<2vxU%g8)B z)%>Jc-^V7zrCBWMxpQW;&bCe=A5Od;6U-(Z_?(b@PlD5FLpgjsngaH1+O9f#+lAye zPN)}3V9d~?)A(9Q&V>Gv#afZ|vkt^@@D4TE$Vb<5|5fo^_mam=H)wwnsftNhy>kEV z5Na!A;WXwRD#ckIj^?gFrS#e|y+u!Q)b;7V$v|dEI?$SwF7D^>rZSr$%nB^$CUcu* zw0UY6VdH9~c7UDuWFsNfQut%$C5n}rHY&DIdWZzaDz(VwP6a2pt{1KP zYMDCa1fh}do)?gN(Ym0R)ml0*Ux7c-rS}F4lz!j%nGDW@-+M-_XA!(mp6;k{QjoM6 zbI@;bXC3rl{Ii(0iZVuy$>3qpG%V0sL9qJwt)EpdNuQ|vW<4L)f!B#0&cgnN0C}ED)3A+%4c~V-z>mqoZ3+-ldyIGtXY=K6+eYk%qLV ztZFS$@*di)bu+byZiGE-3Vh)ds0CP?k7T57f3cdFP`Y+u+w<`Z43?7dd5>L`p7q0t zU+Q^s8F^Gjpx``_h^g$=Wnk-b7}MB=S)Kdf0?fzpTFTC&?VJQYh3PrxwnK#+vDo!Y z_OF4t4?9`_X47>As)lKW^l9%WRB1~2W@)JEHVF+?{`6s#$|Xno8`V~?eYd{8JQsCa z|AO3nT?c$#1GbO9G@eT~sdZRgMP!;8!Zs6!`=wyIWek<$bB{E7~DzlrkIJ&))(k(5WBFM}yX zBwq)RPu>o>Ga>Gkl{r&mx?*+I$sqd`v*V)){y4nxU98-pCSn=fYPRqUSz{?XbAf!= zO7*K^jUnA3X8hM_2TT;R5CHQTuN@qouN=GHql9OSR8ab{Lub$FWc*`(vPjnK9@tLI zEvbE4`e@=)RolCn1r;K@&Xb!vEGEYWG}NhP`J z1K)92Z?NrI(cHO@$fJebBTS7x;{9p20Db-E0{H!wZ zx#Hd#89=C3rM>J{9}70y(X{}^`dg6lmO@Qw=7C~CA@R63`{O(ff$;Jld%b)n(^Ky# zx=KjI^;eagggP^F>6izVYy9vi_|67q1kRYqu_ z>j!eFJLrCtSM7I#Jo%-J4sud~^9&`U_9Qc32U22HxD-{QJsPu`Nk}S#`87hj zV&)YeU2&P!T8!I->OHJfh?rcX}Qn0QdJhh(k=am)8%Wq z;IB>tCHP05FQZSDIP2<+}m5+`{1_(X8$QMh-xNAnXN2P}{nKjLz)6I)#G{P{5;$3>OBNJ`Ba zG$lN?Ql_VUjt5q~`y>5Q_7?V!j@+$*C5#I5CRloGMHPqMC*6W&akBh zfz-(d_WwVZsb}c$N5kq9pwP|QCKqGc%m$N)c^9Z=_r~?xw@{315k3gSWq?n_6nj$Q zV)U7Y(V(K5Em?FX|JHloqi@|MKA1*2lLdQVgp?GhqSqC@zIu@1!#!s|%hW>7*|pjS z%lYI#!~1I&{EG-d-$1KAps9K{XrV;ai=LO9x@u{KUEEG1uJ?yW_#qP3j-T1<8Ao;! z-$Es3r#+|dC6@mxia)t#9t3A<|M*2$#w}mN=&c5h7N#HC&zxyehp*HmS{`%XL?OvH z&KzMC4f1^D{za-;usB91jU`sK`3qVdD~HNUII`~NdrPAN_aln!fSgyJQdfm!PpgK7 z=w59hP)NwJ)maN{DG7(DHp}y9eg-cCG9?G;Cqawd>}yW6ofc9I!%xyyZ^x?Bz!!QR z54oolQhN?-Z#zA#1U~<@BG+6z_l{v2ft}A2Mi+f{gS3It@ViPnzEiV4wu446QyyJb zy=Pk6-$&Y515McGPt%U0aB{9Ftpv~)hx%ygPT#Rf$yTM0RyVKU5QzIlaz>Q718yy5 z)O*^?yDBsAEo`#FD8FP^s_X)Rc>ItqT`jyKgHOE+Wmx7NQR@LaKF=+aG3oA3hWn1! zzg*i!=rVG0jv~R}^dYW%Ro$ujB14WswQkYXX${!G{_6q?ghxSJYv1&VQdDM|XHiS$ z_ncuM{$>zk#qQ1iVTNo_E>B!0GuhWi8F2{NO)qNhth%XjfKE?u+>7!1Y=}v0s4ijTC=) zIhr*jE@J6hrb++t=^$qIrtZ4o@_841gvfa;dD_s{7%!hrUAbPrD)IS>g+yz~6wqe) zed(SFz={BunDR$caGCzRdpG&C+NPJK>#01ooYh7hw-98h$%kI(dmhY?*3`7@#Zu8` znbp~1Tz{fZ71_NVy@-vQclClTKHvPtVsrZk0zL&4SzR2S$jNIswUCR(C;c&%hnZ9l zOKTi2v9j_>ofTOYq62 z!?N*&l)p?>wSQL#GWs#6=wl%{27b|WaU7&gdkggKy!^h|D-Uztn5iVXCbRxEbwV6! z;Knur-pgOLfxjz2f5+*TxBquFp{6Ucf%VZmz>Ne+@ACY~^Z%<|A{48WjDwBMNR?&J z_~hiO{R{QStgMjrk(>iB@gd-_ccO!FiAg0Rfdt-eB>TmQs80nPu8Vu0p%#VD4CtRH z3)(zZ%OCefeOEE5XqyUF5a^a_`>n844`#ndp^echmRO)W55qOEfUw>89@8JG#>Q6C zZWK`kYoB&JUePa^nj#x3FZDZ2-HQWk;i7(*6r{o*0w&0GJQ!p}G91YJo*kF9HA&OUE9t&) zZ@q*N=u(r0T6jm4zpiPArkPB`hMt*r*T1PYPA~=fWUrK{6HnyL$GuFUFTal{rq1X zO9=T6B2qv)sC2x#%f+cP=*Gb$vKA)_`K)KKmPK80v(6tyI{zOUzVo8fzw${oI?|I? zY8awgw!TIOtk+_J)-88%i3`}pSW)=IgQaD#Q3Lf(eq}_drn%fJ=Ms2lR-5ytl*;T6 zs7yhs|FFg_3+(Pf)wEn0g*Rt-h;J~&J(^#+N$MFu@zB4V5j)KN?rEg1szU~J@Ti{# zQOzn!Z@$4`=v&OZx)J~7jzGDl&`ae~@jRHpzT;(Dv&k^;G${$!!w3!ZiV(OoTwMzO zfa`<6i+p}%MG zAxX$RwJM8OzpzKSHdX9R2Wvir5R5(i5aRv3=+Lq;=i9hMEOS1hz2_^pw_7pW5r5>i zI-%&9Z%nA!>R;F3id1?lNR!3PcF^`#(CNO9L==oTg0&?;h6%<-Xy|7q+4wRaa- zcn93y(A)*_5NrN5nl%@Yt)6Z`TwI)JQn-U!GG}pkSX=R9L?5QM0Ij-?oM^!IB+%5O-=&A0?)b4bCDAHqZmKzNW+NZq>WZ`&Z%T8y)t$j*kwE5U+E&KQV z&Zx8xPy$_gTB4xy7Ol`Rke4K zRU!pj*em)Hoh(Oevm~5G305lbi-#Cm_t=|_OG{u8M0his%Qd-*Kc2A$JnK!UY%#?u za!}^hU|QE#eNjNJE(4|YP(mIiWbD{~%^zZqMOmuD-_KL{DFm4sF6wdsiYWvE(Sj-Kh ztclj({Z7ZJJm%5&z2-65XF6+<1NlDP9->M3B8O@?I>aPOO&0C)hK8w6)HCVGWd?jS zHDg;J2?o18mhGtCNxm84rMYCaCIvJM5On01{x10wec4y$g+W)tejmz>8wo47_(Usw zF9<39Ojq#r<*O*{%1;QEC-3+LY>He~KGQPEn3wnySTDAWkg3a^`LR#S%Qr!W(nV-^ zErRKV+nFEHNeVj6*`E$(h@qyMcL{$`Id*+n5)E)10#xxGVb2JS!6Uhb&|G$X0UTTB z4%rr(FW#gXYT!3ic$}#P0Tc2QpM{Y;yP>9$6qx}(qD9qCy&3OkDYr_9#@V*j;|ehS zvu$c9ZlINZ_*0vEl5qAmNmhLk3qh=G@e^VL01z`u5{XXuRctf-x7If(m7C-( zVN6Kk`cM{~%IA}$nzUm>Dy_ov&4n7LnO0=1nLmkT&VcDw{Oc~$s$!fl%N-AY*-C@= z+0G$4Unaewg{?^}rmgZe!H*oV?8j{s*Li1$w54-wp_$?Ln1kI)f-Ks<0a(u& z*0KN=IxVu0dom>F^FFDQUeQ@5RETfqDB>UR2+O=={{_52 z)S_<1L=55zcpHyhuLj)tQKWy7G}+=%@0O#VRhD)Th-+PT(LnDfd`5Gr-3Ob~DNpbHVC4hhK z4Xm{zLLq>g@`oQ5y&mYrUC28fIOao*8mj{|6AI8wzHON^g?p~_somJ1d$0Z;4KV9I zlqx7`X=#PPDJ;Pu@Y&7&O0Ak2wtPd~^Hsiu$LhMVPCicm7jnkLX(9+3k%VU4^6y^WxhvTS`@)>>3ca5W7$4uiv@|x)Om){>$&il~hDT$l*RVm*{j>-zN zs}G;{ZP=G-Se2L^PvC3{Yn2j|qF2nci~CY|$oj!=aMA>@CPea>2zMtHR;CU@p@P0y zV@-US$jkFX_w^CxqKWVJ;5z$Mq*AbX)2JD2(+*;&Rj!#AejI67PjRE#k#DJL*Elp? zH$Gi=;4Q&xRY@_q<0*L;Bt7pc5#&|4EIVJGwn?UuSzYNiT?bh7k8cusXId;R>Ml3M z!(BvA)|FIVV>9>I~%zmfh?tp7LExc%W~bJCr$v)*RdxG=)I8kq{3n7Bcx8+nftE za=n*Rxwh@YN*2QUBP$dd^}7ROLg}B(IfkW%ex@)sDu!<8NxKX^cAtCz`#L1WXM8hi zOT*eG;<{?n8Ajsq8}HMXcV|CaylZBKJa>$(ohUQJOJSAcwrBZeiHr8wpzKPZHn~KD#QF3!x_IDwuo_i0Ll5;c;h$5F}ciNW3Hr`{xK9TL?u5=FTd) zo9dr6>3{7=9_SE;1Y)jv^e2$TY`cV|t~R*HitnnryK?KZn!G`i{3L^2m^bE8{tH6L z_~1w!`>+j%UJLEbsw2uZ^5j);MDN3KpYy?_(nX{?EI8t|DlyxjP8W4yh{a0|$^I#wrj>iR+9_C(9xg=o6~H8s|X zKxTLVe?4tWOx9Rzx^%H#hiUq0#H$I9&ScTD&VbD)bg#E8b+@;k%D|IG#N?U+2%aS2 zGheNCz)Jo0hDMrStX|&dyjO@`xTujO|8tKi7O8yipgvkgz5Dn*ySlnMox%qK$f^S$ z-vq^z>wzZtnG;Q6*{%zwJz{DNYOmTba4rm}mVsl? zVrm?ve~n|QwzPL&+nDXlhfQLy z^Q(DG=Rm|EJ-y1=Qw|Q~@{jmDnJ`0ccJ}Jz&d@JAGdD)k9ffLR#DH%tZRU!=6OaIV zlOTBpD{=GF?PXgxa!X!_C33{@g*+H!8FsU^eyKtzZ*1iz+t>R1+LwQeXW!%H0KZD@ z?=hJPm3r(rSFVzpB%2WT>(m8Ij2cpSe=wC#o<%P5>n(ZV;NXs))xoswwno=AQQziu z^%*#qu%eKO(9bErm6>;y3VNzLCDN!CWTR)|5Y_GTu+R{r8%4nPFkBh-`1&YH`^J1z zZIX*;q%KM&wP7@835aubeI&bnobhkZn-7}TH(s(>igN%szidu3YuPN=i^~KFq zT3$LZX^r&h*TmegPW!H?@?4&JfP^sBb8!G3k_ro zjXjzMl~QsMj77bAIwa^E=8M zn$`WWzsn(mQy(&)voHd>P<-Ev1z_msSQf)OIvKLFWD*8MM2SgS2_h?*2fa)2$l%k>{y>gdeQ9p%_>?^bJ zIJ39@(_=aXEnZiO-lYTxoB(i0D<|eB^LL!dwgt;6i($4!;n;M`I&RG#V9*i^79d_B z3_p2vMRJKRA?I;iy{pC^ZpRggvip0ji~u%$4w}3RbU#Gr>KkHEZFew zQ}Yxw(6UbKoxk><~nl?%zgNzqjNLWt7x&o0r zWeC{|GoM$7^NOf^?hwh*inj>Uqe0XhB0Ya$=m`k)V6Y3c7;D`m8WJ^bh?U_Z{ygDP zsAsrYI6mI@D9IA>nQ&H*eg?wNM6ZZ^Z+8`mTEBTR!B>Z@Qt3_gWY7K(rZGnT@3K%1 zHku@N#HDBS=x-^Jw021`6`@~i`uhSOVJX-EjJ^9ms=yg8vY5QXLhq}`Ml)JtfN5y` zi&I7bNxMK3UTQo+mfuB4xo;=Es_G`{5Npm<`z8sa%c#q`sM66w>%(j0g?TKtDLlL8 zFYd>6e6p@kcs|E49+VPPWs588#qP}JRf!8Q_l@FAi3vy z&@0@7krlGMyVU!bhjKbvi5ZsB^oIdo91_`$zvt2Q9$hYEaVKgY^hird+{^0O&y9Ux=f#A_d&$Tk8LU> zOvvbHO*}2=%iD0(zjr=oz6~5ucZ^M2!TFBFxxla2JjLbZ&x%6krmNpZw9J`C-3NlV zp^f@Kq5c0X5HeT;`x0--|K|+G)9wFfF#cB!Mn*%cg9XzGzugyuf*14q*%UX`mVp4s zbajEovO@**8d4VBr$hC-bQ=#G?ul;KtZ^FU`~n2_R34@2+hk>chKypF11)Slezw$= z&KR83gG3a^?^*|7gTd{qQnupSMMVYI4mDe!%xYu?VbZwaef|)^`uI|8&LiFy?Mp2cX*|ax#g!>N z@hb3%je;AK%TJc4k`p2gg5!h-Z}#A+9mi(RsuD znUL8;arA5X#R|eT^3KhVsb#7YcxsdL_!o_(IG{7S^L6gxWidWTI=6vJT1$6toM0u% zst%?3O4aq%o;QmfXZmAP3Hkg#9P(AP)=%bKx%a4!c@k3LzjL2XLgJjSm;CDWPID+riD zOMblkv+4CwSTok64-&@~vm>g+``*5yxx|LScG&OX1*JbzjW%AZcO*$LDR-c)l^bWc_iz}Bk~cT5bc@b)E9LZ~eVNVz6xf=<=#-ETSs->93XOpS zKB@G)i!(vDiV59lc)eJ-MtuA3?=WwIC#EEusRUBnJr=4Casy8zBnT}#2}N>T`^pF> za(^nzNqa{XV=Wt>v} zC1UO!$$9x_ACim$_dioOZW)1rrGlPldTlT`tp-QpANkEc!fZ)=CV-2XesXfy`of zMVJ292KI$djGM!O*EKc3xwF@r=%s;?JP0Xz4Y;cRb|3ox{`)?BF4<3#k|^MHb)w)u dp^Xv-LEup8O|RX}4;&PvD696WLgw{{{|^^&+ZF%- literal 0 HcmV?d00001 diff --git a/docs/Settings.md b/docs/Settings.md index b291a80a2bc..512b57e83d6 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -2542,19 +2542,9 @@ This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly --- -### mixer_pid_profile_linking - -If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup. - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - -### mixer_switch_on_land +### mixer_automated_switch -If set to on, This mixer_profile will try to switch to another mixer_profile when RTH landing is requested, Only applies if this mixer_profile is a AIRPLANE platform_type. +If set to on, This mixer_profile will try to switch to another mixer_profile when 1.RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius on mixer_profile is a MULTIROTOR or TRICOPTER platform_type. 2. RTH landing is requested on this mixer_profile is a AIRPLANE platform_type | Default | Min | Max | | --- | --- | --- | @@ -2562,9 +2552,9 @@ If set to on, This mixer_profile will try to switch to another mixer_profile whe --- -### mixer_switch_on_rth +### mixer_pid_profile_linking -If set to on, This mixer_profile will try to switch to another mixer_profile when RTH heading home is requested and distance to home is lager than 3*nav_fw_loiter_radius, Only applies if this mixer_profile is a MULTIROTOR or TRICOPTER platform_type. +If enabled, pid profile_index will follow mixer_profile index. Set to OFF(default) if you want to handle PID profile by your self. Recommend to set to ON on all mixer_profiles to let the mixer_profile handle the PID profile switching on a VTOL or mixed platform type setup. | Default | Min | Max | | --- | --- | --- | @@ -2574,7 +2564,7 @@ If set to on, This mixer_profile will try to switch to another mixer_profile whe ### mixer_switch_trans_timer -If switch another mixer_profile is scheduled by mixer_switch_on_rth or mixer_switch_on_land. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. +If switch another mixer_profile is scheduled by mixer_automated_switch or mixer_automated_switch. Activate Mixertransion motor/servo mixing for this many decisecond(0.1s) before the actual mixer_profile switch. | Default | Min | Max | | --- | --- | --- | @@ -5692,9 +5682,19 @@ See tpa_rate. --- +### tpa_on_yaw + +Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors. + +| Default | Min | Max | +| --- | --- | --- | +| OFF | OFF | ON | + +--- + ### tpa_rate -Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. +Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index f15c29d8990..7acc0c29442 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -244,6 +244,20 @@ static void timerHardwareOverride(timerHardware_t * timer) { } } +bool check_pwm_assigned_to_motor_or_servo(void) +{ + // Check TIM_USE_FW_* and TIM_USE_MC_* is consistent, If so, return true, means the pwm mapping will remain same between FW and MC + bool pwm_assigned_to_motor_or_servo = true; + for (int idx = 0; idx < timerHardwareCount; idx++) { + timerHardware_t *timHw = &timerHardware[idx]; + if (timHw->usageFlags & (TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO)) { + pwm_assigned_to_motor_or_servo &= (timHw->usageFlags == TIM_USE_VTOL_SERVO) | (timHw->usageFlags == TIM_USE_VTOL_MOTOR); + } + } + return pwm_assigned_to_motor_or_servo; +} + + bool pwmHasMotorOnTimer(timMotorServoHardware_t * timOutputs, HAL_Timer_t *tim) { for (int i = 0; i < timOutputs->maxTimMotorCount; ++i) { diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index e9b3a0f9573..5b13fe72e32 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -73,6 +73,7 @@ typedef struct { } motorProtocolProperties_t; bool pwmMotorAndServoInit(void); +bool check_pwm_assigned_to_motor_or_servo(void); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); const char * getPwmInitErrorMessage(void); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 634ab89e137..211c6f07047 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -305,7 +305,6 @@ 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/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index d600c533ebf..2d47c08ab3d 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -191,17 +191,17 @@ bool checkMixerProfileHotSwitchAvalibility(void) { return true; } -#ifdef ENABLE_MIXER_PROFILE_MCFW_HOTSWAP - bool MCFW_hotswap_available = true; +#if defined(SITL_BUILD) + bool MCFW_pwm_settings_valid = true; #else - bool MCFW_hotswap_available = false; + bool MCFW_pwm_settings_valid = check_pwm_assigned_to_motor_or_servo(); #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) + if ((!MCFW_pwm_settings_valid) && is_mcfw_switching) { allow_hot_switch = 0; return false; diff --git a/src/main/target/MATEKF405TE/CMakeLists.txt b/src/main/target/MATEKF405TE/CMakeLists.txt index d8fc13c77bc..ce0150c7d3e 100644 --- a/src/main/target/MATEKF405TE/CMakeLists.txt +++ b/src/main/target/MATEKF405TE/CMakeLists.txt @@ -1,3 +1,2 @@ target_stm32f405xg(MATEKF405TE) target_stm32f405xg(MATEKF405TE_SD) -target_stm32f405xg(MATEKF405TE_SD_VTOL) diff --git a/src/main/target/MATEKF405TE/target.c b/src/main/target/MATEKF405TE/target.c index 03c7d98807c..70561e001f3 100644 --- a/src/main/target/MATEKF405TE/target.c +++ b/src/main/target/MATEKF405TE/target.c @@ -25,22 +25,6 @@ #include "drivers/timer_def_stm32f4xx.h" timerHardware_t timerHardware[] = { -#ifdef MATEKF405TE_SD_VTOL -//With INAV firmware, DSHOT can not work on S3, S5,S7 because of DMA clash, pls use ONESHOT or MULTISHOT and calibrate ESC PWM range.<-copied from matek website - DEF_TIM(TIM8, CH4, PC9, TIM_USE_VTOL_MOTOR, 0, 0), // S1 D(2,7,7) UP217 - DEF_TIM(TIM8, CH3, PC8, TIM_USE_VTOL_MOTOR, 0, 0), // S2 D(2,2,0) UP217 - DEF_TIM(TIM1, CH3N, PB15, TIM_USE_VTOL_MOTOR, 0, 0), // S3 D(2,6,0) UP256 - DEF_TIM(TIM1, CH1, PA8, TIM_USE_VTOL_MOTOR, 0, 1), // S4 D(2,1,6) UP256 - - DEF_TIM(TIM2, CH4, PB11, TIM_USE_VTOL_SERVO, 0, 0), // S5 D(1,7,3) UP173 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_VTOL_SERVO, 0, 0), // S6 D(1,1,3) UP173 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_VTOL_SERVO, 0, 0), // S7 D(1,6,3) UP173 - DEF_TIM(TIM2, CH1, PA15, TIM_USE_VTOL_SERVO, 0, 0), // S8 D(1,5,3) UP173 - - DEF_TIM(TIM12, CH1, PB14, TIM_USE_VTOL_SERVO, 0, 0), // S9 DMA NONE - DEF_TIM(TIM13, CH1, PA6, TIM_USE_VTOL_SERVO, 0, 0), // S10 DMA NONE - DEF_TIM(TIM4, CH1, PB6, TIM_USE_VTOL_MOTOR, 0, 0), // S11 D(1,0,2) -#else DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S1 D(2,7,7) UP217 DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0, 0), // S2 D(2,2,0) UP217 DEF_TIM(TIM1, CH3N, PB15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S3 D(2,6,0) UP256 @@ -50,10 +34,10 @@ timerHardware_t timerHardware[] = { DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 D(1,1,3) UP173 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S7 D(1,6,3) UP173 DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S8 D(1,5,3) UP173 + DEF_TIM(TIM12, CH1, PB14, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S9 DMA NONE DEF_TIM(TIM13, CH1, PA6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S10 DMA NONE DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0, 0), // S11 D(1,0,2) -#endif DEF_TIM(TIM3, CH4, PB1, TIM_USE_LED, 0, 0), // 2812LED D(1,2,5) DEF_TIM(TIM11, CH1, PB9, TIM_USE_BEEPER, 0, 0), // BEEPER PWM diff --git a/src/main/target/MATEKF405TE/target.h b/src/main/target/MATEKF405TE/target.h index f4d94f6423b..acaed9daf23 100644 --- a/src/main/target/MATEKF405TE/target.h +++ b/src/main/target/MATEKF405TE/target.h @@ -18,11 +18,6 @@ #pragma once #define USE_TARGET_CONFIG -#ifdef MATEKF405TE_SD_VTOL -#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP -#define MATEKF405TE_SD -#endif - #if defined(MATEKF405TE_SD) # define TARGET_BOARD_IDENTIFIER "MF4T" # define USBD_PRODUCT_STRING "MatekF405TE_SD" diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 1e23ec8f3e1..3fbe45a5cab 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -69,7 +69,6 @@ #define USE_GPS_FAKE #define USE_RANGEFINDER_FAKE #define USE_RX_SIM -#define ENABLE_MIXER_PROFILE_MCFW_HOTSWAP #undef MAX_MIXER_PROFILE_COUNT #define MAX_MIXER_PROFILE_COUNT 2 From 49e69f0aaaa022f3c6772c2dbc92dfc33f7d95cf Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 20 Sep 2023 01:12:48 +0900 Subject: [PATCH 55/60] revert changes of pid controller --- docs/Settings.md | 34 +++------ src/main/common/maths.h | 1 - src/main/fc/controlrate_profile.c | 1 - .../fc/controlrate_profile_config_struct.h | 1 - src/main/fc/settings.yaml | 21 ++---- src/main/flight/pid.c | 75 +++++++++---------- src/main/flight/pid.h | 6 +- 7 files changed, 62 insertions(+), 77 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 70fc6305355..2ddf6249e12 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -1222,6 +1222,16 @@ Iterm is not allowed to grow when stick position is above threshold. This solves --- +### fw_iterm_throw_limit + +Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely. + +| Default | Min | Max | +| --- | --- | --- | +| 165 | FW_ITERM_THROW_LIMIT_MIN | FW_ITERM_THROW_LIMIT_MAX | + +--- + ### fw_level_pitch_gain I-gain for the pitch trim for self-leveling flight modes. Higher values means that AUTOTRIM will be faster but might introduce oscillations @@ -1914,7 +1924,7 @@ Calculated 1G of Acc axis Z to use in INS ### iterm_windup -Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached) +Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | Default | Min | Max | | --- | --- | --- | @@ -4762,16 +4772,6 @@ Video system used. Possible values are `AUTO`, `PAL`, `NTSC`, `HDZERO`, 'DJIWTF' --- -### pid_iterm_limit_percent - -Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely. - -| Default | Min | Max | -| --- | --- | --- | -| 33 | 0 | 200 | - ---- - ### pid_type Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` @@ -5672,19 +5672,9 @@ See tpa_rate. --- -### tpa_on_yaw - -Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors. - -| Default | Min | Max | -| --- | --- | --- | -| OFF | OFF | ON | - ---- - ### tpa_rate -Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. +Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | Default | Min | Max | | --- | --- | --- | diff --git a/src/main/common/maths.h b/src/main/common/maths.h index fc8dbe33295..cc3d1bc5176 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -91,7 +91,6 @@ ) #define MIN(a, b) _CHOOSE(<, a, b) #define MAX(a, b) _CHOOSE(>, a, b) -#define SIGN(a) ((a >= 0) ? 1 : -1) #define _ABS_II(x, var) \ ( __extension__ ({ \ diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index c8b144e136c..18f5bd343b6 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -44,7 +44,6 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) .rcMid8 = SETTING_THR_MID_DEFAULT, .rcExpo8 = SETTING_THR_EXPO_DEFAULT, .dynPID = SETTING_TPA_RATE_DEFAULT, - .dynPID_on_YAW = SETTING_TPA_ON_YAW_DEFAULT, .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT }, diff --git a/src/main/fc/controlrate_profile_config_struct.h b/src/main/fc/controlrate_profile_config_struct.h index e4038537603..e07c328560c 100644 --- a/src/main/fc/controlrate_profile_config_struct.h +++ b/src/main/fc/controlrate_profile_config_struct.h @@ -29,7 +29,6 @@ typedef struct controlRateConfig_s { uint8_t rcMid8; uint8_t rcExpo8; uint8_t dynPID; - bool dynPID_on_YAW; uint16_t pa_breakpoint; // Breakpoint where TPA is activated uint16_t fixedWingTauMs; // Time constant of airplane TPA PT1-filter } throttle; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 036ca7d5c95..bfa596c7812 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1267,7 +1267,7 @@ groups: min: 0 max: 100 - name: tpa_rate - description: "Throttle PID attenuation reduces influence of PDFF on ROLL and PITCH of multi-rotor, PIDFF on ROLL,PITCH,YAW OF fixed_wing as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." + description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." default_value: 0 field: throttle.dynPID min: 0 @@ -1278,11 +1278,6 @@ groups: field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX - - name: tpa_on_yaw - description: "Throttle PID attenuation also reduces influence on YAW for multi-rotor, Should be set to ON for tilting rotors." - type: bool - field: throttle.dynPID_on_YAW - default_value: OFF - name: fw_tpa_time_constant description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. See **PID Attenuation and scaling** Wiki for full details." default_value: 1500 @@ -1856,6 +1851,12 @@ groups: default_value: 0 min: 0 max: 200 + - name: fw_iterm_throw_limit + description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." + default_value: 165 + field: fixedWingItermThrowLimit + min: FW_ITERM_THROW_LIMIT_MIN + max: FW_ITERM_THROW_LIMIT_MAX - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." default_value: 1500 @@ -1899,17 +1900,11 @@ groups: min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup - description: "Used to prevent Iterm accumulation on during maneuvers. Iterm accumulation will be dampened when motors are reaching it's limit (when requested motor correction range is close percentage specified by this parameter, when value is set below 50 no accumulation occors when iterm_windup+50 of motor correction range is reached)" + description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)" default_value: 50 field: itermWindupPointPercent min: 0 max: 90 - - name: pid_iterm_limit_percent - description: "Limits max/min I-term value in stabilization PID controller. It solves the problem of servo saturation before take-off/throwing the airplane into the air. Or multirotors with low authority. By default, error accumulated in I-term can not exceed 33% of total pid throw (around 165us on deafult pidsum_limit of pitch/roll). Set 0 to disable completely." - default_value: 33 - field: pidItermLimitPercent - min: 0 - max: 200 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." default_value: 0 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 03458c58420..2bb4ab70358 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -79,6 +79,7 @@ typedef struct { // Rate integrator float errorGyroIf; + float errorGyroIfLimit; // Used for ANGLE filtering (PT1, we don't need super-sharpness here) pt1Filter_t angleFilterState; @@ -100,6 +101,7 @@ typedef struct { #endif uint16_t pidSumLimit; filterApply4FnPtr ptermFilterApplyFn; + bool itermLimitActive; bool itermFreezeActive; pt3Filter_t rateTargetFilter; @@ -147,7 +149,6 @@ static EXTENDED_FASTRAM float dBoostMaxAtAlleceleration; static EXTENDED_FASTRAM uint8_t yawLpfHz; static EXTENDED_FASTRAM float motorItermWindupPoint; -static EXTENDED_FASTRAM float motorItermWindupPoint_inv; static EXTENDED_FASTRAM float antiWindupScaler; #ifdef USE_ANTIGRAVITY static EXTENDED_FASTRAM float iTermAntigravityGain; @@ -263,8 +264,8 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, - .pidItermLimitPercent = SETTING_PID_ITERM_LIMIT_PERCENT_DEFAULT, + .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, @@ -379,12 +380,14 @@ void pidResetErrorAccumulators(void) // Reset R/P/Y integrator for (int axis = 0; axis < 3; axis++) { pidState[axis].errorGyroIf = 0.0f; + pidState[axis].errorGyroIfLimit = 0.0f; } } void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) { pidState[axis].errorGyroIf -= delta; + pidState[axis].errorGyroIfLimit -= delta; } float getTotalRateTarget(void) @@ -528,7 +531,7 @@ void updatePIDCoefficients(void) pidState[axis].kT = 0.0f; } else { - const float axisTPA = (axis == FD_YAW && (!currentControlRateProfile->throttle.dynPID_on_YAW)) ? 1.0f : tpaFactor; + const float axisTPA = (axis == FD_YAW) ? 1.0f : tpaFactor; pidState[axis].kP = pidBank()->pid[axis].P / FP_PID_RATE_P_MULTIPLIER * axisTPA; pidState[axis].kI = pidBank()->pid[axis].I / FP_PID_RATE_I_MULTIPLIER; pidState[axis].kD = pidBank()->pid[axis].D / FP_PID_RATE_D_MULTIPLIER * axisTPA; @@ -619,7 +622,7 @@ static void pidLevel(const float angleTarget, pidState_t *pidState, flight_dynam angleErrorDeg = DECIDEGREES_TO_DEGREES((float)angleFreefloatDeadband(DEGREES_TO_DECIDEGREES(navConfig()->fw.soaring_pitch_deadband), FD_PITCH)); if (!angleErrorDeg) { pidState->errorGyroIf = 0.0f; - // pidState->errorGyroIfLimit = 0.0f; + pidState->errorGyroIfLimit = 0.0f; } } @@ -725,6 +728,14 @@ static float dTermProcess(pidState_t *pidState, float currentRateTarget, float d return(newDTerm); } +static void applyItermLimiting(pidState_t *pidState) { + if (pidState->itermLimitActive) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidState->errorGyroIfLimit, pidState->errorGyroIfLimit); + } else + { + pidState->errorGyroIfLimit = fabsf(pidState->errorGyroIf); + } +} static void nullRateController(pidState_t *pidState, flight_dynamics_index_t axis, float dT, float dT_inv) { UNUSED(pidState); @@ -749,9 +760,10 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf += rateError * pidState->kI * dT; } - if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); + applyItermLimiting(pidState); + + if (pidProfile()->fixedWingItermThrowLimit != 0) { + pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); @@ -788,7 +800,7 @@ static float FAST_CODE applyItermRelax(const int axis, float currentPidSetpoint, const float setpointLpf = pt1FilterApply(&windupLpf[axis], currentPidSetpoint); const float setpointHpf = fabsf(currentPidSetpoint - setpointLpf); - const float itermRelaxFactor = MAX(0, 1 - setpointHpf / (FLIGHT_MODE(ANGLE_MODE) ? (MC_ITERM_RELAX_SETPOINT_THRESHOLD * 0.2f):MC_ITERM_RELAX_SETPOINT_THRESHOLD)); + const float itermRelaxFactor = MAX(0, 1 - setpointHpf / MC_ITERM_RELAX_SETPOINT_THRESHOLD); return itermErrorRate * itermRelaxFactor; } } @@ -817,30 +829,17 @@ static void FAST_CODE NOINLINE pidApplyMulticopterRateController(pidState_t *pid const float newOutput = newPTerm + newDTerm + pidState->errorGyroIf + newCDTerm; const float newOutputLimited = constrainf(newOutput, -pidState->pidSumLimit, +pidState->pidSumLimit); - float backCalc = newOutputLimited - newOutput;//back-calculation anti-windup - if (SIGN(backCalc) == SIGN(pidState->errorGyroIf)) { - //back calculation anti-windup can only shrink integrator, will not push it to the opposite direction - backCalc = 0.0f; - } float itermErrorRate = applyItermRelax(axis, rateTarget, rateError); #ifdef USE_ANTIGRAVITY itermErrorRate *= iTermAntigravityGain; #endif - if (!pidState->itermFreezeActive) { - pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) - + (backCalc * pidState->kT * antiWindupScaler * dT); - } - - if (pidProfile()->pidItermLimitPercent != 0){ - float itermLimit = pidState->pidSumLimit * pidProfile()->pidItermLimitPercent * 0.01f; - pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -itermLimit, +itermLimit); - } - + pidState->errorGyroIf += (itermErrorRate * pidState->kI * antiWindupScaler * dT) + + ((newOutputLimited - newOutput) * pidState->kT * antiWindupScaler * dT); // Don't grow I-term if motors are at their limit - // applyItermLimiting(pidState); //merged with itermFreezeActive + applyItermLimiting(pidState); axisPID[axis] = newOutputLimited; @@ -1027,34 +1026,35 @@ static void pidApplyFpvCameraAngleMix(pidState_t *pidState, uint8_t fpvCameraAng pidState[YAW].rateTarget = constrainf(yawRate * cosCameraAngle + rollRate * sinCameraAngle, -GYRO_SATURATION_LIMIT, GYRO_SATURATION_LIMIT); } -bool checkItermLimitingActive(pidState_t *pidState) +void checkItermLimitingActive(pidState_t *pidState) { - bool shouldActivate=false; + bool shouldActivate; if (usedPidControllerType == PID_TYPE_PIFF) { shouldActivate = isFixedWingItermLimitActive(pidState->stickPosition); } else { - shouldActivate = mixerIsOutputSaturated(); //just in case, since it is already managed by itermWindupPointPercent + shouldActivate = mixerIsOutputSaturated(); } - return STATE(ANTI_WINDUP) || shouldActivate; + pidState->itermLimitActive = STATE(ANTI_WINDUP) || shouldActivate; } void checkItermFreezingActive(pidState_t *pidState, flight_dynamics_index_t axis) { - bool shouldActivate=false; if (usedPidControllerType == PID_TYPE_PIFF && pidProfile()->fixedWingYawItermBankFreeze != 0 && axis == FD_YAW) { // Do not allow yaw I-term to grow when bank angle is too large float bankAngle = DECIDEGREES_TO_DEGREES(attitude.values.roll); if (fabsf(bankAngle) > pidProfile()->fixedWingYawItermBankFreeze && !(FLIGHT_MODE(AUTO_TUNE) || FLIGHT_MODE(TURN_ASSISTANT) || navigationRequiresTurnAssistance())){ - shouldActivate |= true; + pidState->itermFreezeActive = true; } else { - shouldActivate |= false; + pidState->itermFreezeActive = false; } + } else + { + pidState->itermFreezeActive = false; } - shouldActivate |=checkItermLimitingActive(pidState); - pidState->itermFreezeActive = shouldActivate; + } void FAST_CODE pidController(float dT) @@ -1134,14 +1134,15 @@ void FAST_CODE pidController(float dT) pidApplyFpvCameraAngleMix(pidState, currentControlRateProfile->misc.fpvCamAngleDegrees); } - // Prevent Iterm accumulation when motors are near its saturation - antiWindupScaler = constrainf((1.0f - getMotorMixRange() * MAX(2*motorItermWindupPoint,1)) * motorItermWindupPoint_inv, 0.0f, 1.0f); + // Prevent strong Iterm accumulation during stick inputs + antiWindupScaler = constrainf((1.0f - getMotorMixRange()) * motorItermWindupPoint, 0.0f, 1.0f); for (int axis = 0; axis < 3; axis++) { // Apply setpoint rate of change limits pidApplySetpointRateLimiting(&pidState[axis], axis, dT); // Step 4: Run gyro-driven control + checkItermLimitingActive(&pidState[axis]); checkItermFreezingActive(&pidState[axis], axis); pidControllerApplyFn(&pidState[axis], axis, dT, dT_inv); @@ -1176,9 +1177,7 @@ void pidInit(void) itermRelax = pidProfile()->iterm_relax; yawLpfHz = pidProfile()->yaw_lpf_hz; - motorItermWindupPoint = 1.0f - (pidProfile()->itermWindupPointPercent / 100.0f); - motorItermWindupPoint_inv = 1.0f / motorItermWindupPoint; - + motorItermWindupPoint = 1.0f / (1.0f - (pidProfile()->itermWindupPointPercent / 100.0f)); #ifdef USE_D_BOOST dBoostMin = pidProfile()->dBoostMin; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7c61b7cec21..de0e3126b7c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -31,6 +31,10 @@ #define HEADING_HOLD_RATE_LIMIT_MAX 250 #define HEADING_HOLD_RATE_LIMIT_DEFAULT 90 +#define FW_ITERM_THROW_LIMIT_DEFAULT 165 +#define FW_ITERM_THROW_LIMIT_MIN 0 +#define FW_ITERM_THROW_LIMIT_MAX 500 + #define AXIS_ACCEL_MIN_LIMIT 50 #define HEADING_HOLD_ERROR_LPF_FREQ 2 @@ -117,9 +121,9 @@ typedef struct pidProfile_s { uint16_t pidSumLimit; uint16_t pidSumLimitYaw; - uint16_t pidItermLimitPercent; // Airplane-specific parameters + uint16_t fixedWingItermThrowLimit; float fixedWingReferenceAirspeed; // Reference tuning airspeed for the airplane - the speed for which PID gains are tuned float fixedWingCoordinatedYawGain; // This is the gain of the yaw rate required to keep the yaw rate consistent with the turn rate for a coordinated turn. float fixedWingCoordinatedPitchGain; // This is the gain of the pitch rate to keep the pitch angle constant during coordinated turns. From 24ffa37b3a18500f7d8a8db1254c323a1431ce25 Mon Sep 17 00:00:00 2001 From: shota Date: Wed, 20 Sep 2023 20:01:26 +0900 Subject: [PATCH 56/60] minor fix --- src/main/drivers/pwm_mapping.c | 2 +- src/main/drivers/pwm_mapping.h | 2 +- src/main/fc/fc_msp.c | 6 ++++-- src/main/flight/mixer_profile.c | 2 +- src/main/flight/mixer_profile.h | 1 - 5 files changed, 7 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/pwm_mapping.c b/src/main/drivers/pwm_mapping.c index d502fd6c141..54ee0ad121b 100644 --- a/src/main/drivers/pwm_mapping.c +++ b/src/main/drivers/pwm_mapping.c @@ -228,7 +228,7 @@ static void timerHardwareOverride(timerHardware_t * timer) { } } -bool check_pwm_assigned_to_motor_or_servo(void) +bool checkPwmAssignedToMotorOrServo(void) { // Check TIM_USE_FW_* and TIM_USE_MC_* is consistent, If so, return true, means the pwm mapping will remain same between FW and MC bool pwm_assigned_to_motor_or_servo = true; diff --git a/src/main/drivers/pwm_mapping.h b/src/main/drivers/pwm_mapping.h index 5b13fe72e32..155975d004b 100644 --- a/src/main/drivers/pwm_mapping.h +++ b/src/main/drivers/pwm_mapping.h @@ -73,7 +73,7 @@ typedef struct { } motorProtocolProperties_t; bool pwmMotorAndServoInit(void); -bool check_pwm_assigned_to_motor_or_servo(void); +bool checkPwmAssignedToMotorOrServo(void); const motorProtocolProperties_t * getMotorProtocolProperties(motorPwmProtocolTypes_e proto); pwmInitError_e getPwmInitError(void); const char * getPwmInitErrorMessage(void); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5a363bdff07..8b62fdba391 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1466,7 +1466,8 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP2_INAV_MIXER: sbufWriteU8(dst, mixerConfig()->motorDirectionInverted); - sbufWriteU16(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, mixerConfig()->motorstopOnLow); sbufWriteU8(dst, mixerConfig()->platformType); sbufWriteU8(dst, mixerConfig()->hasFlaps); sbufWriteU16(dst, mixerConfig()->appliedMixerPreset); @@ -2867,7 +2868,8 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_MIXER: if (dataSize == 9) { mixerConfigMutable()->motorDirectionInverted = sbufReadU8(src); - sbufReadU16(src); // Was yaw_jump_prevention_limit + sbufReadU8(src); // Was yaw_jump_prevention_limit + mixerConfigMutable()->motorstopOnLow = sbufReadU8(src); mixerConfigMutable()->platformType = sbufReadU8(src); mixerConfigMutable()->hasFlaps = sbufReadU8(src); mixerConfigMutable()->appliedMixerPreset = sbufReadU16(src); diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 5901c3419e6..3248c13b2d4 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -193,7 +193,7 @@ bool checkMixerProfileHotSwitchAvalibility(void) #if defined(SITL_BUILD) bool MCFW_pwm_settings_valid = true; #else - bool MCFW_pwm_settings_valid = check_pwm_assigned_to_motor_or_servo(); + bool MCFW_pwm_settings_valid = checkPwmAssignedToMotorOrServo(); #endif uint8_t platform_type0 = mixerConfigByIndex(0)->platformType; uint8_t platform_type1 = mixerConfigByIndex(1)->platformType; diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h index c833651c214..cb040665ce8 100644 --- a/src/main/flight/mixer_profile.h +++ b/src/main/flight/mixer_profile.h @@ -14,7 +14,6 @@ typedef struct mixerConfig_s { uint8_t platformType; bool hasFlaps; int16_t appliedMixerPreset; - uint8_t outputMode; bool motorstopOnLow; bool PIDProfileLinking; bool automated_switch; From e4984a52929d982d9dec1bed1105f238aaaf1b5d Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 23 Sep 2023 01:27:07 +0900 Subject: [PATCH 57/60] minor fix --- src/main/flight/mixer_profile.c | 51 +++++---------------------------- 1 file changed, 7 insertions(+), 44 deletions(-) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 3248c13b2d4..5010b65031c 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -106,7 +106,6 @@ void setMixerProfileAT(void) bool checkMixerATRequired(mixerProfileATRequest_e required_action) { //return false if mixerAT condition is not required or setting is not valid - //on non vtol setups , behave as normal if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) { return false; @@ -131,6 +130,7 @@ bool checkMixerATRequired(mixerProfileATRequest_e required_action) bool mixerATUpdateState(mixerProfileATRequest_e required_action) { + //return true if mixerAT is done or not required bool reprocessState; do { @@ -175,66 +175,30 @@ bool mixerATUpdateState(mixerProfileATRequest_e required_action) 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; - } -#if defined(SITL_BUILD) - bool MCFW_pwm_settings_valid = true; -#else - bool MCFW_pwm_settings_valid = checkPwmAssignedToMotorOrServo(); -#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_pwm_settings_valid) && is_mcfw_switching) - { - allow_hot_switch = 0; - return false; - } - allow_hot_switch = 1; return true; } -bool isNavBoxModesEnabled(void) -{ - return 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))); -} - void outputProfileUpdateTask(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - bool nav_mixerAT_inuse = (posControl.navState == NAV_STATE_MIXERAT_IN_PROGRESS || posControl.navState == NAV_STATE_MIXERAT_ABORT); + bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; // transition mode input for servo mix and motor mix - if (!FLIGHT_MODE(FAILSAFE_MODE) && (!nav_mixerAT_inuse)) + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { - if (!isNavBoxModesEnabled()) - { - outputProfileHotSwitch((int)IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); - } - isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION) && (!isNavBoxModesEnabled()); // update BOXMIXERTRANSITION_input + outputProfileHotSwitch((int)IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); + isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); } - isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || nav_mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); + isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); } // switch mixerprofile without reboot bool outputProfileHotSwitch(int profile_index) { 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) { @@ -250,7 +214,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,TODO + { // it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D return false; } if (!checkMixerProfileHotSwitchAvalibility()) @@ -268,7 +232,6 @@ bool outputProfileHotSwitch(int profile_index) // LOG_INFO(PWM, "mixer switch failed to set config"); return false; } - // stopMotorsNoDelay(); // not necessary, but just in case something goes wrong. But will a short period of stop command cause problem? mixerConfigInit(); return true; } From 8328870a454bddb3b13aa6770b49c18821677b09 Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 23 Sep 2023 22:09:28 +0900 Subject: [PATCH 58/60] minor fix --- docs/MixerProfile.md | 2 +- src/main/flight/mixer_profile.c | 4 +++- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/docs/MixerProfile.md b/docs/MixerProfile.md index cbb9bcba225..f6688feaeec 100644 --- a/docs/MixerProfile.md +++ b/docs/MixerProfile.md @@ -106,7 +106,7 @@ Profile files Switching is not available until the runtime sensor calibration is `mixer_profile` 1 will be used as default, `mixer_profile` 2 will be used when the `MIXER PROFILE 2` mode box is activated. Once successfully set, you can see the profiles / model preview etc. will switch accordingly when you view the relevant INAV Configurator tabs. Checking these tabs in the INAV Configurator will help make the setup easier. -Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos, Here is sample of using the `MIXER TRANSITION` mode: +Set `MIXER TRANSITION` accordingly when you want to use `MIXER TRANSITION` input for motors and servos. Here is sample of using the `MIXER TRANSITION` mode: ![Alt text](Screenshots/mixer_profile.png) diff --git a/src/main/flight/mixer_profile.c b/src/main/flight/mixer_profile.c index 5010b65031c..3b0b1fd18f3 100644 --- a/src/main/flight/mixer_profile.c +++ b/src/main/flight/mixer_profile.c @@ -189,7 +189,9 @@ void outputProfileUpdateTask(timeUs_t currentTimeUs) // transition mode input for servo mix and motor mix if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) { - outputProfileHotSwitch((int)IS_RC_MODE_ACTIVE(BOXMIXERPROFILE)); + if (isModeActivationConditionPresent(BOXMIXERPROFILE)){ + outputProfileHotSwitch(IS_RC_MODE_ACTIVE(BOXMIXERPROFILE) == 0 ? 0 : 1); + } isMixerTransitionMixing_requested = IS_RC_MODE_ACTIVE(BOXMIXERTRANSITION); } isMixerTransitionMixing = isMixerTransitionMixing_requested && ((posControl.navState == NAV_STATE_IDLE) || mixerAT_inuse ||(posControl.navState == NAV_STATE_ALTHOLD_IN_PROGRESS)); From 0e93fce97fde139b117863eb4ada718a0989a11d Mon Sep 17 00:00:00 2001 From: shota Date: Sat, 23 Sep 2023 23:21:24 +0900 Subject: [PATCH 59/60] Revert "refactoring" This reverts commit 1d0d5be21ef932b0848f1e04074aba427bcc9f23. --- src/main/flight/servos.c | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index fc0147ca3aa..4d4bb497d19 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -104,6 +104,7 @@ int16_t servo[MAX_SUPPORTED_SERVOS]; static uint8_t servoRuleCount = 0; static servoMixer_t currentServoMixer[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT]; +static bool currentServoMixerActivative[MAX_SERVO_RULES*MAX_MIXER_PROFILE_COUNT];// if true, the rule is used by current servo mixer static bool servoOutputEnabled; static bool mixerUsesServos; @@ -193,7 +194,7 @@ void loadCustomServoMixer(void) } memcpy(¤tServoMixer[servoRuleCount], &tmp_customServoMixers[i], sizeof(servoMixer_t)); - currentServoMixer[servoRuleCount].rate = (j==currentMixerProfileIndex) ? currentServoMixer[servoRuleCount].rate : 0; //set rate to 0 if not active profile + currentServoMixerActivative[servoRuleCount] = j==currentMixerProfileIndex; servoRuleCount++; } } @@ -352,6 +353,9 @@ void servoMixer(float dT) inputRaw = 0; } #endif + if (!currentServoMixerActivative[i]) { + inputRaw = 0; + } /* * Apply mixer speed limit. 1 [one] speed unit is defined as 10us/s: * 0 = no limiting @@ -434,7 +438,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (currentServoMixer[i].rate==0) {continue;} + if (!currentServoMixerActivative[i]) {continue;} // Reset servo middle accumulator const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; @@ -457,7 +461,7 @@ void processServoAutotrimMode(void) if (ARMING_FLAG(ARMED)) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (currentServoMixer[i].rate==0) {continue;} + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -470,7 +474,7 @@ void processServoAutotrimMode(void) if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (currentServoMixer[i].rate==0) {continue;} + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -504,7 +508,7 @@ void processServoAutotrimMode(void) if (trimState == AUTOTRIM_SAVE_PENDING) { for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { - if (currentServoMixer[i].rate==0) {continue;} + if (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { From e108558601563fb1baa280a49786061facd9db84 Mon Sep 17 00:00:00 2001 From: shota Date: Tue, 26 Sep 2023 22:12:50 +0900 Subject: [PATCH 60/60] add MIXER_TRANSITION_ACTIVE OPERAND --- src/main/programming/logic_condition.c | 4 ++++ src/main/programming/logic_condition.h | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 6a08871508d..d3b45453c91 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -776,6 +776,10 @@ static int logicConditionGetFlightOperandValue(int operand) { return currentMixerProfileIndex + 1; break; + case LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE: //0,1 + return isMixerTransitionMixing ? 1 : 0; + break; + case LOGIC_CONDITION_OPERAND_FLIGHT_LOITER_RADIUS: return getLoiterRadius(navConfig()->fw.loiter_radius); diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 446532fb291..941e47f8d0d 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -135,7 +135,8 @@ 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 + LOGIC_CONDITION_OPERAND_FLIGHT_ACTIVE_MIXER_PROFILE, //int // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 40 } logicFlightOperands_e; typedef enum {