From 125cb1f4cc70bb74ae0fdbe8ead6d880f7ea7c8a Mon Sep 17 00:00:00 2001 From: demvlad Date: Mon, 31 Mar 2025 11:25:45 +0300 Subject: [PATCH 01/21] Added non PID airplane flight control system (AFCS) as special flight mode for airplanes --- mk/source.mk | 2 ++ src/main/cli/settings.c | 14 ++++++++ src/main/fc/core.c | 7 ++++ src/main/fc/parameter_names.h | 13 +++++++ src/main/fc/rc_modes.h | 3 +- src/main/fc/runtime_config.h | 4 ++- src/main/flight/airplane_fcs.c | 64 ++++++++++++++++++++++++++++++++++ src/main/flight/airplane_fcs.h | 4 +++ src/main/flight/pid.c | 23 ++++++++++++ src/main/flight/pid.h | 18 ++++++++++ src/main/flight/pid_init.c | 7 ++++ src/main/flight/servos.c | 4 +++ src/main/msp/msp_box.c | 3 +- 13 files changed, 163 insertions(+), 3 deletions(-) create mode 100644 src/main/flight/airplane_fcs.c create mode 100644 src/main/flight/airplane_fcs.h diff --git a/mk/source.mk b/mk/source.mk index b0606dbd6e..2eea6727a6 100644 --- a/mk/source.mk +++ b/mk/source.mk @@ -178,6 +178,7 @@ COMMON_SRC = \ flight/rpm_filter.c \ flight/servos.c \ flight/servos_tricopter.c \ + flight/airplane_fcs.c \ io/serial_4way.c \ io/serial_4way_avrootloader.c \ io/serial_4way_stk500v2.c \ @@ -451,6 +452,7 @@ SPEED_OPTIMISED_SRC += \ flight/mixer.c \ flight/pid.c \ flight/rpm_filter.c \ + flight/airplane_fcs.c \ rx/ibus.c \ rx/rc_stats.c \ rx/rx.c \ diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index b0ad031be9..34d963ee21 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1403,6 +1403,20 @@ const clivalue_t valueTable[] = { { PARAM_NAME_ANGLE_PITCH_OFFSET, VAR_INT16 | PROFILE_VALUE, .config.minmaxUnsigned = { -ANGLE_PITCH_OFFSET_MAX, ANGLE_PITCH_OFFSET_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, angle_pitch_offset) }, #endif +#ifdef USE_AIRPLANE_FCS + { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stick_gain) }, + { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_gain) }, + { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_time) }, + { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, + + { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_stick_gain) }, + { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_damping_gain) }, + + { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stick_gain) }, + { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_gain) }, + { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_time) }, + { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, +#endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY { "tlm_inverted", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_OFF_ON }, PG_TELEMETRY_CONFIG, offsetof(telemetryConfig_t, telemetry_inverted) }, diff --git a/src/main/fc/core.c b/src/main/fc/core.c index fc123e52a5..3867902055 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -1148,6 +1148,13 @@ void processRxModes(timeUs_t currentTimeUs) DISABLE_FLIGHT_MODE(PASSTHRU_MODE); } +#ifdef USE_AIRPLANE_FCS + if (IS_RC_MODE_ACTIVE(BOXAIRPLANEFCS)) { + ENABLE_FLIGHT_MODE(AIRPLANE_FCS_MODE); + } else { + DISABLE_FLIGHT_MODE(AIRPLANE_FCS_MODE); + } +#endif if (mixerConfig()->mixerMode == MIXER_FLYING_WING || mixerConfig()->mixerMode == MIXER_AIRPLANE) { DISABLE_FLIGHT_MODE(HEADFREE_MODE); } diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d9c812183..8d8d7df625 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -276,3 +276,16 @@ #ifdef USE_MAG #define PARAM_NAME_IMU_MAG_DECLINATION "mag_declination" #endif + +#ifdef USE_AIRPLANE_FCS +#define PARAM_NAME_AFCS_PITCH_STICK_GAIN "afcs_pitch_stick_gain" +#define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain" +#define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME "afcs_pitch_damping_filter_time" +#define PARAM_NAME_AFCS_PITCH_STABILITY_GAIN "afcs_pitch_stability_gain" +#define PARAM_NAME_AFCS_ROLL_STICK_GAIN "afcs_roll_stick_gain" +#define PARAM_NAME_AFCS_ROLL_DAMPING_GAIN "afcs_roll_damping_gain" +#define PARAM_NAME_AFCS_YAW_STICK_GAIN "afcs_yaw_stick_gain" +#define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain" +#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME "afcs_yaw_damping_filter_time" +#define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain" +#endif \ No newline at end of file diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 13838c1c23..359b138642 100644 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -40,7 +40,8 @@ typedef enum { BOXFAILSAFE, BOXPOSHOLD, BOXGPSRESCUE, - BOXID_FLIGHTMODE_LAST = BOXGPSRESCUE, + BOXAIRPLANEFCS, + BOXID_FLIGHTMODE_LAST = BOXAIRPLANEFCS, // When new flight modes are added, the parameter group version for 'modeActivationConditions' in src/main/fc/rc_modes.c has to be incremented to ensure that the RC modes configuration is reset. diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index a1df79b426..22b4420997 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -91,7 +91,8 @@ typedef enum { PASSTHRU_MODE = (1 << 8), // RANGEFINDER_MODE= (1 << 9), FAILSAFE_MODE = (1 << 10), - GPS_RESCUE_MODE = (1 << 11) + GPS_RESCUE_MODE = (1 << 11), + AIRPLANE_FCS_MODE = (1 << 12) } flightModeFlags_e; extern uint16_t flightModeFlags; @@ -113,6 +114,7 @@ extern uint16_t flightModeFlags; [BOXPASSTHRU] = LOG2(PASSTHRU_MODE), \ [BOXFAILSAFE] = LOG2(FAILSAFE_MODE), \ [BOXGPSRESCUE] = LOG2(GPS_RESCUE_MODE), \ + [BOXAIRPLANEFCS] = LOG2(AIRPLANE_FCS_MODE), \ } \ /**/ diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c new file mode 100644 index 0000000000..122a358dbd --- /dev/null +++ b/src/main/flight/airplane_fcs.c @@ -0,0 +1,64 @@ +#ifdef USE_AIRPLANE_FCS +#include "fc/rc.h" +#include "sensors/acceleration.h" +#include "sensors/gyro.h" + +void afcsInit(const pidProfile_t *pidProfile) +{ + pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_pitch_damping_filter_time * 0.001f, pidRuntime.dT)); + pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_yaw_damping_filter_time * 0.001f, pidRuntime.dT)); +} + +void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { + pidData[axis].P = 0; + pidData[axis].I = 0; + pidData[axis].D = 0; + pidData[axis].F = 0; + pidData[axis].S = 0; + pidData[axis].Sum = 0; + } + + // Pitch channel + float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_pitch_stick_gain; + float gyroPitch = gyro.gyroADCf[FD_PITCH]; + float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch); + float gyroPitchHigh = gyroPitch - gyroPitchLow; + float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_pitch_damping_gain * 0.001f); + float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f); + pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; + pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; + + // Save control components instead of PID to get logging without additional variables + pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; + pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl; + pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; + + // Roll channel + float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_roll_stick_gain); + float rollDampingCtrl = -1.0f * gyro.gyroADCf[FD_ROLL] * (pidProfile->afcs_roll_damping_gain * 0.001f); + pidData[FD_ROLL].Sum = rollPilotCtrl + rollDampingCtrl; + pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; + + // Save control components instead of PID to get logging without additional variables + pidData[FD_ROLL].F = 10.0f * rollPilotCtrl; + pidData[FD_ROLL].D = 10.0f * rollDampingCtrl; + + // Yaw channel + float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_yaw_stick_gain); + float gyroYaw = gyro.gyroADCf[FD_YAW]; + float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw); + float gyroYawHigh = gyroYaw - gyroYawLow; + float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_yaw_damping_gain * 0.001f); + float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f); + pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl; + pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; + + // Save control components instead of PID to get logging without additional variables + pidData[FD_YAW].F = 10.0f * yawPilotCtrl; + pidData[FD_YAW].D = 10.0f * yawDampingCtrl; + pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; +} +#endif \ No newline at end of file diff --git a/src/main/flight/airplane_fcs.h b/src/main/flight/airplane_fcs.h new file mode 100644 index 0000000000..cbf311aeab --- /dev/null +++ b/src/main/flight/airplane_fcs.h @@ -0,0 +1,4 @@ +#pragma once +#include "pid.h" +void afcsInit(const pidProfile_t *pidProfile); +void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 38dfdee169..21eb66423c 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -64,6 +64,9 @@ #include "pid.h" +#ifdef USE_AIRPLANE_FCS +#include "airplane_fcs.h" +#endif typedef enum { LEVEL_MODE_OFF = 0, LEVEL_MODE_R, @@ -267,6 +270,18 @@ void resetPidProfile(pidProfile_t *pidProfile) .chirp_frequency_start_deci_hz = 2, .chirp_frequency_end_deci_hz = 6000, .chirp_time_seconds = 20, +#ifdef USE_AIRPLANE_FCS + .afcs_pitch_stick_gain = 100, + .afcs_pitch_damping_gain = 20, + .afcs_pitch_damping_filter_time = 100, + .afcs_pitch_stability_gain = 0, + .afcs_roll_stick_gain = 100, + .afcs_roll_damping_gain = 25, + .afcs_yaw_stick_gain = 100, + .afcs_yaw_damping_gain = 500, + .afcs_yaw_damping_filter_time = 3000, + .afcs_yaw_stability_gain = 0, +#endif ); } @@ -1240,6 +1255,14 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim #endif // USE_CHIRP +#ifdef USE_AIRPLANE_FCS + bool isAFCS = isFixedWing() && FLIGHT_MODE(AIRPLANE_FCS_MODE); + if (isAFCS) { + afcsUpdate(pidProfile, currentTimeUs); + return; // The airplanes FCS do not need PID controller + } +#endif + // ----------PID controller---------- for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 360a805370..feffb72f16 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -332,6 +332,19 @@ typedef struct pidProfile_s { uint16_t chirp_frequency_start_deci_hz; // start frequency in units of 0.1 hz uint16_t chirp_frequency_end_deci_hz; // end frequency in units of 0.1 hz uint8_t chirp_time_seconds; // excitation time + +#ifdef USE_AIRPLANE_FCS + uint16_t afcs_pitch_stick_gain; + uint16_t afcs_pitch_damping_gain; + uint16_t afcs_pitch_damping_filter_time; + uint16_t afcs_pitch_stability_gain; + uint16_t afcs_roll_stick_gain; + uint16_t afcs_roll_damping_gain; + uint16_t afcs_yaw_stick_gain; + uint16_t afcs_yaw_damping_gain; + uint16_t afcs_yaw_damping_filter_time; + uint16_t afcs_yaw_stability_gain; +#endif } pidProfile_t; PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles); @@ -550,6 +563,11 @@ typedef struct pidRuntime_s { float chirpFrequencyEndHz; float chirpTimeSeconds; #endif // USE_CHIRP + +#ifdef USE_AIRPLANE_FCS + pt1Filter_t afcsPitchDampingLowpass; + pt1Filter_t afcsYawDampingLowpass; +#endif } pidRuntime_t; extern pidRuntime_t pidRuntime; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 799ee18605..0b0c75ff46 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -47,6 +47,10 @@ #include "sensors/gyro.h" #include "sensors/sensors.h" +#ifdef USE_AIRPLANE_FCS +#include "airplane_fcs.h" +#endif + #include "pid_init.h" #ifdef USE_D_MAX @@ -387,6 +391,9 @@ void pidInit(const pidProfile_t *pidProfile) #ifdef USE_ADVANCED_TPA tpaCurveInit(pidProfile); #endif +#ifdef USE_AIRPLANE_FCS + afcsInit(pidProfile); +#endif } void pidInitConfig(const pidProfile_t *pidProfile) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 31a49fac9d..cd66ebe02b 100644 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -415,6 +415,10 @@ void servoMixer(void) input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; + } else if (FLIGHT_MODE(AIRPLANE_FCS_MODE)) { + input[INPUT_STABILIZED_ROLL] = pidData[FD_ROLL].Sum; + input[INPUT_STABILIZED_PITCH] = pidData[FD_PITCH].Sum; + input[INPUT_STABILIZED_YAW] = pidData[FD_YAW].Sum; } else { // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui input[INPUT_STABILIZED_ROLL] = pidData[FD_ROLL].Sum * PID_SERVO_MIXER_SCALING; diff --git a/src/main/msp/msp_box.c b/src/main/msp/msp_box.c index e816db02d9..9454968b32 100644 --- a/src/main/msp/msp_box.c +++ b/src/main/msp/msp_box.c @@ -101,7 +101,8 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT] = { { .boxId = BOXBEEPERMUTE, .boxName = "BEEPER MUTE", .permanentId = 52}, { .boxId = BOXREADY, .boxName = "READY", .permanentId = 53}, { .boxId = BOXLAPTIMERRESET, .boxName = "LAP TIMER RESET", .permanentId = 54}, - { .boxId = BOXCHIRP, .boxName = "CHIRP", .permanentId = 55} + { .boxId = BOXCHIRP, .boxName = "CHIRP", .permanentId = 55}, + { .boxId = BOXAIRPLANEFCS, .boxName = "AIRPLANE FCS", .permanentId = 56}, }; // mask of enabled IDs, calculated on startup based on enabled features. boxId_e is used as bit index From 5513c359a6e714c78aadf24f39e6ddff842eb70d Mon Sep 17 00:00:00 2001 From: demvlad Date: Fri, 11 Apr 2025 20:23:10 +0300 Subject: [PATCH 02/21] AFCS code refactoring --- src/main/cli/settings.c | 12 ++++++------ src/main/flight/airplane_fcs.c | 12 ++++++------ src/main/flight/pid.c | 10 +++------- 3 files changed, 15 insertions(+), 19 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 34d963ee21..8a8a509bd6 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1404,16 +1404,16 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_AIRPLANE_FCS - { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stick_gain) }, - { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_gain) }, + { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_PITCH]) }, + { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_time) }, { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, - { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_stick_gain) }, - { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_damping_gain) }, + { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) }, + { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_ROLL]) }, - { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stick_gain) }, - { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_gain) }, + { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_YAW]) }, + { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) }, { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_time) }, { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, #endif diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 122a358dbd..8badac577c 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -22,11 +22,11 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs } // Pitch channel - float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_pitch_stick_gain; + float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH]; float gyroPitch = gyro.gyroADCf[FD_PITCH]; float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch); float gyroPitchHigh = gyroPitch - gyroPitchLow; - float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_pitch_damping_gain * 0.001f); + float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f); float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f); pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; @@ -37,8 +37,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; // Roll channel - float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_roll_stick_gain); - float rollDampingCtrl = -1.0f * gyro.gyroADCf[FD_ROLL] * (pidProfile->afcs_roll_damping_gain * 0.001f); + float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]); + float rollDampingCtrl = -1.0f * gyro.gyroADCf[FD_ROLL] * (pidProfile->afcs_damping_gain[FD_ROLL] * 0.001f); pidData[FD_ROLL].Sum = rollPilotCtrl + rollDampingCtrl; pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; @@ -47,11 +47,11 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_ROLL].D = 10.0f * rollDampingCtrl; // Yaw channel - float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_yaw_stick_gain); + float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]); float gyroYaw = gyro.gyroADCf[FD_YAW]; float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw); float gyroYawHigh = gyroYaw - gyroYawLow; - float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_yaw_damping_gain * 0.001f); + float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f); pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl; pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 21eb66423c..fa28ecea2e 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -271,14 +271,10 @@ void resetPidProfile(pidProfile_t *pidProfile) .chirp_frequency_end_deci_hz = 6000, .chirp_time_seconds = 20, #ifdef USE_AIRPLANE_FCS - .afcs_pitch_stick_gain = 100, - .afcs_pitch_damping_gain = 20, + .afcs_stick_gain = { 100, 100, 100 }, + .afcs_damping_gain = { 20, 25, 500 }, .afcs_pitch_damping_filter_time = 100, .afcs_pitch_stability_gain = 0, - .afcs_roll_stick_gain = 100, - .afcs_roll_damping_gain = 25, - .afcs_yaw_stick_gain = 100, - .afcs_yaw_damping_gain = 500, .afcs_yaw_damping_filter_time = 3000, .afcs_yaw_stability_gain = 0, #endif @@ -1549,7 +1545,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim } #ifdef USE_WING - // When PASSTHRU_MODE is active - reset all PIDs to zero so the aircraft won't snap out of control + // When PASSTHRU_MODE is active - reset all PIDs to zero so the aircraft won't snap out of control // because of accumulated PIDs once PASSTHRU_MODE gets disabled. bool isFixedWingAndPassthru = isFixedWing() && FLIGHT_MODE(PASSTHRU_MODE); #endif // USE_WING From 5af1941ad858b098cebe63556b040ca582e06e51 Mon Sep 17 00:00:00 2001 From: demvlad Date: Sat, 12 Apr 2025 15:09:32 +0300 Subject: [PATCH 03/21] afcs filters settings are changed from delay time to cut freq --- src/main/cli/settings.c | 14 +++++++------- src/main/fc/parameter_names.h | 4 ++-- src/main/flight/airplane_fcs.c | 20 ++++++++++---------- src/main/flight/pid.c | 12 ++++++------ src/main/flight/pid.h | 12 ++++-------- 5 files changed, 29 insertions(+), 33 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 8a8a509bd6..a54275cafd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1404,18 +1404,18 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_AIRPLANE_FCS - { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_PITCH]) }, + { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) }, - { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_time) }, - { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, + { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) }, + { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, - { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) }, + { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) }, { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_ROLL]) }, - { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_YAW]) }, + { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_YAW]) }, { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) }, - { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 3000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_time) }, - { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, + { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) }, + { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, #endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 8d8d7df625..0d38f52afc 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -280,12 +280,12 @@ #ifdef USE_AIRPLANE_FCS #define PARAM_NAME_AFCS_PITCH_STICK_GAIN "afcs_pitch_stick_gain" #define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain" -#define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_TIME "afcs_pitch_damping_filter_time" +#define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ "afcs_pitch_damping_filter_freq" #define PARAM_NAME_AFCS_PITCH_STABILITY_GAIN "afcs_pitch_stability_gain" #define PARAM_NAME_AFCS_ROLL_STICK_GAIN "afcs_roll_stick_gain" #define PARAM_NAME_AFCS_ROLL_DAMPING_GAIN "afcs_roll_damping_gain" #define PARAM_NAME_AFCS_YAW_STICK_GAIN "afcs_yaw_stick_gain" #define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain" -#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_TIME "afcs_yaw_damping_filter_time" +#define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ "afcs_yaw_damping_filter_freq" #define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain" #endif \ No newline at end of file diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 8badac577c..5bea01578f 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -5,8 +5,8 @@ void afcsInit(const pidProfile_t *pidProfile) { - pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_pitch_damping_filter_time * 0.001f, pidRuntime.dT)); - pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGainFromDelay(pidProfile->afcs_yaw_damping_filter_time * 0.001f, pidRuntime.dT)); + pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); + pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); } void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) @@ -32,9 +32,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; - pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl; - pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; + pidData[FD_PITCH].F = 5.0f * (pitchPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_PITCH]); + pidData[FD_PITCH].D = 5.0f * pitchDampingCtrl; + pidData[FD_PITCH].P = 5.0f * pitchStabilityCtrl; // Roll channel float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]); @@ -43,8 +43,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_ROLL].F = 10.0f * rollPilotCtrl; - pidData[FD_ROLL].D = 10.0f * rollDampingCtrl; + pidData[FD_ROLL].F = 5.0f * (rollPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_ROLL]); + pidData[FD_ROLL].D = 5.0f * rollDampingCtrl; // Yaw channel float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]); @@ -57,8 +57,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_YAW].F = 10.0f * yawPilotCtrl; - pidData[FD_YAW].D = 10.0f * yawDampingCtrl; - pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; + pidData[FD_YAW].F = 5.0f * (yawPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_YAW]); + pidData[FD_YAW].D = 5.0f * yawDampingCtrl; + pidData[FD_YAW].P = 5.0f * yawStabilityCtrl; } #endif \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fa28ecea2e..babcb4265a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -271,12 +271,12 @@ void resetPidProfile(pidProfile_t *pidProfile) .chirp_frequency_end_deci_hz = 6000, .chirp_time_seconds = 20, #ifdef USE_AIRPLANE_FCS - .afcs_stick_gain = { 100, 100, 100 }, - .afcs_damping_gain = { 20, 25, 500 }, - .afcs_pitch_damping_filter_time = 100, - .afcs_pitch_stability_gain = 0, - .afcs_yaw_damping_filter_time = 3000, - .afcs_yaw_stability_gain = 0, + .afcs_stick_gain = { 100, 100, 100 }, // Percent control output with full stick + .afcs_damping_gain = { 20, 30, 100 }, // percent control range addition by 1 degree per second angle rate * 1000 + .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100 + .afcs_pitch_stability_gain = 0, // percent control range addition by 1g accel z change *100 + .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz * 100 + .afcs_yaw_stability_gain = 0, // percent control range by 1g accel y change *100 #endif ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index feffb72f16..e82c1df0e3 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -334,15 +334,11 @@ typedef struct pidProfile_s { uint8_t chirp_time_seconds; // excitation time #ifdef USE_AIRPLANE_FCS - uint16_t afcs_pitch_stick_gain; - uint16_t afcs_pitch_damping_gain; - uint16_t afcs_pitch_damping_filter_time; + uint8_t afcs_stick_gain[XYZ_AXIS_COUNT]; + uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; + uint16_t afcs_pitch_damping_filter_freq; uint16_t afcs_pitch_stability_gain; - uint16_t afcs_roll_stick_gain; - uint16_t afcs_roll_damping_gain; - uint16_t afcs_yaw_stick_gain; - uint16_t afcs_yaw_damping_gain; - uint16_t afcs_yaw_damping_filter_time; + uint16_t afcs_yaw_damping_filter_freq; uint16_t afcs_yaw_stability_gain; #endif } pidProfile_t; From e4c09603ade61406273447715c634cb12fde4d91 Mon Sep 17 00:00:00 2001 From: demvlad Date: Sun, 13 Apr 2025 17:44:59 +0300 Subject: [PATCH 04/21] added AFCS_DEBUG for airplane flight control system --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/flight/airplane_fcs.c | 19 +++++++++++-------- 3 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index fb8795bd42..127895b2f8 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -125,4 +125,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { [DEBUG_WING_SETPOINT] = "WING_SETPOINT", [DEBUG_AUTOPILOT_POSITION] = "AUTOPILOT_POSITION", [DEBUG_CHIRP] = "CHIRP", + [DEBUG_AFCS] = "AFCS", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b72ebdb496..7c12a04098 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -127,6 +127,7 @@ typedef enum { DEBUG_WING_SETPOINT, DEBUG_AUTOPILOT_POSITION, DEBUG_CHIRP, + DEBUG_AFCS, DEBUG_COUNT } debugType_e; diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 5bea01578f..9674200c15 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -1,7 +1,10 @@ #ifdef USE_AIRPLANE_FCS + #include "fc/rc.h" #include "sensors/acceleration.h" #include "sensors/gyro.h" +#include +#include "build/debug.h" void afcsInit(const pidProfile_t *pidProfile) { @@ -32,9 +35,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_PITCH].F = 5.0f * (pitchPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_PITCH]); - pidData[FD_PITCH].D = 5.0f * pitchDampingCtrl; - pidData[FD_PITCH].P = 5.0f * pitchStabilityCtrl; + pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; + pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl; + pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; // Roll channel float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]); @@ -43,8 +46,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_ROLL].Sum = constrainf(pidData[FD_ROLL].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_ROLL].F = 5.0f * (rollPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_ROLL]); - pidData[FD_ROLL].D = 5.0f * rollDampingCtrl; + pidData[FD_ROLL].F = 10.0f * rollPilotCtrl; + pidData[FD_ROLL].D = 10.0f * rollDampingCtrl; // Yaw channel float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]); @@ -57,8 +60,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - pidData[FD_YAW].F = 5.0f * (yawPilotCtrl + pidRuntime.afcsAutoTrimPosition[FD_YAW]); - pidData[FD_YAW].D = 5.0f * yawDampingCtrl; - pidData[FD_YAW].P = 5.0f * yawStabilityCtrl; + pidData[FD_YAW].F = 10.0f * yawPilotCtrl; + pidData[FD_YAW].D = 10.0f * yawDampingCtrl; + pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; } #endif \ No newline at end of file From 72dfefc1f8fdae1029e9a1e3aa2a22b2aa4eeca8 Mon Sep 17 00:00:00 2001 From: demvlad Date: Tue, 15 Apr 2025 14:51:31 +0300 Subject: [PATCH 05/21] Added accel Z astatic regulator into pitch channel --- src/main/cli/settings.c | 3 ++ src/main/fc/parameter_names.h | 3 ++ src/main/flight/airplane_fcs.c | 50 ++++++++++++++++++++++++++++------ src/main/flight/pid.c | 15 ++++++---- src/main/flight/pid.h | 4 +++ 5 files changed, 60 insertions(+), 15 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a54275cafd..00231b6b0d 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1408,6 +1408,9 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) }, { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, + { PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) }, + { PARAM_NAME_AFCS_PITCH_ACCEL_MAX, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_max) }, + { PARAM_NAME_AFCS_PITCH_ACCEL_MIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_min) }, { PARAM_NAME_AFCS_ROLL_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_ROLL]) }, { PARAM_NAME_AFCS_ROLL_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_ROLL]) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 0d38f52afc..22aa90f72b 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -282,6 +282,9 @@ #define PARAM_NAME_AFCS_PITCH_DAMPING_GAIN "afcs_pitch_damping_gain" #define PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ "afcs_pitch_damping_filter_freq" #define PARAM_NAME_AFCS_PITCH_STABILITY_GAIN "afcs_pitch_stability_gain" +#define PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN "afcs_pitch_accel_i_gain" +#define PARAM_NAME_AFCS_PITCH_ACCEL_MAX "afcs_pitch_accel_max" +#define PARAM_NAME_AFCS_PITCH_ACCEL_MIN "afcs_pitch_accel_min" #define PARAM_NAME_AFCS_ROLL_STICK_GAIN "afcs_roll_stick_gain" #define PARAM_NAME_AFCS_ROLL_DAMPING_GAIN "afcs_roll_damping_gain" #define PARAM_NAME_AFCS_YAW_STICK_GAIN "afcs_yaw_stick_gain" diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 9674200c15..49542c2ef7 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -10,6 +10,7 @@ void afcsInit(const pidProfile_t *pidProfile) { pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); + pidRuntime.afcsAccelError = 0.0f; } void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) @@ -27,12 +28,36 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs // Pitch channel float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH]; float gyroPitch = gyro.gyroADCf[FD_PITCH]; - float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch); - float gyroPitchHigh = gyroPitch - gyroPitchLow; - float pitchDampingCtrl = -1.0f * gyroPitchHigh * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f); - float pitchStabilityCtrl = (acc.accADC.z - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f); + if (pidProfile->afcs_pitch_damping_filter_freq != 0) { + float gyroPitchLow = pt1FilterApply(&pidRuntime.afcsPitchDampingLowpass, gyroPitch); + gyroPitch -= gyroPitchLow; // Damping the pitch gyro high freq part only + } + float pitchDampingCtrl = -1.0f * gyroPitch * (pidProfile->afcs_damping_gain[FD_PITCH] * 0.001f); + float accelZ = acc.accADC.z * acc.dev.acc_1G_rec; + float pitchStabilityCtrl = (accelZ - 1.0f) * (pidProfile->afcs_pitch_stability_gain * 0.01f); + pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; - pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; + pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); + if (pidProfile->afcs_pitch_accel_i_gain != 0) { + float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f + : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; + float accelDelta = accelReq - accelZ; + pidRuntime.afcsAccelError += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; + float output = pidData[FD_PITCH].Sum + pidRuntime.afcsAccelError; + if ( output > 100.0f) { + pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum; + } else if (output < -100.0f) { + pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum; + } + pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError; + DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError)); + DEBUG_SET(DEBUG_AFCS, 4, lrintf(pidData[FD_PITCH].Sum)); + DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelReq * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 6, lrintf(accelZ * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 7, lrintf(accelDelta * 10.0f)); + } + + pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; @@ -52,10 +77,12 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs // Yaw channel float yawPilotCtrl = getSetpointRate(FD_YAW) / getMaxRcRate(FD_YAW) * (pidProfile->afcs_stick_gain[FD_YAW]); float gyroYaw = gyro.gyroADCf[FD_YAW]; - float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw); - float gyroYawHigh = gyroYaw - gyroYawLow; - float yawDampingCtrl = gyroYawHigh * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); - float yawStabilityCtrl = acc.accADC.y * (pidProfile->afcs_yaw_stability_gain * 0.01f); + if (pidProfile->afcs_yaw_damping_filter_freq != 0) { + float gyroYawLow = pt1FilterApply(&pidRuntime.afcsYawDampingLowpass, gyroYaw); + gyroYaw -= gyroYawLow; // Damping the yaw gyro high freq part only + } + float yawDampingCtrl = gyroYaw * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); + float yawStabilityCtrl = acc.accADC.y * acc.dev.acc_1G_rec * (pidProfile->afcs_yaw_stability_gain * 0.01f); pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl; pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; @@ -63,5 +90,10 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_YAW].F = 10.0f * yawPilotCtrl; pidData[FD_YAW].D = 10.0f * yawDampingCtrl; pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; + + DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl)); + DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl)); + DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl)); + } #endif \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index babcb4265a..0b54797ab8 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -271,12 +271,15 @@ void resetPidProfile(pidProfile_t *pidProfile) .chirp_frequency_end_deci_hz = 6000, .chirp_time_seconds = 20, #ifdef USE_AIRPLANE_FCS - .afcs_stick_gain = { 100, 100, 100 }, // Percent control output with full stick - .afcs_damping_gain = { 20, 30, 100 }, // percent control range addition by 1 degree per second angle rate * 1000 - .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100 - .afcs_pitch_stability_gain = 0, // percent control range addition by 1g accel z change *100 - .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz * 100 - .afcs_yaw_stability_gain = 0, // percent control range by 1g accel y change *100 + .afcs_stick_gain = { 100, 100, 100 }, // Percent control output + .afcs_damping_gain = { 20, 30, 50 }, // percent control range addition by 1 degree per second angle rate * 1000 + .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100 + .afcs_pitch_stability_gain = 0, // percent control range addition by 1g accel z change *100 + .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz * 100 + .afcs_yaw_stability_gain = 0, // percent control by 1g accel change *100 + .afcs_pitch_accel_i_gain = 0, // elevator speed for 1g accel difference in %/sec *10 + .afcs_pitch_accel_max = 80, // maximal positive z accel value *10 + .afcs_pitch_accel_min = 60, // maximal negative z accel value *10 #endif ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e82c1df0e3..f7d5d6d37d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -338,6 +338,9 @@ typedef struct pidProfile_s { uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; uint16_t afcs_pitch_damping_filter_freq; uint16_t afcs_pitch_stability_gain; + uint8_t afcs_pitch_accel_i_gain; + uint8_t afcs_pitch_accel_max; + uint8_t afcs_pitch_accel_min; uint16_t afcs_yaw_damping_filter_freq; uint16_t afcs_yaw_stability_gain; #endif @@ -563,6 +566,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; + float afcsAccelError; #endif } pidRuntime_t; From 01728927d85c14439fcd05ec05683d241444b8be Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 17 Apr 2025 13:28:52 +0300 Subject: [PATCH 06/21] Added angle of attack limiter by using of aerodynamics lift force coefficient --- src/main/cli/settings.c | 7 ++++- src/main/fc/parameter_names.h | 4 +++ src/main/flight/airplane_fcs.c | 57 ++++++++++++++++++++++++++++------ src/main/flight/pid.c | 14 ++++++--- src/main/flight/pid.h | 24 ++++++++------ 5 files changed, 81 insertions(+), 25 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 00231b6b0d..c6f2fc3217 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1408,7 +1408,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) }, { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, - { PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) }, + { PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) }, { PARAM_NAME_AFCS_PITCH_ACCEL_MAX, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_max) }, { PARAM_NAME_AFCS_PITCH_ACCEL_MIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_min) }, @@ -1419,6 +1419,11 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) }, { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) }, { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, + + { PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) }, + { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 0, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, + { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, + { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) }, #endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 22aa90f72b..27e1d2811e 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -291,4 +291,8 @@ #define PARAM_NAME_AFCS_YAW_DAMPING_GAIN "afcs_yaw_damping_gain" #define PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ "afcs_yaw_damping_filter_freq" #define PARAM_NAME_AFCS_YAW_STABILITY_GAIN "afcs_yaw_stability_gain" +#define PARAM_NAME_AFCS_WING_LOAD "afcs_wing_load" +#define PARAM_NAME_AFCS_AIR_DENSITY "afcs_air_density" +#define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit" +#define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain" #endif \ No newline at end of file diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 49542c2ef7..30f4346c38 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -3,6 +3,7 @@ #include "fc/rc.h" #include "sensors/acceleration.h" #include "sensors/gyro.h" +#include "io/gps.h" #include #include "build/debug.h" @@ -10,7 +11,21 @@ void afcsInit(const pidProfile_t *pidProfile) { pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); - pidRuntime.afcsAccelError = 0.0f; + pidRuntime.afcsPitchControlErrorSum = 0.0f; +} + +static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ) +{ + float liftC = 0.0f; + const float speedThreshold = 2.0f; //gps speed thresold + const float limitLiftC = 2.0f; + if (speed > speedThreshold) { + const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f; + liftC = accelZ * (0.001f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; + liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal AoA + } + + return liftC; } void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) @@ -38,25 +53,49 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); - if (pidProfile->afcs_pitch_accel_i_gain != 0) { + + bool isLimitAoA = false; + if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) { + float speed = 0.01f * gpsSol.speed3d; + float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ); + float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; + float delta; + if (liftCoef > 0.5f) { + delta = limitLiftC - liftCoef; + if (delta < 0.0f) { + isLimitAoA = true; + pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + } + } else if (liftCoef < -0.5f) { + delta = -limitLiftC - liftCoef; + if (delta > 0.0f) { + isLimitAoA = true; + pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + } + } + } + + if (isLimitAoA == false && pidProfile->afcs_pitch_accel_i_gain != 0) { float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; float accelDelta = accelReq - accelZ; - pidRuntime.afcsAccelError += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; - float output = pidData[FD_PITCH].Sum + pidRuntime.afcsAccelError; + pidRuntime.afcsPitchControlErrorSum += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; + float output = pidData[FD_PITCH].Sum + pidRuntime.afcsPitchControlErrorSum; if ( output > 100.0f) { - pidRuntime.afcsAccelError = 100.0f - pidData[FD_PITCH].Sum; + pidRuntime.afcsPitchControlErrorSum = 100.0f - pidData[FD_PITCH].Sum; } else if (output < -100.0f) { - pidRuntime.afcsAccelError = -100.0f - pidData[FD_PITCH].Sum; + pidRuntime.afcsPitchControlErrorSum = -100.0f - pidData[FD_PITCH].Sum; } - pidData[FD_PITCH].Sum += pidRuntime.afcsAccelError; - DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsAccelError)); + + DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsPitchControlErrorSum)); DEBUG_SET(DEBUG_AFCS, 4, lrintf(pidData[FD_PITCH].Sum)); DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelReq * 10.0f)); DEBUG_SET(DEBUG_AFCS, 6, lrintf(accelZ * 10.0f)); DEBUG_SET(DEBUG_AFCS, 7, lrintf(accelDelta * 10.0f)); } - + + pidData[FD_PITCH].Sum += pidRuntime.afcsPitchControlErrorSum; + pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0b54797ab8..d61e1e0a02 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -275,11 +275,15 @@ void resetPidProfile(pidProfile_t *pidProfile) .afcs_damping_gain = { 20, 30, 50 }, // percent control range addition by 1 degree per second angle rate * 1000 .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100 .afcs_pitch_stability_gain = 0, // percent control range addition by 1g accel z change *100 - .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz * 100 - .afcs_yaw_stability_gain = 0, // percent control by 1g accel change *100 - .afcs_pitch_accel_i_gain = 0, // elevator speed for 1g accel difference in %/sec *10 - .afcs_pitch_accel_max = 80, // maximal positive z accel value *10 - .afcs_pitch_accel_min = 60, // maximal negative z accel value *10 + .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz *100 + .afcs_yaw_stability_gain = 0, // percent control by 1g Y accel change *100 + .afcs_pitch_accel_i_gain = 250, // elevator speed for 1g Z accel difference in %/sec *10 + .afcs_pitch_accel_max = 80, // maximal positive Z accel value *10 + .afcs_pitch_accel_min = 60, // maximal negative Z accel value *10 + .afcs_wing_load = 5600, // wing load (mass / WingArea) g/decimeter^2 * 100. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think + .afcs_air_density = 1225, // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data + .afcs_lift_c_limit = 12, // Limit aerodinamics lift force coefficient value *10 + .afcs_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10 #endif ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index f7d5d6d37d..e79d7f5212 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -334,15 +334,19 @@ typedef struct pidProfile_s { uint8_t chirp_time_seconds; // excitation time #ifdef USE_AIRPLANE_FCS - uint8_t afcs_stick_gain[XYZ_AXIS_COUNT]; - uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; - uint16_t afcs_pitch_damping_filter_freq; - uint16_t afcs_pitch_stability_gain; - uint8_t afcs_pitch_accel_i_gain; - uint8_t afcs_pitch_accel_max; - uint8_t afcs_pitch_accel_min; - uint16_t afcs_yaw_damping_filter_freq; - uint16_t afcs_yaw_stability_gain; + uint8_t afcs_stick_gain[XYZ_AXIS_COUNT]; // Percent control output + uint16_t afcs_damping_gain[XYZ_AXIS_COUNT]; // percent control range addition by 1 degree per second angle rate * 1000 + uint16_t afcs_pitch_damping_filter_freq; // pitch damping filter cut freq Hz * 100 + uint16_t afcs_pitch_stability_gain; // percent control range addition by 1g accel z change *100 + uint16_t afcs_pitch_accel_i_gain; // elevator speed for 1g Z accel difference in %/sec *10 + uint8_t afcs_pitch_accel_max; // maximal positive Z accel value *10 + uint8_t afcs_pitch_accel_min; // maximal negative Z accel value *10 + uint16_t afcs_yaw_damping_filter_freq; // yaw damping filter cut freq Hz *100 + uint16_t afcs_yaw_stability_gain; // percent control by 1g Y accel change *100 + uint16_t afcs_wing_load; // wing load (mass / WingArea) g/decimeter^2 * 100. + uint16_t afcs_air_density; // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data + uint8_t afcs_lift_c_limit; // Limit aerodinamics lift force coefficient value *10 + uint16_t afcs_aoa_limiter_gain; // elevator speed for 0.1 lift force coef difference in %/sec *10 #endif } pidProfile_t; @@ -566,7 +570,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; - float afcsAccelError; + float afcsPitchControlErrorSum; #endif } pidRuntime_t; From a0fc8fa6ddda6d87d6eaa1fff778cb83c8d87df7 Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 17 Apr 2025 14:29:22 +0300 Subject: [PATCH 07/21] AFCS code refactoring --- src/main/flight/airplane_fcs.c | 102 ++++++++++++++++++--------------- src/main/flight/pid.h | 2 +- 2 files changed, 58 insertions(+), 46 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 30f4346c38..936d61fc0a 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -11,7 +11,7 @@ void afcsInit(const pidProfile_t *pidProfile) { pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); - pidRuntime.afcsPitchControlErrorSum = 0.0f; + pidRuntime.afcsElevatorAddition = 0.0f; } static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ) @@ -28,6 +28,54 @@ static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, return liftC; } +static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float pitchPilotCtrl, float accelZ) +{ + if (pidProfile->afcs_pitch_accel_i_gain != 0) { + float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f + : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; + float accelDelta = accelReq - accelZ; + pidRuntime.afcsElevatorAddition += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; + float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition; + if ( output > 100.0f) { + pidRuntime.afcsElevatorAddition = 100.0f - pidData[FD_PITCH].Sum; + } else if (output < -100.0f) { + pidRuntime.afcsElevatorAddition = -100.0f - pidData[FD_PITCH].Sum; + } + + DEBUG_SET(DEBUG_AFCS, 1, lrintf(accelReq * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 2, lrintf(accelDelta * 10.0f)); + } +} + +// The angle of attack limiter. The aerodynamics lift force coefficient depends by angle of attack. Therefore it possible to use this coef instead of AoA value. +static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float accelZ) +{ + bool isLimitAoA = false; + if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) { + float speed = 0.01f * gpsSol.speed3d; + float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ); + float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; + float liftCoefDiff; + if (liftCoef > 0.5f) { + liftCoefDiff = limitLiftC - liftCoef; + if (liftCoefDiff < 0.0f) { + isLimitAoA = true; + pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + } + } else if (liftCoef < -0.5f) { + liftCoefDiff = -limitLiftC - liftCoef; + if (liftCoefDiff > 0.0f) { + isLimitAoA = true; + pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + } + } + DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoef * 100.0f)); + DEBUG_SET(DEBUG_AFCS, 4, lrintf(liftCoefDiff * 100.0f)); + } + return isLimitAoA; +} + + void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) { UNUSED(currentTimeUs); @@ -54,48 +102,13 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); - bool isLimitAoA = false; - if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) { - float speed = 0.01f * gpsSol.speed3d; - float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ); - float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; - float delta; - if (liftCoef > 0.5f) { - delta = limitLiftC - liftCoef; - if (delta < 0.0f) { - isLimitAoA = true; - pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; - } - } else if (liftCoef < -0.5f) { - delta = -limitLiftC - liftCoef; - if (delta > 0.0f) { - isLimitAoA = true; - pidRuntime.afcsPitchControlErrorSum += delta * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; - } - } + bool isLimitAoA = updateAngleOfAttackLimiter(pidProfile, accelZ); + if (isLimitAoA == false) { + updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ); } + pidData[FD_PITCH].Sum += pidRuntime.afcsElevatorAddition; + DEBUG_SET(DEBUG_AFCS, 0, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); - if (isLimitAoA == false && pidProfile->afcs_pitch_accel_i_gain != 0) { - float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f - : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; - float accelDelta = accelReq - accelZ; - pidRuntime.afcsPitchControlErrorSum += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; - float output = pidData[FD_PITCH].Sum + pidRuntime.afcsPitchControlErrorSum; - if ( output > 100.0f) { - pidRuntime.afcsPitchControlErrorSum = 100.0f - pidData[FD_PITCH].Sum; - } else if (output < -100.0f) { - pidRuntime.afcsPitchControlErrorSum = -100.0f - pidData[FD_PITCH].Sum; - } - - DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsPitchControlErrorSum)); - DEBUG_SET(DEBUG_AFCS, 4, lrintf(pidData[FD_PITCH].Sum)); - DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelReq * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 6, lrintf(accelZ * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 7, lrintf(accelDelta * 10.0f)); - } - - pidData[FD_PITCH].Sum += pidRuntime.afcsPitchControlErrorSum; - pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables @@ -130,9 +143,8 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs pidData[FD_YAW].D = 10.0f * yawDampingCtrl; pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; - DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl)); - DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl)); - DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl)); - + DEBUG_SET(DEBUG_AFCS, 5, lrintf(pitchPilotCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 6, lrintf(pitchDampingCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 7, lrintf(pitchStabilityCtrl * 10.0f)); } #endif \ No newline at end of file diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index e79d7f5212..03d760e65e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -570,7 +570,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; - float afcsPitchControlErrorSum; + float afcsElevatorAddition; #endif } pidRuntime_t; From ff5cf0673dec3d9ba4071ffdbb6b06cd66226b7d Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 17 Apr 2025 14:43:39 +0300 Subject: [PATCH 08/21] Added servo velocity limit --- src/main/cli/settings.c | 1 + src/main/fc/parameter_names.h | 1 + src/main/flight/airplane_fcs.c | 28 ++++++++++++++++++++-------- src/main/flight/airplane_fcs.h | 2 +- src/main/flight/pid.c | 3 ++- src/main/flight/pid.h | 1 + 6 files changed, 26 insertions(+), 10 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index c6f2fc3217..6510acc040 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1424,6 +1424,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 0, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) }, + { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) }, #endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 27e1d2811e..1c9c65b52f 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -295,4 +295,5 @@ #define PARAM_NAME_AFCS_AIR_DENSITY "afcs_air_density" #define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit" #define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain" +#define PARAM_NAME_AFCS_SERVO_TIME "afcs_servo_time" #endif \ No newline at end of file diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 936d61fc0a..99b4c5270b 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -16,25 +16,32 @@ void afcsInit(const pidProfile_t *pidProfile) static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ) { - float liftC = 0.0f; - const float speedThreshold = 2.0f; //gps speed thresold const float limitLiftC = 2.0f; + float liftC = 0.0f; + const float speedThreshold = 2.5f; //gps speed thresold if (speed > speedThreshold) { const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f; liftC = accelZ * (0.001f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; - liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal AoA + liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal values during plane launch + } else { + liftC = limitLiftC; } return liftC; } +//The astatic accel Z controller by stick position static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float pitchPilotCtrl, float accelZ) { if (pidProfile->afcs_pitch_accel_i_gain != 0) { + const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; float accelDelta = accelReq - accelZ; - pidRuntime.afcsElevatorAddition += accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f) * pidRuntime.dT; + float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition; if ( output > 100.0f) { pidRuntime.afcsElevatorAddition = 100.0f - pidData[FD_PITCH].Sum; @@ -56,17 +63,23 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float acc float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ); float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; float liftCoefDiff; + const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s + float servoVelocity; if (liftCoef > 0.5f) { liftCoefDiff = limitLiftC - liftCoef; if (liftCoefDiff < 0.0f) { isLimitAoA = true; - pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } else if (liftCoef < -0.5f) { liftCoefDiff = -limitLiftC - liftCoef; if (liftCoefDiff > 0.0f) { isLimitAoA = true; - pidRuntime.afcsElevatorAddition += liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f) * pidRuntime.dT; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoef * 100.0f)); @@ -76,9 +89,8 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float acc } -void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs) +void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) { - UNUSED(currentTimeUs); for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { pidData[axis].P = 0; pidData[axis].I = 0; diff --git a/src/main/flight/airplane_fcs.h b/src/main/flight/airplane_fcs.h index cbf311aeab..c9284be675 100644 --- a/src/main/flight/airplane_fcs.h +++ b/src/main/flight/airplane_fcs.h @@ -1,4 +1,4 @@ #pragma once #include "pid.h" void afcsInit(const pidProfile_t *pidProfile); -void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile, timeUs_t currentTimeUs); +void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index d61e1e0a02..bc69a04e69 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -284,6 +284,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .afcs_air_density = 1225, // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data .afcs_lift_c_limit = 12, // Limit aerodinamics lift force coefficient value *10 .afcs_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10 + .afcs_servo_time = 90, // minimal time of servo movement from neutrale to maximum, ms #endif ); } @@ -1261,7 +1262,7 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim #ifdef USE_AIRPLANE_FCS bool isAFCS = isFixedWing() && FLIGHT_MODE(AIRPLANE_FCS_MODE); if (isAFCS) { - afcsUpdate(pidProfile, currentTimeUs); + afcsUpdate(pidProfile); return; // The airplanes FCS do not need PID controller } #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 03d760e65e..7cdd23886d 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -347,6 +347,7 @@ typedef struct pidProfile_s { uint16_t afcs_air_density; // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data uint8_t afcs_lift_c_limit; // Limit aerodinamics lift force coefficient value *10 uint16_t afcs_aoa_limiter_gain; // elevator speed for 0.1 lift force coef difference in %/sec *10 + uint16_t afcs_servo_time; // minimal time of servo movement from neutrale to maximum, ms #endif } pidProfile_t; From 048ccc00458d1b7bd099a36d1ed4120b09e7c567 Mon Sep 17 00:00:00 2001 From: demvlad Date: Fri, 18 Apr 2025 08:54:05 +0300 Subject: [PATCH 09/21] Added checking angle of attack limiter valid work conditions --- src/main/cli/settings.c | 8 +-- src/main/flight/airplane_fcs.c | 90 ++++++++++++++++++++++------------ src/main/flight/pid.c | 2 +- src/main/flight/pid.h | 2 +- 4 files changed, 65 insertions(+), 37 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 6510acc040..a821942500 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1420,11 +1420,11 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) }, { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, - { PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 2000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) }, - { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 0, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, - { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, + { PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 1500 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) }, + { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 1200, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, + { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) }, - { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) }, + { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) }, #endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 99b4c5270b..ee227e97cd 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -1,6 +1,7 @@ #ifdef USE_AIRPLANE_FCS #include "fc/rc.h" +#include "fc/runtime_config.h" #include "sensors/acceleration.h" #include "sensors/gyro.h" #include "io/gps.h" @@ -21,7 +22,7 @@ static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, const float speedThreshold = 2.5f; //gps speed thresold if (speed > speedThreshold) { const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f; - liftC = accelZ * (0.001f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; + liftC = accelZ * (0.01f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal values during plane launch } else { liftC = limitLiftC; @@ -49,41 +50,65 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float pidRuntime.afcsElevatorAddition = -100.0f - pidData[FD_PITCH].Sum; } - DEBUG_SET(DEBUG_AFCS, 1, lrintf(accelReq * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 2, lrintf(accelDelta * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 4, lrintf(accelReq * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelDelta * 10.0f)); } } // The angle of attack limiter. The aerodynamics lift force coefficient depends by angle of attack. Therefore it possible to use this coef instead of AoA value. static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float accelZ) { + static bool isLimiterEnabled = false; + static float validLiftCoefTime = 0.0f; + const float timeForValid = 3.0f; bool isLimitAoA = false; - if (gpsSol.numSat > 5 && pidProfile->afcs_aoa_limiter_gain != 0) { - float speed = 0.01f * gpsSol.speed3d; - float liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ); - float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; - float liftCoefDiff; - const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s - float servoVelocity; - if (liftCoef > 0.5f) { - liftCoefDiff = limitLiftC - liftCoef; - if (liftCoefDiff < 0.0f) { - isLimitAoA = true; - servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); - servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; - } - } else if (liftCoef < -0.5f) { - liftCoefDiff = -limitLiftC - liftCoef; - if (liftCoefDiff > 0.0f) { - isLimitAoA = true; - servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); - servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + + if (pidProfile->afcs_aoa_limiter_gain != 0 + && ARMING_FLAG(ARMED) + && gpsSol.numSat > 5) { + + float speed = 0.01f * gpsSol.speed3d, + liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ), + limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; + + // Enable AoA limiter after 2s flight with good lift coef. It prevents issues during plane launch + if (isLimiterEnabled == false) { + if (liftCoef < limitLiftC && liftCoef > -limitLiftC) { + validLiftCoefTime += pidRuntime.dT; + if (validLiftCoefTime > timeForValid) { + isLimiterEnabled = true; + } } } - DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoef * 100.0f)); - DEBUG_SET(DEBUG_AFCS, 4, lrintf(liftCoefDiff * 100.0f)); + + if (isLimiterEnabled) { + const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s + float liftCoefDiff = 0.0f, + servoVelocity = 0.0f; + if (liftCoef > 0.5f) { + liftCoefDiff = limitLiftC - liftCoef; + if (liftCoefDiff < 0.0f) { + isLimitAoA = true; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + } + } else if (liftCoef < -0.5f) { + liftCoefDiff = -limitLiftC - liftCoef; + if (liftCoefDiff > 0.0f) { + isLimitAoA = true; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + } + } + DEBUG_SET(DEBUG_AFCS, 7, lrintf(liftCoefDiff * 100.0f)); + } + + DEBUG_SET(DEBUG_AFCS, 6, lrintf(liftCoef * 100.0f)); + } else if (isLimiterEnabled) { + isLimiterEnabled = false; + validLiftCoefTime = 0.0f; } return isLimitAoA; } @@ -114,19 +139,21 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) pidData[FD_PITCH].Sum = pitchPilotCtrl + pitchDampingCtrl + pitchStabilityCtrl; pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); + // Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack bool isLimitAoA = updateAngleOfAttackLimiter(pidProfile, accelZ); if (isLimitAoA == false) { updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ); } pidData[FD_PITCH].Sum += pidRuntime.afcsElevatorAddition; - DEBUG_SET(DEBUG_AFCS, 0, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables + // Do not use I, because it needs init to zero after switch to other flight modes from current AFCS mode pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl; pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; + pidData[FD_PITCH].S = 10.0f * pidRuntime.afcsElevatorAddition; // Roll channel float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]); @@ -155,8 +182,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) pidData[FD_YAW].D = 10.0f * yawDampingCtrl; pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; - DEBUG_SET(DEBUG_AFCS, 5, lrintf(pitchPilotCtrl * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 6, lrintf(pitchDampingCtrl * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 7, lrintf(pitchStabilityCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); } #endif \ No newline at end of file diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index bc69a04e69..b208c22d4b 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -280,7 +280,7 @@ void resetPidProfile(pidProfile_t *pidProfile) .afcs_pitch_accel_i_gain = 250, // elevator speed for 1g Z accel difference in %/sec *10 .afcs_pitch_accel_max = 80, // maximal positive Z accel value *10 .afcs_pitch_accel_min = 60, // maximal negative Z accel value *10 - .afcs_wing_load = 5600, // wing load (mass / WingArea) g/decimeter^2 * 100. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think + .afcs_wing_load = 560, // wing load (mass / WingArea) g/decimeter^2 * 10. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think .afcs_air_density = 1225, // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data .afcs_lift_c_limit = 12, // Limit aerodinamics lift force coefficient value *10 .afcs_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 7cdd23886d..febda0ad07 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -343,7 +343,7 @@ typedef struct pidProfile_s { uint8_t afcs_pitch_accel_min; // maximal negative Z accel value *10 uint16_t afcs_yaw_damping_filter_freq; // yaw damping filter cut freq Hz *100 uint16_t afcs_yaw_stability_gain; // percent control by 1g Y accel change *100 - uint16_t afcs_wing_load; // wing load (mass / WingArea) g/decimeter^2 * 100. + uint16_t afcs_wing_load; // wing load (mass / WingArea) g/decimeter^2 * 10. uint16_t afcs_air_density; // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data uint8_t afcs_lift_c_limit; // Limit aerodinamics lift force coefficient value *10 uint16_t afcs_aoa_limiter_gain; // elevator speed for 0.1 lift force coef difference in %/sec *10 From 56cc9f914c99137d3c6c6d8199a63ff852997bb9 Mon Sep 17 00:00:00 2001 From: demvlad Date: Fri, 18 Apr 2025 20:21:18 +0300 Subject: [PATCH 10/21] Added license in airplane_fcs.* files --- src/main/flight/airplane_fcs.c | 22 ++++++++++++++++++++++ src/main/flight/airplane_fcs.h | 21 +++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index ee227e97cd..46add8e391 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -1,5 +1,27 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + #ifdef USE_AIRPLANE_FCS +#include "platform.h" #include "fc/rc.h" #include "fc/runtime_config.h" #include "sensors/acceleration.h" diff --git a/src/main/flight/airplane_fcs.h b/src/main/flight/airplane_fcs.h index c9284be675..9de5fe3ee1 100644 --- a/src/main/flight/airplane_fcs.h +++ b/src/main/flight/airplane_fcs.h @@ -1,3 +1,24 @@ +/* + * This file is part of Betaflight. + * + * Betaflight is free software. You can redistribute this software + * and/or modify this software under the terms of the GNU General + * Public License as published by the Free Software Foundation, + * either version 3 of the License, or (at your option) any later + * version. + * + * Betaflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. + * + * See the GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public + * License along with this software. + * + * If not, see . + */ + #pragma once #include "pid.h" void afcsInit(const pidProfile_t *pidProfile); From ac8fe74a5d0ca8d032557002870db26238b01be5 Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Fri, 18 Apr 2025 23:15:01 +0300 Subject: [PATCH 11/21] Code style improvement Co-authored-by: Mark Haslinghuis --- src/main/fc/parameter_names.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index 1c9c65b52f..de4a9eec21 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -296,4 +296,4 @@ #define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit" #define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain" #define PARAM_NAME_AFCS_SERVO_TIME "afcs_servo_time" -#endif \ No newline at end of file +#endif From 4ce36d7cd97966b2fab39c9403a79972140c4f9a Mon Sep 17 00:00:00 2001 From: Vladimir Demidov Date: Fri, 18 Apr 2025 23:15:15 +0300 Subject: [PATCH 12/21] Code style improvement Co-authored-by: Mark Haslinghuis --- src/main/flight/airplane_fcs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 46add8e391..3425ac97dc 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -209,4 +209,4 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl * 10.0f)); DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); } -#endif \ No newline at end of file +#endif From ae1ff951df2a213d0eed4df5b68a7adeb91b85e5 Mon Sep 17 00:00:00 2001 From: demvlad Date: Sat, 19 Apr 2025 18:12:38 +0300 Subject: [PATCH 13/21] AFCS AoA limiter code refactoring --- src/main/flight/airplane_fcs.c | 123 +++++++++++++++++---------------- 1 file changed, 62 insertions(+), 61 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 3425ac97dc..efd7449c9c 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -37,20 +37,39 @@ void afcsInit(const pidProfile_t *pidProfile) pidRuntime.afcsElevatorAddition = 0.0f; } -static float computeLiftCoefficient(const pidProfile_t *pidProfile, float speed, float accelZ) +static bool computeLiftCoefficient(const pidProfile_t *pidProfile, float accelZ, float *liftCoef) { - const float limitLiftC = 2.0f; - float liftC = 0.0f; - const float speedThreshold = 2.5f; //gps speed thresold - if (speed > speedThreshold) { - const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f; - liftC = accelZ * (0.01f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; - liftC = constrainf(liftC, -limitLiftC, limitLiftC); //limit lift force coef value for small speed to prevent unreal values during plane launch + static bool isLiftCoefValid = false; + static float validLiftCoefTime = 0.0f; + const float timeForValid = 3.0f; + *liftCoef = 0.0f; + if (ARMING_FLAG(ARMED) && gpsSol.numSat > 5) { + const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; + const float speedThreshold = 2.5f; //gps speed thresold + float speed = 0.01f * gpsSol.speed3d; + if (speed > speedThreshold) { + const float airSpeedPressure = (0.001f * pidProfile->afcs_air_density) * sq(speed) / 2.0f; + *liftCoef = accelZ * (0.01f * pidProfile->afcs_wing_load) * G_ACCELERATION / airSpeedPressure; + + // Enable AoA limiter after 3s flight with good lift coef. It prevents issues during plane launch + if (isLiftCoefValid == false) { + if (*liftCoef < limitLiftC && *liftCoef > -limitLiftC) { + validLiftCoefTime += pidRuntime.dT; + if (validLiftCoefTime > timeForValid) { + isLiftCoefValid = true; + } + } + } + } else { + isLiftCoefValid = false; + validLiftCoefTime = 0.0f; + } } else { - liftC = limitLiftC; + isLiftCoefValid = false; + validLiftCoefTime = 0.0f; } - return liftC; + return isLiftCoefValid; } //The astatic accel Z controller by stick position @@ -64,7 +83,8 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; - + + // limit integrator output float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition; if ( output > 100.0f) { pidRuntime.afcsElevatorAddition = 100.0f - pidData[FD_PITCH].Sum; @@ -78,60 +98,35 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float } // The angle of attack limiter. The aerodynamics lift force coefficient depends by angle of attack. Therefore it possible to use this coef instead of AoA value. -static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float accelZ) +static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float liftCoef) { - static bool isLimiterEnabled = false; - static float validLiftCoefTime = 0.0f; - const float timeForValid = 3.0f; bool isLimitAoA = false; + if (pidProfile->afcs_aoa_limiter_gain != 0) { + const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; - if (pidProfile->afcs_aoa_limiter_gain != 0 - && ARMING_FLAG(ARMED) - && gpsSol.numSat > 5) { - - float speed = 0.01f * gpsSol.speed3d, - liftCoef = computeLiftCoefficient(pidProfile, speed, accelZ), - limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; - - // Enable AoA limiter after 2s flight with good lift coef. It prevents issues during plane launch - if (isLimiterEnabled == false) { - if (liftCoef < limitLiftC && liftCoef > -limitLiftC) { - validLiftCoefTime += pidRuntime.dT; - if (validLiftCoefTime > timeForValid) { - isLimiterEnabled = true; - } + const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s + float liftCoefDiff = 0.0f, + servoVelocity = 0.0f; + if (liftCoef > 0.0f) { + liftCoefDiff = limitLiftC - liftCoef; + if (liftCoefDiff < 0.0f) { + isLimitAoA = true; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + } + } else { + liftCoefDiff = -limitLiftC - liftCoef; + if (liftCoefDiff > 0.0f) { + isLimitAoA = true; + servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); + servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); + pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } - - if (isLimiterEnabled) { - const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s - float liftCoefDiff = 0.0f, - servoVelocity = 0.0f; - if (liftCoef > 0.5f) { - liftCoefDiff = limitLiftC - liftCoef; - if (liftCoefDiff < 0.0f) { - isLimitAoA = true; - servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); - servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; - } - } else if (liftCoef < -0.5f) { - liftCoefDiff = -limitLiftC - liftCoef; - if (liftCoefDiff > 0.0f) { - isLimitAoA = true; - servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); - servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; - } - } - DEBUG_SET(DEBUG_AFCS, 7, lrintf(liftCoefDiff * 100.0f)); - } - - DEBUG_SET(DEBUG_AFCS, 6, lrintf(liftCoef * 100.0f)); - } else if (isLimiterEnabled) { - isLimiterEnabled = false; - validLiftCoefTime = 0.0f; + DEBUG_SET(DEBUG_AFCS, 7, lrintf(liftCoefDiff * 100.0f)); } + return isLimitAoA; } @@ -162,7 +157,12 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) pidData[FD_PITCH].Sum = constrainf(pidData[FD_PITCH].Sum, -100.0f, 100.0f); // Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack - bool isLimitAoA = updateAngleOfAttackLimiter(pidProfile, accelZ); + float liftCoef; + bool isValidLiftC = computeLiftCoefficient(pidProfile, accelZ, &liftCoef); + bool isLimitAoA = false; + if (isValidLiftC) { + isLimitAoA = updateAngleOfAttackLimiter(pidProfile, liftCoef); + } if (isLimitAoA == false) { updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ); } @@ -195,7 +195,7 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) gyroYaw -= gyroYawLow; // Damping the yaw gyro high freq part only } float yawDampingCtrl = gyroYaw * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); - float yawStabilityCtrl = acc.accADC.y * acc.dev.acc_1G_rec * (pidProfile->afcs_yaw_stability_gain * 0.01f); + float yawStabilityCtrl = acc.accADC.y * acc.dev.acc_1G_rec * (pidProfile->afcs_yaw_stability_gain * 0.01f); pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl; pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; @@ -208,5 +208,6 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f)); DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl * 10.0f)); DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 6, lrintf(liftCoef * 100.0f)); } #endif From b95e336c0a79b57bd4565a92e621f24a22a7214c Mon Sep 17 00:00:00 2001 From: demvlad Date: Sat, 19 Apr 2025 19:48:24 +0300 Subject: [PATCH 14/21] Added roll to yaw cross link control to improve roll rotation on high angle of attack --- src/main/cli/settings.c | 3 +++ src/main/fc/parameter_names.h | 3 +++ src/main/flight/airplane_fcs.c | 28 ++++++++++++++++++++++++---- src/main/flight/pid.c | 5 ++++- src/main/flight/pid.h | 3 +++ 5 files changed, 37 insertions(+), 5 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index a821942500..12d10ca462 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1425,6 +1425,9 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) }, { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) }, + { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_start) }, + { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_stop) }, + { PARAM_NAME_AFCS_ROLL_TO_YAW_LINK, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_to_yaw_link) }, #endif // PG_TELEMETRY_CONFIG #ifdef USE_TELEMETRY diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index de4a9eec21..dbed54f5b4 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -296,4 +296,7 @@ #define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit" #define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain" #define PARAM_NAME_AFCS_SERVO_TIME "afcs_servo_time" +#define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START "afcs_roll_yaw_clift_start" +#define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP "afcs_roll_yaw_clift_stop" +#define PARAM_NAME_AFCS_ROLL_TO_YAW_LINK "afcs_roll_to_yaw_link" #endif diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index efd7449c9c..b4f7f5824f 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -83,7 +83,7 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; - + // limit integrator output float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition; if ( output > 100.0f) { @@ -130,6 +130,21 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif return isLimitAoA; } +// Roll to yaw control cross link to improve roll rotation at high angle of attack +static float rollToYawCrossLinkControl(const pidProfile_t *pidProfile, float rollPilotControl, float liftCoef) +{ + float crossYawControl = 0.0f, + roll_yaw_clift_start = 0.1f * pidProfile->afcs_roll_yaw_clift_start, + roll_yaw_clift_stop = 0.1f * pidProfile->afcs_roll_yaw_clift_stop; + + if (pidProfile->afcs_roll_to_yaw_link && liftCoef > roll_yaw_clift_start) { + float k = (liftCoef - roll_yaw_clift_start) / (roll_yaw_clift_stop - roll_yaw_clift_start); + k = constrainf(k, 0.0f, 1.0f); + crossYawControl = k * rollPilotControl * (pidProfile->afcs_roll_to_yaw_link * 0.1f); + } + + return crossYawControl; +} void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) { @@ -158,9 +173,9 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) // Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack float liftCoef; - bool isValidLiftC = computeLiftCoefficient(pidProfile, accelZ, &liftCoef); + bool isValidLiftCoef = computeLiftCoefficient(pidProfile, accelZ, &liftCoef); bool isLimitAoA = false; - if (isValidLiftC) { + if (isValidLiftCoef) { isLimitAoA = updateAngleOfAttackLimiter(pidProfile, liftCoef); } if (isLimitAoA == false) { @@ -196,13 +211,18 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) } float yawDampingCtrl = gyroYaw * (pidProfile->afcs_damping_gain[FD_YAW] * 0.001f); float yawStabilityCtrl = acc.accADC.y * acc.dev.acc_1G_rec * (pidProfile->afcs_yaw_stability_gain * 0.01f); - pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl; + float rollToYawCrossControl = 0.0f; + if (isValidLiftCoef) { + rollToYawCrossControl = rollToYawCrossLinkControl(pidProfile, rollPilotCtrl, liftCoef); + } + pidData[FD_YAW].Sum = yawPilotCtrl + yawDampingCtrl + yawStabilityCtrl + rollToYawCrossControl; pidData[FD_YAW].Sum = constrainf(pidData[FD_YAW].Sum, -100.0f, 100.0f) / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables pidData[FD_YAW].F = 10.0f * yawPilotCtrl; pidData[FD_YAW].D = 10.0f * yawDampingCtrl; pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; + pidData[FD_YAW].S = 10.0f * rollToYawCrossControl; DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl * 10.0f)); DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f)); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b208c22d4b..73b667ffa7 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -282,9 +282,12 @@ void resetPidProfile(pidProfile_t *pidProfile) .afcs_pitch_accel_min = 60, // maximal negative Z accel value *10 .afcs_wing_load = 560, // wing load (mass / WingArea) g/decimeter^2 * 10. The g/decimeter^2 units is more comfortable for perception, than kg/m^2, i think .afcs_air_density = 1225, // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data - .afcs_lift_c_limit = 12, // Limit aerodinamics lift force coefficient value *10 + .afcs_lift_c_limit = 15, // Limit aerodinamics lift force coefficient value *10 .afcs_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10 .afcs_servo_time = 90, // minimal time of servo movement from neutrale to maximum, ms + .afcs_roll_yaw_clift_start = 8, // Aerodynamics lift force coef to start yaw control for roll rotation *10 + .afcs_roll_yaw_clift_stop = 15, // Aerodynamics lift force coef to maximum yaw control for roll rotation *10 + .afcs_roll_to_yaw_link = 0, // The maximal yaw control value to support roll rotation, % *10 #endif ); } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index febda0ad07..4c921d9df0 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -348,6 +348,9 @@ typedef struct pidProfile_s { uint8_t afcs_lift_c_limit; // Limit aerodinamics lift force coefficient value *10 uint16_t afcs_aoa_limiter_gain; // elevator speed for 0.1 lift force coef difference in %/sec *10 uint16_t afcs_servo_time; // minimal time of servo movement from neutrale to maximum, ms + uint8_t afcs_roll_yaw_clift_start; // Aerodynamics lift force coef to start yaw control for roll rotation *10 + uint8_t afcs_roll_yaw_clift_stop; // Aerodynamics lift force coef to maximum yaw control for roll rotation *10 + uint8_t afcs_roll_to_yaw_link; // The maximal yaw control value to support roll rotation, % *10 #endif } pidProfile_t; From c764c1f093ffa6863bf12f072bd2b591339ec469 Mon Sep 17 00:00:00 2001 From: demvlad Date: Sat, 19 Apr 2025 21:02:05 +0300 Subject: [PATCH 15/21] resolved missing 0.01 in cross roll-yaw control formula --- src/main/flight/airplane_fcs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index b4f7f5824f..2aade7954c 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -140,7 +140,7 @@ static float rollToYawCrossLinkControl(const pidProfile_t *pidProfile, float rol if (pidProfile->afcs_roll_to_yaw_link && liftCoef > roll_yaw_clift_start) { float k = (liftCoef - roll_yaw_clift_start) / (roll_yaw_clift_stop - roll_yaw_clift_start); k = constrainf(k, 0.0f, 1.0f); - crossYawControl = k * rollPilotControl * (pidProfile->afcs_roll_to_yaw_link * 0.1f); + crossYawControl = k * (0.01f * rollPilotControl) * (pidProfile->afcs_roll_to_yaw_link * 0.1f); } return crossYawControl; From bbb348575e068e1fdfe85d7fe33d2409885c5600 Mon Sep 17 00:00:00 2001 From: demvlad Date: Mon, 21 Apr 2025 21:17:41 +0300 Subject: [PATCH 16/21] Resolved wrong pitch stick sign issue --- src/main/flight/airplane_fcs.c | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 2aade7954c..bed8645cf2 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -79,7 +79,7 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; - float accelDelta = accelReq - accelZ; + float accelDelta = accelZ - accelReq; float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; @@ -108,16 +108,16 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif float liftCoefDiff = 0.0f, servoVelocity = 0.0f; if (liftCoef > 0.0f) { - liftCoefDiff = limitLiftC - liftCoef; - if (liftCoefDiff < 0.0f) { + liftCoefDiff = liftCoef - limitLiftC; + if (liftCoefDiff > 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } else { - liftCoefDiff = -limitLiftC - liftCoef; - if (liftCoefDiff > 0.0f) { + liftCoefDiff = liftCoef + limitLiftC; + if (liftCoefDiff < 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); From 1cedc2cb59451913b8253bbefed3d3ab3cd294f8 Mon Sep 17 00:00:00 2001 From: demvlad Date: Tue, 22 Apr 2025 13:21:08 +0300 Subject: [PATCH 17/21] resolved sign mistake by required accel z computing --- src/main/flight/airplane_fcs.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index bed8645cf2..245a797198 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -77,8 +77,8 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float { if (pidProfile->afcs_pitch_accel_i_gain != 0) { const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s - float accelReq = pitchPilotCtrl > 0.0f ? (0.1f * pidProfile->afcs_pitch_accel_max - 1.0f) * pitchPilotCtrl * 0.01f + 1.0f - : (0.1f * pidProfile->afcs_pitch_accel_min + 1.0f) * pitchPilotCtrl * 0.01f + 1.0f; + float accelReq = pitchPilotCtrl < 0.0f ? (1.0f - 0.1f * pidProfile->afcs_pitch_accel_max) * pitchPilotCtrl * 0.01f + 1.0f + : -(1.0f + 0.1f * pidProfile->afcs_pitch_accel_min) * pitchPilotCtrl * 0.01f + 1.0f; float accelDelta = accelZ - accelReq; float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); From e64c8e0da69d10f0a6e0740d6560faa7183d5a45 Mon Sep 17 00:00:00 2001 From: demvlad Date: Tue, 22 Apr 2025 13:56:58 +0300 Subject: [PATCH 18/21] increased AFCS stability settings range --- src/main/cli/settings.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 12d10ca462..cd4f8224e9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1407,7 +1407,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_PITCH_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_PITCH]) }, { PARAM_NAME_AFCS_PITCH_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_damping_filter_freq) }, - { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, + { PARAM_NAME_AFCS_PITCH_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_stability_gain) }, { PARAM_NAME_AFCS_PITCH_ACCEL_I_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_i_gain) }, { PARAM_NAME_AFCS_PITCH_ACCEL_MAX, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_max) }, { PARAM_NAME_AFCS_PITCH_ACCEL_MIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_pitch_accel_min) }, @@ -1418,7 +1418,7 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_YAW_STICK_GAIN, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 200 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_stick_gain[FD_YAW]) }, { PARAM_NAME_AFCS_YAW_DAMPING_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_damping_gain[FD_YAW]) }, { PARAM_NAME_AFCS_YAW_DAMPING_FILTER_FREQ, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 100 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_damping_filter_freq) }, - { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, + { PARAM_NAME_AFCS_YAW_STABILITY_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_yaw_stability_gain) }, { PARAM_NAME_AFCS_WING_LOAD, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 200, 1500 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_wing_load) }, { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 1200, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, From cb401d1ed912c7df7e0743c64c17dcf5a25bb640 Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 24 Apr 2025 09:42:13 +0300 Subject: [PATCH 19/21] Added lift coef forcast in AoA limiter --- src/main/cli/settings.c | 2 ++ src/main/fc/parameter_names.h | 2 ++ src/main/flight/airplane_fcs.c | 13 ++++++++++--- src/main/flight/pid.c | 6 ++++-- src/main/flight/pid.h | 3 +++ 5 files changed, 21 insertions(+), 5 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index cd4f8224e9..015d35a1dd 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1424,6 +1424,8 @@ const clivalue_t valueTable[] = { { PARAM_NAME_AFCS_AIR_DENSITY, VAR_UINT16, .config.minmaxUnsigned = { 1200, 1300 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_air_density) }, { PARAM_NAME_AFCS_LIFT_C_LIMIT, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 5, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_lift_c_limit) }, { PARAM_NAME_AFCS_AOA_LIMITER_GAIN, VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_gain) }, + { PARAM_NAME_AFCS_AOA_LIMITER_FILTER_FREQ, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, UINT8_MAX }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_filter_freq) }, + { PARAM_NAME_AFCS_AOA_LIMITER_FORCAST_TIME, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 30 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_aoa_limiter_forcast_time) }, { PARAM_NAME_AFCS_SERVO_TIME, VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 5, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_servo_time) }, { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_start) }, { PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP, VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 20 }, PG_PID_PROFILE, offsetof(pidProfile_t, afcs_roll_yaw_clift_stop) }, diff --git a/src/main/fc/parameter_names.h b/src/main/fc/parameter_names.h index dbed54f5b4..96234ce1cd 100644 --- a/src/main/fc/parameter_names.h +++ b/src/main/fc/parameter_names.h @@ -295,6 +295,8 @@ #define PARAM_NAME_AFCS_AIR_DENSITY "afcs_air_density" #define PARAM_NAME_AFCS_LIFT_C_LIMIT "afcs_lift_c_limit" #define PARAM_NAME_AFCS_AOA_LIMITER_GAIN "afcs_aoa_limiter_gain" +#define PARAM_NAME_AFCS_AOA_LIMITER_FILTER_FREQ "afcs_aoa_limiter_filter_freq" +#define PARAM_NAME_AFCS_AOA_LIMITER_FORCAST_TIME "afcs_aoa_limiter_forcast_time" #define PARAM_NAME_AFCS_SERVO_TIME "afcs_servo_time" #define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_START "afcs_roll_yaw_clift_start" #define PARAM_NAME_AFCS_ROLL_YAW_CLIFT_STOP "afcs_roll_yaw_clift_stop" diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 245a797198..97f62d5ad7 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -34,6 +34,7 @@ void afcsInit(const pidProfile_t *pidProfile) { pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); + pt1FilterInit(&pidRuntime.afcsLiftCoefLowpass, pt1FilterGain(pidProfile->afcs_aoa_limiter_filter_freq * 0.1f, pidRuntime.dT)); pidRuntime.afcsElevatorAddition = 0.0f; } @@ -101,14 +102,20 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float liftCoef) { bool isLimitAoA = false; + static float liftCoefLast = 0.0f; + float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef); + float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT; + liftCoefLast = liftCoefF; + liftCoefF += liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f); + if (pidProfile->afcs_aoa_limiter_gain != 0) { const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s float liftCoefDiff = 0.0f, servoVelocity = 0.0f; - if (liftCoef > 0.0f) { - liftCoefDiff = liftCoef - limitLiftC; + if (liftCoefF > 0.0f) { + liftCoefDiff = liftCoefF - limitLiftC; if (liftCoefDiff > 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); @@ -116,7 +123,7 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; } } else { - liftCoefDiff = liftCoef + limitLiftC; + liftCoefDiff = liftCoefF + limitLiftC; if (liftCoefDiff < 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 73b667ffa7..fd4cc7b807 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -273,9 +273,9 @@ void resetPidProfile(pidProfile_t *pidProfile) #ifdef USE_AIRPLANE_FCS .afcs_stick_gain = { 100, 100, 100 }, // Percent control output .afcs_damping_gain = { 20, 30, 50 }, // percent control range addition by 1 degree per second angle rate * 1000 - .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq Hz * 100 + .afcs_pitch_damping_filter_freq = 160, // pitch damping filter cut freq 1.6Hz (Tf=0.1s) .afcs_pitch_stability_gain = 0, // percent control range addition by 1g accel z change *100 - .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq Hz *100 + .afcs_yaw_damping_filter_freq = 5, // yaw damping filter cut freq 0.05Hz (Tf=3s) .afcs_yaw_stability_gain = 0, // percent control by 1g Y accel change *100 .afcs_pitch_accel_i_gain = 250, // elevator speed for 1g Z accel difference in %/sec *10 .afcs_pitch_accel_max = 80, // maximal positive Z accel value *10 @@ -284,6 +284,8 @@ void resetPidProfile(pidProfile_t *pidProfile) .afcs_air_density = 1225, // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data .afcs_lift_c_limit = 15, // Limit aerodinamics lift force coefficient value *10 .afcs_aoa_limiter_gain = 250, // elevator speed for 0.1 lift force coef difference in %/sec *10 + .afcs_aoa_limiter_filter_freq = 30, // aoa limiter lift coef filter cut freq 3Hz * 10 + .afcs_aoa_limiter_forcast_time = 10, // aoa limiter lift coef forcast time, 1s *10 .afcs_servo_time = 90, // minimal time of servo movement from neutrale to maximum, ms .afcs_roll_yaw_clift_start = 8, // Aerodynamics lift force coef to start yaw control for roll rotation *10 .afcs_roll_yaw_clift_stop = 15, // Aerodynamics lift force coef to maximum yaw control for roll rotation *10 diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 4c921d9df0..8c8a4f533e 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -347,6 +347,8 @@ typedef struct pidProfile_s { uint16_t afcs_air_density; // The current atmosphere air density [mg/m^3], the MSA 1225 g/m^3 value is default. TODO: Dynamical air density computing by using baro sensors data uint8_t afcs_lift_c_limit; // Limit aerodinamics lift force coefficient value *10 uint16_t afcs_aoa_limiter_gain; // elevator speed for 0.1 lift force coef difference in %/sec *10 + uint8_t afcs_aoa_limiter_filter_freq; // aoa limiter lift coef filter cut freq Hz * 10 + uint8_t afcs_aoa_limiter_forcast_time; // aoa limiter lift coef forcast time, s *10 uint16_t afcs_servo_time; // minimal time of servo movement from neutrale to maximum, ms uint8_t afcs_roll_yaw_clift_start; // Aerodynamics lift force coef to start yaw control for roll rotation *10 uint8_t afcs_roll_yaw_clift_stop; // Aerodynamics lift force coef to maximum yaw control for roll rotation *10 @@ -574,6 +576,7 @@ typedef struct pidRuntime_s { #ifdef USE_AIRPLANE_FCS pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; + pt1Filter_t afcsLiftCoefLowpass; float afcsElevatorAddition; #endif } pidRuntime_t; From 84431502b48c4c870ee32ea1a835271bafd11ed5 Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 24 Apr 2025 10:49:40 +0300 Subject: [PATCH 20/21] The PID I value uses for AFCS pitch channel --- src/main/flight/airplane_fcs.c | 59 ++++++++++++++++++---------------- src/main/flight/pid.c | 10 ++++++ src/main/flight/pid.h | 2 +- 3 files changed, 42 insertions(+), 29 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 97f62d5ad7..07ae1e6101 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -35,7 +35,7 @@ void afcsInit(const pidProfile_t *pidProfile) pt1FilterInit(&pidRuntime.afcsPitchDampingLowpass, pt1FilterGain(pidProfile->afcs_pitch_damping_filter_freq * 0.01, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsYawDampingLowpass, pt1FilterGain(pidProfile->afcs_yaw_damping_filter_freq * 0.01f, pidRuntime.dT)); pt1FilterInit(&pidRuntime.afcsLiftCoefLowpass, pt1FilterGain(pidProfile->afcs_aoa_limiter_filter_freq * 0.1f, pidRuntime.dT)); - pidRuntime.afcsElevatorAddition = 0.0f; + pidRuntime.isReadyAFCS = false; } static bool computeLiftCoefficient(const pidProfile_t *pidProfile, float accelZ, float *liftCoef) @@ -83,18 +83,18 @@ static void updateAstaticAccelZController(const pidProfile_t *pidProfile, float float accelDelta = accelZ - accelReq; float servoVelocity = accelDelta * (pidProfile->afcs_pitch_accel_i_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT; // limit integrator output - float output = pidData[FD_PITCH].Sum + pidRuntime.afcsElevatorAddition; + float output = pidData[FD_PITCH].Sum + pidData[FD_PITCH].I; if ( output > 100.0f) { - pidRuntime.afcsElevatorAddition = 100.0f - pidData[FD_PITCH].Sum; + pidData[FD_PITCH].I = 100.0f - pidData[FD_PITCH].Sum; } else if (output < -100.0f) { - pidRuntime.afcsElevatorAddition = -100.0f - pidData[FD_PITCH].Sum; + pidData[FD_PITCH].I = -100.0f - pidData[FD_PITCH].Sum; } - DEBUG_SET(DEBUG_AFCS, 4, lrintf(accelReq * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 5, lrintf(accelDelta * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 0, lrintf(accelReq * 10.0f)); + DEBUG_SET(DEBUG_AFCS, 1, lrintf(accelDelta * 10.0f)); } } @@ -120,7 +120,7 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT; } } else { liftCoefDiff = liftCoefF + limitLiftC; @@ -128,10 +128,11 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit); - pidRuntime.afcsElevatorAddition += servoVelocity * pidRuntime.dT; + pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT; } } - DEBUG_SET(DEBUG_AFCS, 7, lrintf(liftCoefDiff * 100.0f)); + DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoefF * 100.0f)); + DEBUG_SET(DEBUG_AFCS, 4, lrintf(liftCoefDiff * 100.0f)); } return isLimitAoA; @@ -155,16 +156,21 @@ static float rollToYawCrossLinkControl(const pidProfile_t *pidProfile, float rol void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) { - for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { - pidData[axis].P = 0; - pidData[axis].I = 0; - pidData[axis].D = 0; - pidData[axis].F = 0; - pidData[axis].S = 0; - pidData[axis].Sum = 0; + // Clear all PID values by first AFCS run + if (!pidRuntime.isReadyAFCS) { + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { + pidData[axis].P = 0; + pidData[axis].I = 0; + pidData[axis].D = 0; + pidData[axis].F = 0; + pidData[axis].S = 0; + pidData[axis].Sum = 0; + } + pidRuntime.isReadyAFCS = true; } // Pitch channel + pidData[FD_PITCH].I /= 10.0f; //restore % last value float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH]; float gyroPitch = gyro.gyroADCf[FD_PITCH]; if (pidProfile->afcs_pitch_damping_filter_freq != 0) { @@ -188,16 +194,15 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) if (isLimitAoA == false) { updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ); } - pidData[FD_PITCH].Sum += pidRuntime.afcsElevatorAddition; + pidData[FD_PITCH].Sum += pidData[FD_PITCH].I; pidData[FD_PITCH].Sum = pidData[FD_PITCH].Sum / 100.0f * 500.0f; // Save control components instead of PID to get logging without additional variables - // Do not use I, because it needs init to zero after switch to other flight modes from current AFCS mode pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl; pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl; - pidData[FD_PITCH].P = 10.0f * pitchStabilityCtrl; - pidData[FD_PITCH].S = 10.0f * pidRuntime.afcsElevatorAddition; + pidData[FD_PITCH].S = 10.0f * pitchStabilityCtrl; + pidData[FD_PITCH].I *= 10.0f; // Store *10 value // Roll channel float rollPilotCtrl = getSetpointRate(FD_ROLL) / getMaxRcRate(FD_ROLL) * (pidProfile->afcs_stick_gain[FD_ROLL]); @@ -228,13 +233,11 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) // Save control components instead of PID to get logging without additional variables pidData[FD_YAW].F = 10.0f * yawPilotCtrl; pidData[FD_YAW].D = 10.0f * yawDampingCtrl; - pidData[FD_YAW].P = 10.0f * yawStabilityCtrl; - pidData[FD_YAW].S = 10.0f * rollToYawCrossControl; + pidData[FD_YAW].S = 10.0f * yawStabilityCtrl; + pidData[FD_YAW].P = 10.0f * rollToYawCrossControl; - DEBUG_SET(DEBUG_AFCS, 0, lrintf(pitchPilotCtrl * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 1, lrintf(pitchDampingCtrl * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 2, lrintf(pitchStabilityCtrl * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 3, lrintf(pidRuntime.afcsElevatorAddition * 10.0f)); - DEBUG_SET(DEBUG_AFCS, 6, lrintf(liftCoef * 100.0f)); + DEBUG_SET(DEBUG_AFCS, 2, lrintf(liftCoef * 100.0f)); + DEBUG_SET(DEBUG_AFCS, 5, lrintf(pidData[FD_PITCH].I)); + DEBUG_SET(DEBUG_AFCS, 6, lrintf(pidData[FD_YAW].P)); } #endif diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index fd4cc7b807..2ceafefc29 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -1269,6 +1269,16 @@ void FAST_CODE pidController(const pidProfile_t *pidProfile, timeUs_t currentTim if (isAFCS) { afcsUpdate(pidProfile); return; // The airplanes FCS do not need PID controller + } else if (pidRuntime.isReadyAFCS) { // Clear the all PID values after AFCS work + for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) { + pidData[axis].P = 0; + pidData[axis].I = 0; + pidData[axis].D = 0; + pidData[axis].F = 0; + pidData[axis].S = 0; + pidData[axis].Sum = 0; + } + pidRuntime.isReadyAFCS = false; } #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 8c8a4f533e..d56bc46ee1 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -574,10 +574,10 @@ typedef struct pidRuntime_s { #endif // USE_CHIRP #ifdef USE_AIRPLANE_FCS + bool isReadyAFCS; pt1Filter_t afcsPitchDampingLowpass; pt1Filter_t afcsYawDampingLowpass; pt1Filter_t afcsLiftCoefLowpass; - float afcsElevatorAddition; #endif } pidRuntime_t; From 5f9409e5bef8dd0b671aa79392c3b3647d897835 Mon Sep 17 00:00:00 2001 From: demvlad Date: Thu, 24 Apr 2025 11:04:45 +0300 Subject: [PATCH 21/21] AoA limiter Lift coef forcast algorithm is improved --- src/main/flight/airplane_fcs.c | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/src/main/flight/airplane_fcs.c b/src/main/flight/airplane_fcs.c index 07ae1e6101..37490152ea 100644 --- a/src/main/flight/airplane_fcs.c +++ b/src/main/flight/airplane_fcs.c @@ -106,7 +106,7 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef); float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT; liftCoefLast = liftCoefF; - liftCoefF += liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f); + float liftCoefForcastChange = liftCoefVelocity * (pidProfile->afcs_aoa_limiter_forcast_time * 0.1f); if (pidProfile->afcs_aoa_limiter_gain != 0) { const float limitLiftC = 0.1f * pidProfile->afcs_lift_c_limit; @@ -116,6 +116,9 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif servoVelocity = 0.0f; if (liftCoefF > 0.0f) { liftCoefDiff = liftCoefF - limitLiftC; + if (liftCoefForcastChange > 0.0f) { + liftCoefDiff += liftCoefForcastChange; + } if (liftCoefDiff > 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); @@ -124,6 +127,9 @@ static bool updateAngleOfAttackLimiter(const pidProfile_t *pidProfile, float lif } } else { liftCoefDiff = liftCoefF + limitLiftC; + if (liftCoefForcastChange < 0.0f) { + liftCoefDiff += liftCoefForcastChange; + } if (liftCoefDiff < 0.0f) { isLimitAoA = true; servoVelocity = liftCoefDiff * (pidProfile->afcs_aoa_limiter_gain * 0.1f); @@ -170,7 +176,7 @@ void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile) } // Pitch channel - pidData[FD_PITCH].I /= 10.0f; //restore % last value + pidData[FD_PITCH].I /= 10.0f; //restore % last value float pitchPilotCtrl = getSetpointRate(FD_PITCH) / getMaxRcRate(FD_PITCH) * pidProfile->afcs_stick_gain[FD_PITCH]; float gyroPitch = gyro.gyroADCf[FD_PITCH]; if (pidProfile->afcs_pitch_damping_filter_freq != 0) {