diff --git a/.gitignore b/.gitignore index c86d809162b..b2c3b9ff546 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/docs/MixerProfile.md b/docs/MixerProfile.md new file mode 100644 index 00000000000..f6688feaeec --- /dev/null +++ b/docs/MixerProfile.md @@ -0,0 +1,144 @@ +# MixerProfile + +A MixerProfile is a set of motor mixer, servo-mixer and platform type configuration settings to enable VTOL transitions. + +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 +- A VTOL specific FC target or `timer_output_mode` overrides was required in the early stage of the development, But since unified mapping introduced in INAV 7.0 It is not needed anymore. +- ~~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 set the `timer_output_mode` overrides to allow a consistent enumeration and thus mapping between MR and FW modes.~~ +- ~~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.~~ +## Configuration +### Timer overrides +Set the timer overrides for the outputs that you are intended to use. +For SITL builds, is not necessary to set timer overrides. +Please note that there are some display issues on the configurator that will show wrong mapping on the mixer_profile which has less motor/servo compared with the another +### 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 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). + +The following 2 `mixer_profile` sections are added in the CLI: + +``` +#switch to mixer_profile by cli +mixer_profile 1 + +set platform_type = AIRPLANE +set model_preview_type = 26 +# 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 aeroplane setting on mixer_profile 1 + +``` +mixer_profile 2 + +set platform_type = TRICOPTER +set model_preview_type = 1 +# also enable pid_profile auto handling +set mixer_pid_profile_linking = ON +save +``` +Then finish the multi-rotor setting on `mixer_profile` 2. + +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 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. + +### Mixer Transition input + +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 + +#### 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: + +``` +# rule no; servo index; input source; rate; speed; activate logic function number +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. 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 + +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= 0 && i < MAX_MIXER_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); + cliPrintHashLine("Mixer: motor mixer"); + cliDumpPrintLinef(dumpMask, primaryMotorMixer_CopyArray()[0].throttle == 0.0f, "\r\nmmix reset\r\n"); + printMotorMix(dumpMask, primaryMotorMixer_CopyArray(), primaryMotorMixer(0)); + cliPrintHashLine("Mixer: servo mixer"); + cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray()[0].rate == 0, "smix reset\r\n"); + printServoMix(dumpMask, customServoMixers_CopyArray(), customServoMixers(0)); +} + #ifdef USE_CLI_BATCH static void cliPrintCommandBatchWarning(const char *warning) { @@ -3687,6 +3720,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 { @@ -3699,12 +3734,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 @@ -3738,16 +3775,6 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("Timer overrides"); printTimerOutputModes(dumpMask, timerOverrides_CopyArray, timerOverrides(0), -1); - cliPrintHashLine("Mixer: motor 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("Mixer: servo mixer"); - cliDumpPrintLinef(dumpMask, customServoMixers_CopyArray[0].rate == 0, "smix reset\r\n"); - printServoMix(dumpMask, customServoMixers_CopyArray, customServoMixers(0)); - // print servo parameters cliPrintHashLine("Outputs [servo]"); printServo(dumpMask, servoParams_CopyArray, servoParams(0)); @@ -3829,6 +3856,10 @@ 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_MIXER_PROFILE_COUNT; ++ii) { + cliDumpMixerProfile(ii, dumpMask); + } for (int ii = 0; ii < MAX_PROFILE_COUNT; ++ii) { cliDumpProfile(ii, dumpMask); } @@ -3837,8 +3868,10 @@ static void printConfig(const char *cmdline, bool doDiff) } setConfigProfile(currentProfileIndexSave); setConfigBatteryProfile(currentBatteryProfileIndexSave); + setConfigMixerProfile(currentMixerProfileIndexSave); cliPrintHashLine("restore original profile selection"); + cliPrintLinef("mixer_profile %d", currentMixerProfileIndexSave + 1); cliPrintLinef("profile %d", currentProfileIndexSave + 1); cliPrintLinef("battery_profile %d", currentBatteryProfileIndexSave + 1); @@ -3847,11 +3880,15 @@ static void printConfig(const char *cmdline, bool doDiff) #endif } else { // dump just the current profiles + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); cliDumpProfile(getConfigProfile(), dumpMask); cliDumpBatteryProfile(getConfigBatteryProfile(), dumpMask); } } - + if (dumpMask & DUMP_MIXER_PROFILE) { + cliDumpMixerProfile(getConfigMixerProfile(), dumpMask); + } + if (dumpMask & DUMP_PROFILE) { cliDumpProfile(getConfigProfile(), dumpMask); } @@ -4011,6 +4048,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 3b884db7474..27ed9eddd00 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) @@ -330,6 +331,7 @@ void readEEPROM(void) setConfigProfile(getConfigProfile()); setConfigBatteryProfile(getConfigBatteryProfile()); + setConfigMixerProfile(getConfigMixerProfile()); validateAndFixConfig(); activateConfig(); @@ -469,6 +471,37 @@ 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 + suspendRxSignal(); + writeEEPROM(); + readEEPROM(); + resumeRxSignal(); + } + beeperConfirmationBeeps(profileIndex + 1); +} + void setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]) { gyroConfigMutable()->gyro_zero_cal[X] = (int16_t) getGyroZero[X]; diff --git a/src/main/fc/config.h b/src/main/fc/config.h index e32a705e1a1..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_MOTOR_STOP = 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, @@ -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) @@ -138,6 +139,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 setGyroCalibration(float getGyroZero[XYZ_AXIS_COUNT]); void setGravityCalibration(float getGravity); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 8cf5a5ce107..87d9fbe7461 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -81,6 +81,7 @@ #include "telemetry/telemetry.h" +#include "flight/mixer_profile.h" #include "flight/mixer.h" #include "flight/servos.h" #include "flight/pid.h" @@ -618,7 +619,7 @@ void processRx(timeUs_t currentTimeUs) const bool throttleIsLow = throttleStickIsLow(); // 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 (throttleIsLow) { @@ -705,14 +706,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 } } @@ -745,8 +746,8 @@ void processRx(timeUs_t currentTimeUs) } else if (rcControlsConfig()->airmodeHandlingType == STICK_CENTER) { if (throttleIsLow) { - if (STATE(AIRMODE_ACTIVE) && !failsafeIsActive()) { - if ((rollPitchStatus == CENTERED) || (feature(FEATURE_MOTOR_STOP) && !STATE(FIXED_WING_LEGACY))) { + if (STATE(AIRMODE_ACTIVE)) { + if ((rollPitchStatus == CENTERED) || (ifMotorstopFeatureEnabled() && !STATE(FIXED_WING_LEGACY))) { ENABLE_STATE(ANTI_WINDUP); } else { @@ -764,7 +765,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); } @@ -792,7 +793,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 1b6d2df4715..211c6f07047 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -304,9 +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 - servosInit(); - mixerUpdateStateFlags(); - mixerInit(); + mixerConfigInit(); // Some sanity checking if (motorConfig()->motorPwmProtocol == PWM_TYPE_BRUSHED) { diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index a2bc170b0a1..8b62fdba391 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -74,6 +74,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" @@ -1465,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); @@ -2866,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); @@ -3230,6 +3233,10 @@ 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) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 59aba5f8fe9..f7c88d6e5b0 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" @@ -99,6 +100,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { .boxId = BOXCHANGEMISSION, .boxName = "MISSION CHANGE", .permanentId = 59 }, { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 60 }, { .boxId = BOXMULTIFUNCTION, .boxName = "MULTI FUNCTION", .permanentId = 61 }, + { .boxId = BOXMIXERPROFILE, .boxName = "MIXER PROFILE 2", .permanentId = 62 }, + { .boxId = BOXMIXERTRANSITION, .boxName = "MIXER TRANSITION", .permanentId = 63 }, { .boxId = CHECKBOX_ITEM_COUNT, .boxName = NULL, .permanentId = 0xFF } }; @@ -350,6 +353,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) @@ -418,6 +426,10 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #endif #ifdef USE_MULTI_FUNCTIONS CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMULTIFUNCTION)), BOXMULTIFUNCTION); +#endif +#if (MAX_MIXER_PROFILE_COUNT > 1) + 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/rc_controls.c b/src/main/fc/rc_controls.c index c396fa8814f..8a03ed81284 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -51,6 +51,7 @@ #include "flight/pid.h" #include "flight/failsafe.h" +#include "flight/mixer.h" #include "io/gps.h" #include "io/beeper.h" @@ -213,7 +214,7 @@ void processRcStickPositions(bool isThrottleLow) // perform actions bool armingSwitchIsActive = IS_RC_MODE_ACTIVE(BOXARM); - 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 (!isThrottleLow) { tryArm(); diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 674fdded8d2..6b56fa556b1 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -79,6 +79,8 @@ typedef enum { BOXCHANGEMISSION = 50, BOXBEEPERMUTE = 51, BOXMULTIFUNCTION = 52, + BOXMIXERPROFILE = 53, + BOXMIXERTRANSITION = 54, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.c b/src/main/fc/settings.c index 8dc17edc631..8ea69b82f09 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(mixerProfile_t) * getConfigMixerProfile(); } return 0; } diff --git a/src/main/fc/settings.h b/src/main/fc/settings.h index b14289ba27c..7fdfd66bf25 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_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,11 +29,12 @@ typedef enum { } setting_type_e; typedef enum { - // value section, bits 4-5 + // 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 { @@ -42,8 +43,9 @@ typedef enum { MODE_LOOKUP = (1 << SETTING_MODE_OFFSET), // 0x40 } setting_mode_e; -#define SETTING_TYPE_MASK (0x0F) -#define SETTING_SECTION_MASK (0x30) + +#define SETTING_TYPE_MASK (0x07) +#define SETTING_SECTION_MASK (0x38) #define SETTING_MODE_MASK (0xC0) typedef struct settingMinMaxConfig_s { diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2602868c4db..bfa596c7812 100755 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1134,31 +1134,56 @@ groups: field: powerLimits.burstPowerFalldownTime max: 3000 - - name: PG_MIXER_CONFIG - type: mixerConfig_t + - name: PG_MIXER_PROFILE + type: mixerProfile_t + headers: ["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: motorstop_on_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. 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_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.automated_switch + type: bool + - name: mixer_switch_trans_timer + 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 + max: 200 + + - name: PG_REVERSIBLE_MOTORS_CONFIG type: reversibleMotorsConfig_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 771a22b3215..35674c1f5dc 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -32,6 +32,7 @@ #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,15 +83,6 @@ PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, .neutral = SETTING_3D_NEUTRAL_DEFAULT ); -PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 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 -); - PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 10); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, @@ -100,9 +92,6 @@ PG_RESET_TEMPLATE(motorConfig_t, motorConfig, .mincommand = SETTING_MIN_COMMAND_DEFAULT, .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); - PG_REGISTER_ARRAY_WITH_RESET_FN(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides, PG_TIMER_OVERRIDE_CONFIG, 0); #define CRASH_OVER_AFTER_CRASH_FLIP_STICK_MIN 0.15f @@ -125,14 +114,29 @@ 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){ + return currentMixerConfig.motorstopOnLow; } uint8_t getMotorCount(void) { @@ -158,35 +162,44 @@ 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); } + 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) @@ -208,7 +221,7 @@ void mixerInit(void) mixerResetDisarmedMotors(); - if (mixerConfig()->motorDirectionInverted) { + if (currentMixerConfig.motorDirectionInverted) { motorYawMultiplier = -1; } else { motorYawMultiplier = 1; @@ -231,7 +244,7 @@ void mixerResetDisarmedMotors(void) reversibleMotorsThrottleState = MOTOR_DIRECTION_FORWARD; - if (feature(FEATURE_MOTOR_STOP)) { + if (ifMotorstopFeatureEnabled()) { motorValueWhenStopped = motorZeroCommand; } else { motorValueWhenStopped = throttleIdleValue; @@ -289,7 +302,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); @@ -468,7 +481,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(void) @@ -614,6 +627,16 @@ 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 (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.h b/src/main/flight/mixer.h index 4274b284174..9ee6a20654e 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -64,7 +64,6 @@ typedef struct motorMixer_s { float yaw; } motorMixer_t; -PG_DECLARE_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer); typedef struct timerOverride_s { uint8_t outputMode; @@ -72,15 +71,6 @@ typedef struct timerOverride_s { PG_DECLARE_ARRAY(timerOverride_t, HARDWARE_TIMER_DEFINITION_COUNT, timerOverrides); -typedef struct mixerConfig_s { - int8_t motorDirectionInverted; - uint8_t platformType; - bool hasFlaps; - int16_t appliedMixerPreset; -} mixerConfig_t; - -PG_DECLARE(mixerConfig_t, mixerConfig); - typedef struct reversibleMotorsConfig_s { uint16_t deadband_low; // min 3d value uint16_t deadband_high; // max 3d value @@ -117,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(bool); uint16_t setDesiredThrottle(uint16_t throttle, bool allowMotorStop); @@ -135,6 +126,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 new file mode 100644 index 00000000000..3b0b1fd18f3 --- /dev/null +++ b/src/main/flight/mixer_profile.c @@ -0,0 +1,239 @@ +#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 "common/axis.h" +#include "flight/pid.h" +#include "flight/servos.h" +#include "flight/failsafe.h" +#include "navigation/navigation.h" +#include "navigation/navigation_private.h" + +#include "fc/fc_core.h" +#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; +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); + +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 + .motorstopOnLow = SETTING_MOTORSTOP_ON_LOW_DEFAULT, + .PIDProfileLinking = SETTING_MIXER_PID_PROFILE_LINKING_DEFAULT, + .automated_switch = SETTING_MIXER_AUTOMATED_SWITCH_DEFAULT, + .switchTransitionTimer = SETTING_MIXER_SWITCH_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); + } + 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 + ); + } + } +} + +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"); + setConfigProfile(getConfigMixerProfile()); + pidInit(); + pidInitFilters(); + pidResetErrorAccumulators(); //should be safer to reset error accumulators + schedulePidGainsUpdate(); + navigationUsePIDs(); // set navigation pid gains + } +} + +void setMixerProfileAT(void) +{ + mixerProfileAT.transitionStartTime = millis(); + mixerProfileAT.transitionTransEndTime = mixerProfileAT.transitionStartTime + (timeMs_t)currentMixerConfig.switchTransitionTimer * 100; +} + +bool checkMixerATRequired(mixerProfileATRequest_e required_action) +{ + //return false if mixerAT condition is not required or setting is not valid + if ((!STATE(AIRPLANE)) && (!STATE(MULTIROTOR))) + { + return false; + } + if (!isModeActivationConditionPresent(BOXMIXERPROFILE)) + { + return false; + } + + 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; +} + +bool mixerATUpdateState(mixerProfileATRequest_e required_action) +{ + //return true if mixerAT is done or not required + bool reprocessState; + do + { + reprocessState=false; + if (required_action==MIXERAT_REQUEST_ABORT){ + isMixerTransitionMixing_requested = false; + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + return true; + } + switch (mixerProfileAT.phase){ + case MIXERAT_PHASE_IDLE: + //check if mixerAT is required + 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_TRANSITIONING; + reprocessState = true; + break; + case MIXERAT_PHASE_TRANSITIONING: + isMixerTransitionMixing_requested = true; + if (millis() > mixerProfileAT.transitionTransEndTime){ + isMixerTransitionMixing_requested = false; + outputProfileHotSwitch(nextProfileIndex); + mixerProfileAT.phase = MIXERAT_PHASE_IDLE; + reprocessState = true; + //transition is done + } + return false; + break; + default: + break; + } + } + while (reprocessState); + return true; +} + +bool checkMixerProfileHotSwitchAvalibility(void) +{ + if (MAX_MIXER_PROFILE_COUNT != 2) + { + return false; + } + return true; +} + +void outputProfileUpdateTask(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + bool mixerAT_inuse = mixerProfileAT.phase != MIXERAT_PHASE_IDLE; + // transition mode input for servo mix and motor mix + if (!FLIGHT_MODE(FAILSAFE_MODE) && (!mixerAT_inuse)) + { + 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)); +} + +// switch mixerprofile without reboot +bool outputProfileHotSwitch(int profile_index) +{ + static bool allow_hot_switch = true; + // 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 (areSensorsCalibrating()) + { // it seems like switching before sensors calibration complete will cause pid stops to respond, especially in D + return false; + } + if (!checkMixerProfileHotSwitchAvalibility()) + { + // LOG_INFO(PWM, "mixer switch failed, checkMixerProfileHotSwitchAvalibility"); + return false; + } + 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; + } + if (!setConfigMixerProfile(profile_index)) + { + // LOG_INFO(PWM, "mixer switch failed to set config"); + return false; + } + mixerConfigInit(); + return true; +} diff --git a/src/main/flight/mixer_profile.h b/src/main/flight/mixer_profile.h new file mode 100644 index 00000000000..cb040665ce8 --- /dev/null +++ b/src/main/flight/mixer_profile.h @@ -0,0 +1,77 @@ +#pragma once + +#include "config/parameter_group.h" +#include "flight/failsafe.h" +#include "flight/mixer.h" +#include "flight/servos.h" + +#ifndef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 +#endif + +typedef struct mixerConfig_s { + int8_t motorDirectionInverted; + uint8_t platformType; + bool hasFlaps; + int16_t appliedMixerPreset; + bool motorstopOnLow; + bool PIDProfileLinking; + bool automated_switch; + int16_t switchTransitionTimer; +} mixerConfig_t; +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); +typedef enum { + MIXERAT_REQUEST_NONE, //no request, stats checking only + MIXERAT_REQUEST_RTH, + MIXERAT_REQUEST_LAND, + MIXERAT_REQUEST_ABORT, +} mixerProfileATRequest_e; + +//mixerProfile Automated Transition PHASE +typedef enum { + MIXERAT_PHASE_IDLE, + MIXERAT_PHASE_TRANSITION_INITIALIZE, + MIXERAT_PHASE_TRANSITIONING, + MIXERAT_PHASE_DONE, +} mixerProfileATState_e; + +typedef struct mixerProfileAT_s { + mixerProfileATState_e phase; + bool transitionInputMixing; + timeMs_t transitionStartTime; + timeMs_t transitionStabEndTime; + timeMs_t transitionTransEndTime; +} mixerProfileAT_t; +extern mixerProfileAT_t mixerProfileAT; +bool checkMixerATRequired(mixerProfileATRequest_e required_action); +bool mixerATUpdateState(mixerProfileATRequest_e required_action); + +extern mixerConfig_t currentMixerConfig; +extern int currentMixerProfileIndex; +extern bool isMixerTransitionMixing; +#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_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)) +#define mixerMotorMixersByIndex(index) (mixerProfiles(index)->MotorMixers) +#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/pid.c b/src/main/flight/pid.c index b2fd0383a25..2bb4ab70358 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -43,6 +43,7 @@ #include "flight/pid.h" #include "flight/imu.h" #include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/rpm_filter.h" #include "flight/kalman.h" #include "flight/smith_predictor.h" @@ -1065,7 +1066,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 @@ -1213,9 +1214,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/pid_autotune.c b/src/main/flight/pid_autotune.c index 2eaef1175ca..140c4ab61d9 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/flight/servos.c b/src/main/flight/servos.c index 67235a1133f..4d4bb497d19 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -70,11 +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) +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, @@ -83,7 +82,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance) #ifdef USE_PROGRAMMING_FRAMEWORK ,.conditionId = -1 #endif - ); + ); } } @@ -104,7 +103,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; @@ -115,7 +115,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; @@ -172,28 +172,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++; + } } } @@ -231,7 +234,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; } @@ -261,7 +264,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; } } @@ -297,6 +300,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[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: // data - middle = input @@ -335,19 +340,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 @@ -355,7 +363,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], inputRaw, currentServoMixer[i].speed * 10, dT); servo[target] += ((int32_t)inputLimited * currentServoMixer[i].rate) / 100; } @@ -430,6 +438,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) { @@ -451,6 +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 (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -463,6 +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 (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -496,6 +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 (!currentServoMixerActivative[i]) {continue;} const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t source = currentServoMixer[i].inputSource; if (source == axis) { @@ -593,6 +606,10 @@ void processServoAutotrim(const float dT) { return; } #endif + if(!STATE(AIRPLANE)) + { + return; + } if (feature(FEATURE_FW_AUTOTRIM)) { processContinuousServoAutotrim(dT); } else { diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 841415ea234..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_MIXER_TRANSITION = 38, INPUT_SOURCE_COUNT } inputSource_e; @@ -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/navigation/navigation.c b/src/main/navigation/navigation.c index 12076127458..51105f24ff3 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -47,7 +47,7 @@ #include "fc/settings.h" #include "flight/imu.h" -#include "flight/mixer.h" +#include "flight/mixer_profile.h" #include "flight/pid.h" #include "io/beeper.h" @@ -69,6 +69,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 @@ -308,6 +309,9 @@ 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 navigationFSMEvent_t navOnEnteringState_NAV_STATE_MIXERAT_ABORT(navigationFSMState_t previousState); static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { /** Idle state ******************************************************/ @@ -328,6 +332,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, } }, @@ -596,6 +601,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, } }, @@ -654,6 +660,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, } }, @@ -807,6 +814,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, } }, @@ -941,6 +949,52 @@ 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 | NAV_REQUIRE_THRTILT | NAV_MIXERAT, + .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 | 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_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_STATE_MIXERAT_ABORT] = { + .persistentId = NAV_PERSISTENT_ID_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, + .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, + + } + }, }; static navigationFSMStateFlags_t navGetStateFlags(navigationFSMState_t state) @@ -1366,6 +1420,10 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } + if (checkMixerATRequired(MIXERAT_REQUEST_RTH) && (calculateDistanceToDestination(&posControl.rthState.homePosition.pos) > (navConfig()->fw.loiter_radius * 3))){ + 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); @@ -1456,12 +1514,22 @@ 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; + 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; @@ -1854,6 +1922,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) +{ + 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); + 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; @@ -3784,14 +3916,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; } @@ -4205,15 +4337,6 @@ 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); - } #if defined(NAV_NON_VOLATILE_WAYPOINT_STORAGE) /* configure WP missions at boot */ #ifdef USE_MULTI_MISSION @@ -4274,7 +4397,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; } @@ -4369,7 +4492,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_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 0881752fc5f..3a88e4ea366 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" @@ -293,6 +294,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. diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index f5072c42195..c408f109c9b 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -157,6 +157,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 @@ -164,6 +165,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, @@ -228,6 +230,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, + NAV_PERSISTENT_ID_MIXERAT_ABORT = 41, } navigationPersistentId_e; typedef enum { @@ -275,6 +280,10 @@ typedef enum { NAV_STATE_CRUISE_IN_PROGRESS, NAV_STATE_CRUISE_ADJUSTING, + NAV_STATE_MIXERAT_INITIALIZE, + NAV_STATE_MIXERAT_IN_PROGRESS, + NAV_STATE_MIXERAT_ABORT, + NAV_STATE_COUNT, } navigationFSMState_t; @@ -304,6 +313,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 { diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index f07e487f9cb..d3b45453c91 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -46,6 +46,7 @@ #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" @@ -424,6 +425,7 @@ static int logicConditionCompute( pidInit(); pidInitFilters(); schedulePidGainsUpdate(); + navigationUsePIDs(); //set navigation pid gains profileChanged = true; } return profileChanged; @@ -769,6 +771,14 @@ 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 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 779dbb1b985..941e47f8d0d 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -135,6 +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 // 39 + LOGIC_CONDITION_OPERAND_FLIGHT_MIXER_TRANSITION_ACTIVE, //0,1 // 40 } logicFlightOperands_e; typedef enum { 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 diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 0afc15b5bef..78e52f75c2a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -564,7 +564,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 = (throttleStickIsLow() && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + throttleOffset = (throttleStickIsLow() && 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/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" diff --git a/src/main/target/SITL/target.h b/src/main/target/SITL/target.h index 78f5022a9eb..3fbe45a5cab 100644 --- a/src/main/target/SITL/target.h +++ b/src/main/target/SITL/target.h @@ -69,6 +69,8 @@ #define USE_GPS_FAKE #define USE_RANGEFINDER_FAKE #define USE_RX_SIM +#undef MAX_MIXER_PROFILE_COUNT +#define MAX_MIXER_PROFILE_COUNT 2 #define USE_MSP_OSD #define USE_OSD diff --git a/src/main/target/common.h b/src/main/target/common.h index 815b3ae41c8..c8b04f437eb 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -190,5 +190,6 @@ #define USE_SERIALRX_SUMD #define USE_TELEMETRY_HOTT #define USE_HOTT_TEXTMODE - +#else +#define MAX_MIXER_PROFILE_COUNT 1 #endif diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 90c1377bc80..30237c28552 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"