mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-12 19:10:32 +03:00
Merge 5f9409e5be into acbab53d13
This commit is contained in:
Vladimir Demidov
GitHub
commit
526d576f3f
15 changed files with 429 additions and 4 deletions
|
|
@ -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 \
|
||||
|
|
|
|||
|
|
@ -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",
|
||||
};
|
||||
|
|
|
|||
|
|
@ -127,6 +127,7 @@ typedef enum {
|
|||
DEBUG_WING_SETPOINT,
|
||||
DEBUG_AUTOPILOT_POSITION,
|
||||
DEBUG_CHIRP,
|
||||
DEBUG_AFCS,
|
||||
DEBUG_COUNT
|
||||
} debugType_e;
|
||||
|
||||
|
|
|
|||
|
|
@ -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) },
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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.
|
||||
|
||||
|
|
|
|||
|
|
@ -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), \
|
||||
} \
|
||||
/**/
|
||||
|
||||
|
|
|
|||
249
src/main/flight/airplane_fcs.c
Normal file
249
src/main/flight/airplane_fcs.c
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#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 <math.h>
|
||||
#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
|
||||
25
src/main/flight/airplane_fcs.h
Normal file
25
src/main/flight/airplane_fcs.h
Normal file
|
|
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
#include "pid.h"
|
||||
void afcsInit(const pidProfile_t *pidProfile);
|
||||
void FAST_CODE afcsUpdate(const pidProfile_t *pidProfile);
|
||||
|
|
@ -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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
|
|
|
|||
|
|
@ -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
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue