Feedback linearization

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\<kp\<ky).
+The last two parameters are the pitchTorque and yawTorque ratios describe the relative abilities of the motors to produce torque along a given axis. This depends on the motor geometry. A stretch-X frame will have a pitchTorque ratio greater than one but a wide-x frame will have lower ratio. The yawTorque ratio is often less than one since the propeller drag constant is less than the propeller thrust constant but the value for it may be determined from a simple hover test as long as the the flight controller has a current meter.
+
+The torqueInertia ratio may be multiplied by the corresponding inertia and torque ratio's for the other axis to calculate their torque to inertia ratios:
+torqueInertiaPitch = torqueInertia \* pitchTorque / pitchInertia
+
+## Calculating the Parameters
+It is assumed that the frame is symmetrical ie\(any X or H design) and that the motor layout and weights have the largest effect on moments of inertia. Note that this is a rough estimation only. The estimation can be improved by acounting for the inertia due to the battery, body, HD camera, and electronics. If one has a 3d model of their exact build they could use a 3d modelling program to directly calculate the moments of inertia. That would give the most accurate results.
+
+There are several measurements that need to be taken:
+1. W is the width of the motor layout in **millimeters**
+2. L is the length of the motor layout in **millimeters**
+3. m is the mass of a motor + propellor + mounting in **grams**
+4. M is the all up weight of the quad in **grams**
+5. Perform a hover test to get Ih the average hover current in **amps**
+
+Calculate important Characteristics
+1. TWR is the thrust to weight ratio of the quad
+
+Important constants:
+1. gravitational acceleration g = 9.81 m/s^2
+2. motor velocity constant Kv \(as reported by the manufacturer)
+
+The parameters are calculated as follows
+1. torqueInertia = 1e3 * \( M * g * TWR) / \(4 * W * m)
+2. pitchInertia = L^2 / W^2
+3. yawInertia = = \(L^2 + W^2) / W^2
+4. pitchTorque = L / W
+5. yawTorque = 1e6 * \(16.4 * Ih) / \(W * Kv * M * g)

src/main/build/debug.c

@@ -103,4 +103,5 @@ const char * const debugModeNames[DEBUG_COUNT] = {
     "D_ABG_STATE",
     "RC_PREDICTOR",
     "SMITH_PREDICT",
+    "FEEDBACK_LINEARIZATION",
 };

src/main/build/debug.h

@@ -119,6 +119,7 @@ typedef enum {
     DEBUG_D_ABG_STATE,
     DEBUG_RC_PREDICTOR,
     DEBUG_SMITH_PREDICTOR,
+    DEBUG_FEEDBACK_LINEARIZATION,
     DEBUG_COUNT
 } debugType_e;
 

src/main/cli/settings.c

@@ -1153,6 +1153,15 @@ const clivalue_t valueTable[] = {
 #ifdef USE_THRUST_LINEARIZATION
     { "thrust_linear",              VAR_UINT8 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 150 }, PG_PID_PROFILE, offsetof(pidProfile_t, thrustLinearization) },
 #endif
+
+#ifdef USE_FEEDBACK_LINEARIZATION
+    { "torque_inertia_ratio",       VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 10000 }, PG_PID_PROFILE, offsetof(pidProfile_t, torqueInertiaRatio) },
+    { "pitch_torque_ratio",         VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchTorqueRatio) },
+    { "yaw_torque_ratio",           VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawTorqueRatio) },
+    { "pitch_inertia_ratio",        VAR_UINT8  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchInertiaRatio) },
+    { "yaw_inertia_ratio",          VAR_UINT16  | PROFILE_VALUE, .config.minmaxUnsigned = { 1, 500 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawInertiaRatio) },
+#endif
+
 #ifdef USE_INTERPOLATED_SP
     { "ff_interpolate_sp",          VAR_UINT8 | PROFILE_VALUE | MODE_LOOKUP, .config.lookup = {TABLE_INTERPOLATED_SP}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_interpolate_sp) },
     { "ff_max_rate_limit",          VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = {0, 150}, PG_PID_PROFILE, offsetof(pidProfile_t, ff_max_rate_limit) },

src/main/cms/cms_menu_imu.c

@@ -587,6 +587,14 @@ static uint8_t cmsx_ff_smooth_factor;
 static uint8_t cmsx_ff_jitter_factor;
 #endif
 
+#ifdef USE_FEEDBACK_LINEARIZATION
+static uint16_t  cmsx_torqueInertiaRatio;
+static uint8_t  cmsx_pitchTorqueRatio;
+static uint8_t  cmsx_yawTorqueRatio;
+static uint8_t cmsx_pitchInertiaRatio;
+static uint16_t  cmsx_yawInertiaRatio;
+#endif
+
 static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
 {
     UNUSED(pDisp);
@@ -623,6 +631,14 @@ static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp)
     cmsx_ff_jitter_factor = pidProfile->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

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;

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

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

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;

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