Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 00:35:39 +03:00
Code Issues Wiki Activity

Added non PID airplane flight control system (AFCS) as special flight mode for airplanes

Browse source
This commit is contained in:
demvlad 2025-03-31 11:25:45 +03:00
parent 9eb7d1920b
commit 125cb1f4cc
13 changed files with 163 additions and 3 deletions
Download patch file Download diff file Expand all files Collapse all files

2
mk/source.mk
View file

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

14
src/main/cli/settings.c
View file

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

7
src/main/fc/core.c
View file

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

13
src/main/fc/parameter_names.h
View file

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

3
src/main/fc/rc_modes.h
View file

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

4
src/main/fc/runtime_config.h
View file

@ -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), \
} \
/**/

64
src/main/flight/airplane_fcs.c Normal file
View file

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

4
src/main/flight/airplane_fcs.h Normal file
View file

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

23
src/main/flight/pid.c
View file

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

18
src/main/flight/pid.h
View file

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

7
src/main/flight/pid_init.c
View file

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

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

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

3
src/main/msp/msp_box.c
View file

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