diff --git a/docs/Feedback_Linearization.md b/docs/Feedback_Linearization.md new file mode 100644 index 0000000000..f498fd6a70 --- /dev/null +++ b/docs/Feedback_Linearization.md @@ -0,0 +1,42 @@ +# Feedback Linearization +Feedback Linearization is a non-linear control technique used for non-linear systems. The rotational physics of a quadcopter are described by [Euler's Equation](https://en.wikipedia.org/wiki/Euler%27s_equations_(rigid_body_dynamics)) for Rigid Bodies and are non-linear. As can be shown from the equation, the axes are coupled to one another and rotation about two axes of quadcopter creates a torque on the third axis. A PID controller is a linear controller and isn't designed for this kind of system. However, it is seen that in many practical applications a PID controller with gain scheduling (TPA, Antigravity, EMUBoost etc) can provide sufficient performance. However, feedback linearization makes a non-linear system look linear from the perspective of the controller allowing use of a linear controller on a non-linear system. The hypothesis is that feedback linearization will improve performance of the quadcopter in situations with rotational velocties on multiple axis such as coordinated turns, split-s turns, cross compensated rolls/flips, hard 180 turns etc. Furthermore, it should help with propwash since propwash often shows up on multiple axes at the same time. + +The code is designed so that physical parameters of the quadcopter don't need to be directly known. The physical parameters are described instead by non-dimensional constants which may be tuned intuitively by someone who isn't academically trained in the dynamics of a quadcopter. + +## Physical Constants +There are 5 parameters that need to be calculated: +1. torqueInertia ratio +2. pitchInertia ratio +3. yawInertia ratio +4. pitchTorque ratio +5. yawTorque ratio +The Torque to Inertia ratio describes the ratio of the maximum torque on the roll axis to it's moment of inertia. This is similar to the concept of thrust to weight ratio. +The 2nd two constants are the pitchInertia and yawInertia ratios. They describe the relative size of the pitch and yaw moments of inertia with respect to the roll moment of inertia. Due to the elongated body and typical distribution of mass along the pitch axis, The pitchInertia and yawInertia ratios are almost always greater than one with the yaw ratio being larger than the pitch ratio \(1\ff_jitter_factor; #endif +#ifdef USE_FEEDBACK_LINEARIZATION + cmsx_torqueInertiaRatio = pidProfile->torqueInertiaRatio; + cmsx_pitchTorqueRatio = pidProfile->pitchTorqueRatio; + cmsx_yawTorqueRatio = pidProfile->yawTorqueRatio; + cmsx_pitchInertiaRatio = pidProfile->pitchInertiaRatio; + cmsx_yawInertiaRatio = pidProfile->yawInertiaRatio; +#endif + #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION cmsx_vbat_sag_compensation = pidProfile->vbat_sag_compensation; #endif @@ -657,13 +673,20 @@ static const void *cmsx_profileOtherOnExit(displayPort_t *pDisp, const OSD_Entry pidProfile->motor_output_limit = cmsx_motorOutputLimit; pidProfile->auto_profile_cell_count = cmsx_autoProfileCellCount; - #ifdef USE_INTERPOLATED_SP pidProfile->ff_interpolate_sp = cmsx_ff_interpolate_sp; pidProfile->ff_smooth_factor = cmsx_ff_smooth_factor; pidProfile->ff_jitter_factor = cmsx_ff_jitter_factor; #endif +#ifdef USE_FEEDBACK_LINEARIZATION + pidProfile->torqueInertiaRatio = cmsx_torqueInertiaRatio; + pidProfile->pitchTorqueRatio = cmsx_pitchTorqueRatio; + pidProfile->yawTorqueRatio = cmsx_yawTorqueRatio; + pidProfile->pitchInertiaRatio = cmsx_pitchInertiaRatio; + pidProfile->yawInertiaRatio = cmsx_yawInertiaRatio; +#endif + #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION pidProfile->vbat_sag_compensation = cmsx_vbat_sag_compensation; #endif @@ -707,6 +730,14 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { { "AUTO CELL CNT", OME_INT8, NULL, &(OSD_INT8_t) { &cmsx_autoProfileCellCount, AUTO_PROFILE_CELL_COUNT_CHANGE, MAX_AUTO_DETECT_CELL_COUNT, 1}, 0 }, +#ifdef USE_FEEDBACK_LINEARIZATION + { "TORQUE INERTIA RATIO", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_torqueInertiaRatio, 1, 10000, 1 } , 0 }, + { "PITCH TORQUE RATIO", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchTorqueRatio, 1, 250, 1 } , 0 }, + { "YAW TORQUE RATIO", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawTorqueRatio, 1, 250, 1 } , 0 }, + { "PITCH INERTIA RATIO", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchInertiaRatio, 1, 250, 1 } , 0 }, + { "YAW INERTIA RATION", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_yawInertiaRatio, 1, 500, 1 } , 0 }, +#endif + #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION { "VBAT_SAG_COMP", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_vbat_sag_compensation, 0, 150, 1 }, 0 }, #endif diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index d99da82561..76f8ce4c27 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -487,6 +487,10 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) } #endif +#ifdef USE_FEEDBACK_LINEARIZATION +applyFeedbackLinearization(pidData, gyro.gyroADCf); +#endif + // Calculate and Limit the PID sum const float scaledAxisPidRoll = constrainf(pidData[FD_ROLL].Sum, -currentPidProfile->pidSumLimit, currentPidProfile->pidSumLimit) / PID_MIXER_SCALING; diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3563c389e3..07682a66c2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -184,6 +184,11 @@ void resetPidProfile(pidProfile_t *pidProfile) .dyn_lpf_curve_expo = 5, .vbat_sag_compensation = 0, .dtermMeasurementSlider = 100, + .torqueInertiaRatio = 10000, + .pitchTorqueRatio = 100, + .yawTorqueRatio = 100, + .pitchInertiaRatio = 100, + .yawInertiaRatio = 100, .emuBoostPR = 15, .emuBoostY = 40, .dtermBoost = 0, @@ -968,3 +973,26 @@ float pidGetPidFrequency() { return pidRuntime.pidFrequency; } + +#ifdef USE_FEEDBACK_LINEARIZATION +void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) +{ + float k2 = pidRuntime.pitchInertiaRatio; + float k3 = pidRuntime.yawInertiaRatio; + float kt2 = pidRuntime.pitchTorqueRatio; + float kt3 = pidRuntime.yawTorqueRatio; + float TIR = pidRuntime.torqueInertiaRatio; + if (!(kt2==0 || kt3==0 || TIR==0)){ + float roll_linearization = (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (TIR); + float pitch_linearization = (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (TIR * kt2); + float yaw_linearization = (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (TIR * kt3); + pids[FD_ROLL].Sum += roll_linearization; + pids[FD_PITCH].Sum += pitch_linearization; + pids[FD_YAW].Sum += yaw_linearization; + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 0, lrintf(100.0f * pids[FD_ROLL].Sum / currentPidProfile->pidSumLimit)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 1, lrintf(100.0f * pids[FD_PITCH].Sum / currentPidProfile->pidSumLimit)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 2, lrintf(100.0f * pids[FD_YAW].Sum / currentPidProfile->pidSumLimitYaw)); + + } +} +#endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index aa592a02f0..e2f561392c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -167,6 +167,12 @@ typedef struct pidProfile_s { uint8_t dtermMeasurementSlider; + uint16_t torqueInertiaRatio; + uint8_t pitchTorqueRatio; + uint8_t yawTorqueRatio; + uint8_t pitchInertiaRatio; + uint16_t yawInertiaRatio; + uint16_t emuBoostPR; uint16_t emuBoostY; uint16_t dtermBoost; @@ -279,6 +285,12 @@ typedef struct pidRuntime_s { float dtermMeasurementSlider; float dtermMeasurementSliderInverse; + float torqueInertiaRatio; + float pitchTorqueRatio; + float yawTorqueRatio; + float pitchInertiaRatio; + float yawInertiaRatio; + float emuBoostPR; float emuBoostY; float emuBoostLimitPR; @@ -389,3 +401,4 @@ float pidGetFfBoostFactor(); float pidGetFfSmoothFactor(); float pidGetFfJitterFactor(); float dynLpfCutoffFreq(float throttle, uint16_t dynLpfMin, uint16_t dynLpfMax, uint8_t expo); +void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData); diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index c6e397735b..2b45f6e71a 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -247,6 +247,12 @@ void pidInitConfig(const pidProfile_t *pidProfile) pidRuntime.dtermMeasurementSlider = pidProfile->dtermMeasurementSlider / 100; pidRuntime.dtermMeasurementSliderInverse = 1 - (pidProfile->dtermMeasurementSlider / 100); + pidRuntime.torqueInertiaRatio = ((float)pidProfile->torqueInertiaRatio)/100.0f; + pidRuntime.pitchTorqueRatio = ((float)pidProfile->pitchTorqueRatio)/100.0f; + pidRuntime.yawTorqueRatio = ((float)pidProfile->yawTorqueRatio)/100.0f; + pidRuntime.pitchInertiaRatio = ((float)pidProfile->pitchInertiaRatio)/100.0f; + pidRuntime.yawInertiaRatio = ((float)pidProfile->yawInertiaRatio)/100.0f; + pidRuntime.emuBoostPR = (pidProfile->emuBoostPR * pidProfile->emuBoostPR / 1000000) * 0.003; pidRuntime.emuBoostY = (pidProfile->emuBoostY * pidProfile->emuBoostY / 1000000) * 0.003; pidRuntime.emuBoostLimitPR = powf(pidProfile->emuBoostPR, 0.75f) * 1.4; diff --git a/src/main/target/common_pre.h b/src/main/target/common_pre.h index 46120f1c06..e59f5dd4e1 100644 --- a/src/main/target/common_pre.h +++ b/src/main/target/common_pre.h @@ -396,4 +396,5 @@ #define USE_SMITH_PREDICTOR #define USE_RX_LINK_UPLINK_POWER #define USE_GPS_PLUS_CODES +#define USE_FEEDBACK_LINEARIZATION #endif