diff --git a/mk/source.mk b/mk/source.mk
index 90a7cda738..c21fb1c3c8 100644
--- a/mk/source.mk
+++ b/mk/source.mk
@@ -176,6 +176,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 \
@@ -450,6 +451,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/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/cli/settings.c b/src/main/cli/settings.c
index 9e6c0ddbf2..cb5610c020 100644
--- a/src/main/cli/settings.c
+++ b/src/main/cli/settings.c
@@ -1423,6 +1423,34 @@ 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_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, 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) },
+
+ { 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_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, 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) },
+ { 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) },
+ { 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
{ "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 66784078c4..77a1f87e4e 100644
--- a/src/main/fc/core.c
+++ b/src/main/fc/core.c
@@ -1132,6 +1132,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 9dd99ed845..aab48d2430 100644
--- a/src/main/fc/parameter_names.h
+++ b/src/main/fc/parameter_names.h
@@ -276,3 +276,29 @@
#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_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"
+#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"
+#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"
+#define PARAM_NAME_AFCS_ROLL_TO_YAW_LINK "afcs_roll_to_yaw_link"
+#endif
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..37490152ea
--- /dev/null
+++ b/src/main/flight/airplane_fcs.c
@@ -0,0 +1,249 @@
+/*
+ * 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"
+#include "sensors/gyro.h"
+#include "io/gps.h"
+#include
+#include "build/debug.h"
+
+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.isReadyAFCS = false;
+}
+
+static bool computeLiftCoefficient(const pidProfile_t *pidProfile, float accelZ, float *liftCoef)
+{
+ 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 {
+ isLiftCoefValid = false;
+ validLiftCoefTime = 0.0f;
+ }
+
+ return isLiftCoefValid;
+}
+
+//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 ? (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);
+ pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT;
+
+ // limit integrator output
+ float output = pidData[FD_PITCH].Sum + pidData[FD_PITCH].I;
+ if ( output > 100.0f) {
+ pidData[FD_PITCH].I = 100.0f - pidData[FD_PITCH].Sum;
+ } else if (output < -100.0f) {
+ pidData[FD_PITCH].I = -100.0f - pidData[FD_PITCH].Sum;
+ }
+
+ DEBUG_SET(DEBUG_AFCS, 0, lrintf(accelReq * 10.0f));
+ DEBUG_SET(DEBUG_AFCS, 1, 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 liftCoef)
+{
+ bool isLimitAoA = false;
+ static float liftCoefLast = 0.0f;
+ float liftCoefF = pt1FilterApply(&pidRuntime.afcsLiftCoefLowpass, liftCoef);
+ float liftCoefVelocity = (liftCoefF - liftCoefLast) / pidRuntime.dT;
+ liftCoefLast = liftCoefF;
+ 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;
+
+ const float servoVelocityLimit = 100.0f / (pidProfile->afcs_servo_time * 0.001f); // Limit servo velocity %/s
+ float liftCoefDiff = 0.0f,
+ 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);
+ servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
+ pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT;
+ }
+ } 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);
+ servoVelocity = constrainf(servoVelocity, -servoVelocityLimit, servoVelocityLimit);
+ pidData[FD_PITCH].I += servoVelocity * pidRuntime.dT;
+ }
+ }
+ DEBUG_SET(DEBUG_AFCS, 3, lrintf(liftCoefF * 100.0f));
+ DEBUG_SET(DEBUG_AFCS, 4, lrintf(liftCoefDiff * 100.0f));
+ }
+
+ 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 * (0.01f * rollPilotControl) * (pidProfile->afcs_roll_to_yaw_link * 0.1f);
+ }
+
+ return crossYawControl;
+}
+
+void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile)
+{
+ // 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) {
+ 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);
+
+ // Hold required accel z value. If it is unpossible due of big angle of attack value, then limit angle of attack
+ float liftCoef;
+ bool isValidLiftCoef = computeLiftCoefficient(pidProfile, accelZ, &liftCoef);
+ bool isLimitAoA = false;
+ if (isValidLiftCoef) {
+ isLimitAoA = updateAngleOfAttackLimiter(pidProfile, liftCoef);
+ }
+ if (isLimitAoA == false) {
+ updateAstaticAccelZController(pidProfile, pitchPilotCtrl, accelZ);
+ }
+ 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
+ pidData[FD_PITCH].F = 10.0f * pitchPilotCtrl;
+ pidData[FD_PITCH].D = 10.0f * pitchDampingCtrl;
+ 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]);
+ 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;
+
+ // 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_stick_gain[FD_YAW]);
+ float gyroYaw = gyro.gyroADCf[FD_YAW];
+ 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);
+ 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].S = 10.0f * yawStabilityCtrl;
+ pidData[FD_YAW].P = 10.0f * rollToYawCrossControl;
+
+ 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/airplane_fcs.h b/src/main/flight/airplane_fcs.h
new file mode 100644
index 0000000000..9de5fe3ee1
--- /dev/null
+++ b/src/main/flight/airplane_fcs.h
@@ -0,0 +1,25 @@
+/*
+ * 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);
+void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile);
diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c
index 38dfdee169..2ceafefc29 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,27 @@ 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_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 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 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
+ .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 = 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
+ .afcs_roll_to_yaw_link = 0, // The maximal yaw control value to support roll rotation, % *10
+#endif
);
}
@@ -1240,6 +1264,24 @@ 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);
+ 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
+
// ----------PID controller----------
for (int axis = FD_ROLL; axis <= FD_YAW; ++axis) {
@@ -1526,7 +1568,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
diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h
index 360a805370..d56bc46ee1 100644
--- a/src/main/flight/pid.h
+++ b/src/main/flight/pid.h
@@ -332,6 +332,28 @@ 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
+ 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 * 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
+ 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
+ uint8_t afcs_roll_to_yaw_link; // The maximal yaw control value to support roll rotation, % *10
+#endif
} pidProfile_t;
PG_DECLARE_ARRAY(pidProfile_t, PID_PROFILE_COUNT, pidProfiles);
@@ -550,6 +572,13 @@ typedef struct pidRuntime_s {
float chirpFrequencyEndHz;
float chirpTimeSeconds;
#endif // USE_CHIRP
+
+#ifdef USE_AIRPLANE_FCS
+ bool isReadyAFCS;
+ pt1Filter_t afcsPitchDampingLowpass;
+ pt1Filter_t afcsYawDampingLowpass;
+ pt1Filter_t afcsLiftCoefLowpass;
+#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