From e7dbcb7f237226e9627eefd9363dc916e13b68f5 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Thu, 27 May 2021 22:58:56 -0600 Subject: [PATCH 01/12] Feedback linearization law added to mixer. --- docs/FeedbackLinearization_notes.txt | 2 ++ src/main/flight/mixer.c | 11 +++++++++++ src/main/flight/pid.c | 5 +++++ src/main/flight/pid.h | 12 ++++++++++++ src/main/flight/pid_init.c | 6 ++++++ 5 files changed, 36 insertions(+) create mode 100644 docs/FeedbackLinearization_notes.txt diff --git a/docs/FeedbackLinearization_notes.txt b/docs/FeedbackLinearization_notes.txt new file mode 100644 index 0000000000..9e3dcca20e --- /dev/null +++ b/docs/FeedbackLinearization_notes.txt @@ -0,0 +1,2 @@ +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 of Motion for a Rigid Body. As can be shown from the equation, rotation about two axis of quadcopter creates a torque on the third axis. The PID controller is a linear controller and is not designed to handle this kind of system. However, it is seen that in many practical applications a PID controller is sufficient with gain scheduling (TPA, Antigravity, EMUBoost etc). However, feedback linearization makes a non-linear system look linear from the perspective of the linear controller allowing it to perform the job it is designed for. 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 compensating rolls, hard 180 turns etc. Furthermore, it should help with propwash since propwash typically shows up on multiple axis at the same time. +The code is designed so that physical parameters of the quadcopter don't need to be directly known and non-dimensional constants which may be intuitively tuned by someone who isn't academically trained in the dynamics of a quadcopter. The first 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 pitch inertia and yawInertia ratios are almost always greater than one with the yaw ratio being larger than the pitch ratio (1yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3563c389e3..3d57e66337 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 = 0, + .pitchTorqueRatio = 0, + .yawTorqueRatio = 0, + .pitchInertiaRatio = 0, + .yawInertiaRatio = 0, .emuBoostPR = 15, .emuBoostY = 40, .dtermBoost = 0, diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index aa592a02f0..bf07105db2 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; + uint8_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; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index c6e397735b..898015fee8 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; From 89bce4566a5c3b11e740987fee762576420cb7a0 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Sun, 30 May 2021 16:07:11 -0600 Subject: [PATCH 02/12] added cli and osd settings and cleaned up code --- src/main/cli/settings.c | 9 +++++++++ src/main/cms/cms_menu_imu.c | 33 ++++++++++++++++++++++++++++++++- src/main/flight/mixer.c | 9 +-------- src/main/flight/pid.c | 14 ++++++++++++++ src/main/flight/pid.h | 3 ++- src/main/flight/pid_init.c | 2 +- src/main/target/common_pre.h | 1 + 7 files changed, 60 insertions(+), 11 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 699d798fd0..5b9afbd99f 100644 --- a/src/main/cli/settings.c +++ b/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 = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, torqueInertiaRatio) }, + { "pitch_torque_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchTorqueRatio) }, + { "yaw_torque_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawTorqueRatio) }, + { "pitch_inertia_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchInertiaRatio) }, + { "yaw_inertia_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, 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) }, diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index a6e62baaab..d3debef211 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/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 uint8_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 + { "THR BOOST", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_torqueInertiaRatio, 0, 1000, 1 } , 0 }, + { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchTorqueRatio, 0, 250, 1 } , 0 }, + { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawTorqueRatio, 0, 250, 1 } , 0 }, + { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchInertiaRatio, 0, 250, 1 } , 0 }, + { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawInertiaRatio, 0, 250, 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 2d9ffa2abd..818bc4345e 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -506,14 +506,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; #ifdef USE_FEEDBACK_LINEARIZATION -k2 = pidRuntime.pitchInertiaRatio; -k3 = pidRuntime.yawInertiaRatio; -kt2 = pidRuntime.pitchTorqueRatio; -kt3 = pidRuntime.yawTorqueRatio; -ToIR = pidRuntime.TorqueInertiaRatio; -pidData[FD_ROLL].SUM += (k3-k2)/ToIR*0.0003046f*gyro.gyroADCf[FD_YAW]*gyro.gyroADCf[FD_PITCH]/PID_MIXER_SCALING; -pidData[FD_PITCH].SUM += (1.0f-k3)/ToIR/kt2*0.0003046f*gyro.gyroADCf[FD_YAW]*gyro.gyroADCf[FD_ROLL]/PID_MIXER_SCALING; -pidData[FD_YAW].SUM += (k2-1.0f)/ToIR/kt3*0.0003046f*gyro.gyroADCf[FD_ROLL]*gyro.gyroADCf[FD_PITCH]/PID_MIXER_SCALING; +doFeedbackLinearization(pidData, gyro.gyroADCf); #endif if (!mixerConfig()->yaw_motors_reversed) { diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 3d57e66337..0823743a47 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -973,3 +973,17 @@ 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; + pidData[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); + pidData[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); + pidData[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); +} +#endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index bf07105db2..db17d0b0b8 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -167,7 +167,7 @@ typedef struct pidProfile_s { uint8_t dtermMeasurementSlider; - uint16_t TorqueInertiaRatio; + uint16_t torqueInertiaRatio; uint8_t pitchTorqueRatio; uint8_t yawTorqueRatio; uint8_t pitchInertiaRatio; @@ -401,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 898015fee8..449affd63e 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -247,7 +247,7 @@ 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.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; 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 From b17ebb4c3ea392a66d114dbc31cd23af2cd33c4b Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Sun, 30 May 2021 16:38:41 -0600 Subject: [PATCH 03/12] fixes --- src/main/flight/mixer.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 818bc4345e..6d9f40bfff 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -506,7 +506,7 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; #ifdef USE_FEEDBACK_LINEARIZATION -doFeedbackLinearization(pidData, gyro.gyroADCf); +applyFeedbackLinearization(pidData, gyro.gyroADCf); #endif if (!mixerConfig()->yaw_motors_reversed) { From 4ac7845789b2d932e9eea3af72c531d50779dfe4 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Sun, 30 May 2021 16:38:53 -0600 Subject: [PATCH 04/12] fix --- src/main/flight/pid.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 0823743a47..5cfb81b019 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -982,8 +982,8 @@ void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) float kt2 = pidRuntime.pitchTorqueRatio; float kt3 = pidRuntime.yawTorqueRatio; float TIR = pidRuntime.TorqueInertiaRatio; - pidData[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); - pidData[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); - pidData[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); + pids[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); + pids[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); + pids[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); } #endif From ad8f50c926cd38adc0680d6f83ea7e971fe0cfc2 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Tue, 8 Jun 2021 11:39:24 -0600 Subject: [PATCH 05/12] change defaults protect against 0 division fix osd --- src/main/cli/settings.c | 10 +++++----- src/main/cms/cms_menu_imu.c | 12 ++++++------ src/main/flight/pid.c | 20 +++++++++++--------- src/main/flight/pid.h | 4 ++-- src/main/flight/pid_init.c | 2 +- 5 files changed, 25 insertions(+), 23 deletions(-) diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 5b9afbd99f..08cfb3dbb9 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -1155,11 +1155,11 @@ const clivalue_t valueTable[] = { #endif #ifdef USE_FEEDBACK_LINEARIZATION - { "torque_inertia_ratio", VAR_UINT16 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 1000 }, PG_PID_PROFILE, offsetof(pidProfile_t, torqueInertiaRatio) }, - { "pitch_torque_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchTorqueRatio) }, - { "yaw_torque_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawTorqueRatio) }, - { "pitch_inertia_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, pitchInertiaRatio) }, - { "yaw_inertia_ratio", VAR_UINT8 | PROFILE_VALUE, .config.minmaxUnsigned = { 0, 250 }, PG_PID_PROFILE, offsetof(pidProfile_t, yawInertiaRatio) }, + { "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 diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index d3debef211..791a17eecb 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -592,7 +592,7 @@ static uint16_t cmsx_torqueInertiaRatio; static uint8_t cmsx_pitchTorqueRatio; static uint8_t cmsx_yawTorqueRatio; static uint8_t cmsx_pitchInertiaRatio; -static uint8_t cmsx_yawInertiaRatio; +static uint16_t cmsx_yawInertiaRatio; #endif static const void *cmsx_profileOtherOnEnter(displayPort_t *pDisp) @@ -731,11 +731,11 @@ 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 - { "THR BOOST", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_torqueInertiaRatio, 0, 1000, 1 } , 0 }, - { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchTorqueRatio, 0, 250, 1 } , 0 }, - { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawTorqueRatio, 0, 250, 1 } , 0 }, - { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_pitchInertiaRatio, 0, 250, 1 } , 0 }, - { "THR BOOST", OME_UINT8, NULL, &(OSD_UINT8_t) { &cmsx_yawInertiaRatio, 0, 250, 1 } , 0 }, + { "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_UINT8_t) { &cmsx_yawInertiaRatio, 1, 500, 1 } , 0 }, #endif #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 5cfb81b019..8a166378c0 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -184,11 +184,11 @@ void resetPidProfile(pidProfile_t *pidProfile) .dyn_lpf_curve_expo = 5, .vbat_sag_compensation = 0, .dtermMeasurementSlider = 100, - .torqueInertiaRatio = 0, - .pitchTorqueRatio = 0, - .yawTorqueRatio = 0, - .pitchInertiaRatio = 0, - .yawInertiaRatio = 0, + .torqueInertiaRatio = 10000, + .pitchTorqueRatio = 100, + .yawTorqueRatio = 100, + .pitchInertiaRatio = 100, + .yawInertiaRatio = 100, .emuBoostPR = 15, .emuBoostY = 40, .dtermBoost = 0, @@ -981,9 +981,11 @@ void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) float k3 = pidRuntime.yawInertiaRatio; float kt2 = pidRuntime.pitchTorqueRatio; float kt3 = pidRuntime.yawTorqueRatio; - float TIR = pidRuntime.TorqueInertiaRatio; - pids[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); - pids[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); - pids[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); + float TIR = pidRuntime.torqueInertiaRatio; + if (!(kt2==0 || kt3==0 || TIR==0)){ + pids[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); + pids[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); + pids[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); + } } #endif diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index db17d0b0b8..e2f561392c 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -171,7 +171,7 @@ typedef struct pidProfile_s { uint8_t pitchTorqueRatio; uint8_t yawTorqueRatio; uint8_t pitchInertiaRatio; - uint8_t yawInertiaRatio; + uint16_t yawInertiaRatio; uint16_t emuBoostPR; uint16_t emuBoostY; @@ -285,7 +285,7 @@ typedef struct pidRuntime_s { float dtermMeasurementSlider; float dtermMeasurementSliderInverse; - float TorqueInertiaRatio; + float torqueInertiaRatio; float pitchTorqueRatio; float yawTorqueRatio; float pitchInertiaRatio; diff --git a/src/main/flight/pid_init.c b/src/main/flight/pid_init.c index 449affd63e..2b45f6e71a 100644 --- a/src/main/flight/pid_init.c +++ b/src/main/flight/pid_init.c @@ -247,7 +247,7 @@ 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.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; From 8716c8cc9970f8016d63c290e105b46cdda510b7 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Sat, 3 Jul 2021 00:55:55 -0600 Subject: [PATCH 06/12] added debugs --- src/main/build/debug.c | 1 + src/main/build/debug.h | 1 + src/main/cms/cms_menu_imu.c | 2 +- src/main/flight/pid.c | 3 +++ 4 files changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.c b/src/main/build/debug.c index 024a4885c2..3ac36351ee 100644 --- a/src/main/build/debug.c +++ b/src/main/build/debug.c @@ -103,4 +103,5 @@ const char * const debugModeNames[DEBUG_COUNT] = { "D_ABG_STATE", "RC_PREDICTOR", "SMITH_PREDICT", + "FEEDBACK_LINEARIZATION", }; diff --git a/src/main/build/debug.h b/src/main/build/debug.h index 068d5c5379..173facd10e 100644 --- a/src/main/build/debug.h +++ b/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; diff --git a/src/main/cms/cms_menu_imu.c b/src/main/cms/cms_menu_imu.c index 791a17eecb..c6e3713973 100644 --- a/src/main/cms/cms_menu_imu.c +++ b/src/main/cms/cms_menu_imu.c @@ -735,7 +735,7 @@ static const OSD_Entry cmsx_menuProfileOtherEntries[] = { { "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_UINT8_t) { &cmsx_yawInertiaRatio, 1, 500, 1 } , 0 }, + { "YAW INERTIA RATION", OME_UINT16, NULL, &(OSD_UINT16_t) { &cmsx_yawInertiaRatio, 1, 500, 1 } , 0 }, #endif #ifdef USE_BATTERY_VOLTAGE_SAG_COMPENSATION diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8a166378c0..1c2d3c5592 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -986,6 +986,9 @@ void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) pids[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); pids[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); pids[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 1, lrintf(pids[FD_ROLL].Sum * PID_MIXER_SCALING)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 2, lrintf(pids[FD_PITCH].Sum * PID_MIXER_SCALING)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 3, lrintf(pids[FD_YAW].Sum * PID_MIXER_SCALING)); } } #endif From 030ecc64088aeea1cf9cf1725ddb47b97a8d3005 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Sat, 3 Jul 2021 12:47:04 -0600 Subject: [PATCH 07/12] fixed improper scaling --- src/main/flight/mixer.c | 8 ++++---- src/main/flight/pid.c | 16 ++++++++++------ 2 files changed, 14 insertions(+), 10 deletions(-) diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 6d9f40bfff..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; @@ -505,10 +509,6 @@ FAST_CODE_NOINLINE void mixTable(timeUs_t currentTimeUs) float scaledAxisPidYaw = constrainf(pidData[FD_YAW].Sum, -yawPidSumLimit, yawPidSumLimit) / PID_MIXER_SCALING; -#ifdef USE_FEEDBACK_LINEARIZATION -applyFeedbackLinearization(pidData, gyro.gyroADCf); -#endif - if (!mixerConfig()->yaw_motors_reversed) { scaledAxisPidYaw = -scaledAxisPidYaw; } diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 1c2d3c5592..906815f62f 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -983,12 +983,16 @@ void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) float kt3 = pidRuntime.yawTorqueRatio; float TIR = pidRuntime.torqueInertiaRatio; if (!(kt2==0 || kt3==0 || TIR==0)){ - pids[FD_ROLL].Sum += (k3-k2) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR); - pids[FD_PITCH].Sum += (1.0f-k3) * 0.0003046f * gyroData[FD_YAW] * gyroData[FD_ROLL] / (PID_MIXER_SCALING * TIR * kt2); - pids[FD_YAW].Sum += (k2-1.0f) * 0.0003046f * gyroData[FD_ROLL] * gyroData[FD_PITCH] / (PID_MIXER_SCALING * TIR * kt3); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 1, lrintf(pids[FD_ROLL].Sum * PID_MIXER_SCALING)); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 2, lrintf(pids[FD_PITCH].Sum * PID_MIXER_SCALING)); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 3, lrintf(pids[FD_YAW].Sum * PID_MIXER_SCALING)); + 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); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 0, lrintf(1000.0f * (pids[FD_ROLL].Sum + roll_linearization) / pids[FD_ROLL].Sum)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 1, lrintf(1000.0f * (pids[FD_PITCH].Sum + pitch_linearization) / pids[FD_PITCH].Sum)); + DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 2, lrintf(1000.0f * (pids[FD_YAW].Sum + yaw_linearization) / pids[FD_YAW].Sum)); + + pids[FD_ROLL].Sum += roll_linearization; + pids[FD_PITCH].Sum += pitch_linearization; + pids[FD_YAW].Sum += yaw_linearization; } } #endif From 3d0f36e18b08049a9fddb995a3acacd5fccac580 Mon Sep 17 00:00:00 2001 From: Brendan Calef Date: Thu, 15 Jul 2021 18:26:10 -0600 Subject: [PATCH 08/12] Better debug calculation --- src/main/flight/pid.c | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 906815f62f..07682a66c2 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -986,13 +986,13 @@ void applyFeedbackLinearization(pidAxisData_t *pids, float *gyroData) 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); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 0, lrintf(1000.0f * (pids[FD_ROLL].Sum + roll_linearization) / pids[FD_ROLL].Sum)); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 1, lrintf(1000.0f * (pids[FD_PITCH].Sum + pitch_linearization) / pids[FD_PITCH].Sum)); - DEBUG_SET(DEBUG_FEEDBACK_LINEARIZATION, 2, lrintf(1000.0f * (pids[FD_YAW].Sum + yaw_linearization) / pids[FD_YAW].Sum)); - 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 From 1bfdd07b27b6f3216822574467f4c1bca9726161 Mon Sep 17 00:00:00 2001 From: UpMostBeast Date: Sat, 12 Mar 2022 16:44:37 -0700 Subject: [PATCH 09/12] Verify markups --- docs/FeedbackLinearization_notes.txt | 42 ++++++++++++++++++++++++++-- 1 file changed, 40 insertions(+), 2 deletions(-) diff --git a/docs/FeedbackLinearization_notes.txt b/docs/FeedbackLinearization_notes.txt index 9e3dcca20e..835a830566 100644 --- a/docs/FeedbackLinearization_notes.txt +++ b/docs/FeedbackLinearization_notes.txt @@ -1,2 +1,40 @@ -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 of Motion for a Rigid Body. As can be shown from the equation, rotation about two axis of quadcopter creates a torque on the third axis. The PID controller is a linear controller and is not designed to handle this kind of system. However, it is seen that in many practical applications a PID controller is sufficient with gain scheduling (TPA, Antigravity, EMUBoost etc). However, feedback linearization makes a non-linear system look linear from the perspective of the linear controller allowing it to perform the job it is designed for. 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 compensating rolls, hard 180 turns etc. Furthermore, it should help with propwash since propwash typically shows up on multiple axis at the same time. -The code is designed so that physical parameters of the quadcopter don't need to be directly known and non-dimensional constants which may be intuitively tuned by someone who isn't academically trained in the dynamics of a quadcopter. The first 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 pitch inertia and yawInertia ratios are almost always greater than one with the yaw ratio being larger than the pitch ratio (1 Date: Sat, 12 Mar 2022 16:48:10 -0700 Subject: [PATCH 10/12] Delete FeedbackLinearization_notes.txt Delete text file. To be replace with .md file --- docs/FeedbackLinearization_notes.txt | 40 ---------------------------- 1 file changed, 40 deletions(-) delete mode 100644 docs/FeedbackLinearization_notes.txt diff --git a/docs/FeedbackLinearization_notes.txt b/docs/FeedbackLinearization_notes.txt deleted file mode 100644 index 835a830566..0000000000 --- a/docs/FeedbackLinearization_notes.txt +++ /dev/null @@ -1,40 +0,0 @@ -# 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\ Date: Mon, 14 Mar 2022 13:34:16 -0600 Subject: [PATCH 11/12] Add .md file --- docs/Feedback_Linearization.md | 40 ++++++++++++++++++++++++++++++++++ 1 file changed, 40 insertions(+) create mode 100644 docs/Feedback_Linearization.md diff --git a/docs/Feedback_Linearization.md b/docs/Feedback_Linearization.md new file mode 100644 index 0000000000..d88fa0031b --- /dev/null +++ b/docs/Feedback_Linearization.md @@ -0,0 +1,40 @@ +# 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\ Date: Mon, 14 Mar 2022 13:53:44 -0600 Subject: [PATCH 12/12] Update Feedback_Linearization.md finalized the procedure for calculating the parameters including equations and descriptions of all the variables. Should be good enough for testing. --- docs/Feedback_Linearization.md | 20 +++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/docs/Feedback_Linearization.md b/docs/Feedback_Linearization.md index d88fa0031b..f498fd6a70 100644 --- a/docs/Feedback_Linearization.md +++ b/docs/Feedback_Linearization.md @@ -21,20 +21,22 @@ torqueInertiaPitch = torqueInertia \* pitchTorque / pitchInertia 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 +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 -Perform a hover test and obtain the following pieces of information: -1. Average current at hover -2. Average motor RPM at hover +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 = +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 = \ No newline at end of file +5. yawTorque = 1e6 * \(16.4 * Ih) / \(W * Kv * M * g)