Skip to content

Commit 7c0db59

Browse files
haslinghuishaslinghuis
committed
Fix legacy PID Tuning Sliders
1 parent 3b26ca6 commit 7c0db59

File tree

1 file changed

+62
-36
lines changed

1 file changed

+62
-36
lines changed

‎src/js/TuningSliders.js‎

Lines changed: 62 additions & 36 deletions
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,8 @@ const TuningSliders = {
1616
sliderDTermFilter: 0,
1717
sliderDTermFilterMultiplier: 1,
1818

19+
dMinFeatureEnabled: true,
20+
defaultPDRatio: 0,
1921
PID_DEFAULT: [],
2022
FILTER_DEFAULT: {},
2123

@@ -28,10 +30,23 @@ const TuningSliders = {
2830

2931
const D_MIN_RATIO = 0.85;
3032

33+
TuningSliders.setDMinFeatureEnabled = function(dMinFeatureEnabled) {
34+
this.dMinFeatureEnabled = dMinFeatureEnabled;
35+
if (this.dMinFeatureEnabled) {
36+
this.defaultPDRatio = this.PID_DEFAULT[2] / this.PID_DEFAULT[0];
37+
} else {
38+
this.defaultPDRatio = this.PID_DEFAULT[2] / (this.PID_DEFAULT[0] * (1 / D_MIN_RATIO));
39+
}
40+
};
41+
3142
TuningSliders.initialize = function() {
3243
this.PID_DEFAULT = FC.getPidDefaults();
3344
this.FILTER_DEFAULT = FC.getFilterDefaults();
3445

46+
if (semver.lt(FC.CONFIG.apiVersion, API_VERSION_1_44)) {
47+
this.setDMinFeatureEnabled($('#dMinSwitch').is(':checked'));
48+
}
49+
3550
this.setExpertMode($('input[name="expertModeCheckbox"]').is(':checked'));
3651

3752
this.initPidSlidersPosition();
@@ -76,10 +91,14 @@ TuningSliders.initPidSlidersPosition = function() {
7691
if (semver.lte(FC.CONFIG.apiVersion, API_VERSION_1_43)) {
7792
// used to estimate PID slider positions based on PIDF values, and set respective slider position
7893
// provides only an estimation due to limitation of feature without firmware support, to be improved in later versions
79-
this.sliderMasterMultiplier = Math.floor(FC.PIDS[2][1] / this.PID_DEFAULT[11] * 10) / 10;
80-
this.sliderPDRatio = Math.floor(FC.PIDS[0][0] / FC.PIDS[0][2] / (this.PID_DEFAULT[0] / this.PID_DEFAULT[2]) * 10) / 10;
81-
this.sliderPDGain = Math.floor(FC.ADVANCED_TUNING.dMinRoll / this.sliderMasterMultiplier / this.PID_DEFAULT[3] * 10) / 10;
82-
this.sliderFFGain = Math.floor(FC.ADVANCED_TUNING.feedforwardRoll / this.sliderMasterMultiplier / this.PID_DEFAULT[4] * 10) / 10;
94+
this.sliderMasterMultiplier = Math.round(FC.PIDS[2][1] / this.PID_DEFAULT[11] * 10) / 10;
95+
this.sliderPDRatio = Math.round(FC.PIDS[0][2] / FC.PIDS[0][0] / this.defaultPDRatio * 10) / 10;
96+
if (this.dMinFeatureEnabled) {
97+
this.sliderPDGain = Math.round(FC.ADVANCED_TUNING.dMinRoll / this.sliderPDRatio / this.sliderMasterMultiplier / this.PID_DEFAULT[3] * 10) / 10;
98+
} else {
99+
this.sliderPDGain = Math.round(FC.PIDS[0][0] / this.sliderMasterMultiplier / (this.PID_DEFAULT[2] * (1 / D_MIN_RATIO)) * 10) / 10;
100+
}
101+
this.sliderFFGain = Math.round(FC.ADVANCED_TUNING.feedforwardRoll / this.sliderMasterMultiplier / this.PID_DEFAULT[4] * 10) / 10;
83102
} else {
84103
this.sliderPidsMode = FC.TUNING_SLIDERS.slider_pids_mode;
85104
this.sliderMasterMultiplier = FC.TUNING_SLIDERS.slider_master_multiplier / 100;
@@ -302,22 +321,25 @@ TuningSliders.legacyCalculateNewPids = function() {
302321
// only used for 4.1 where calculation is not done in firmware
303322
if (this.dMinFeatureEnabled) {
304323
//dmin
305-
FC.ADVANCED_TUNING.dMinRoll = Math.floor(this.PID_DEFAULT[3] * this.sliderPDGain);
306-
FC.ADVANCED_TUNING.dMinPitch = Math.floor(this.PID_DEFAULT[8] * this.sliderPDGain);
324+
FC.ADVANCED_TUNING.dMinRoll = Math.floor(this.PID_DEFAULT[3] * this.sliderPDGain * this.sliderPDRatio);
325+
FC.ADVANCED_TUNING.dMinPitch = Math.floor(this.PID_DEFAULT[8] * this.sliderPDGain * this.sliderPDRatio);
307326
// dmax
308-
FC.PIDS[0][2] = Math.floor(this.PID_DEFAULT[2] * this.sliderPDGain);
309-
FC.PIDS[1][2] = Math.floor(this.PID_DEFAULT[7] * this.sliderPDGain);
327+
FC.PIDS[0][2] = Math.floor(this.PID_DEFAULT[2] * this.sliderPDGain * this.sliderPDRatio);
328+
FC.PIDS[1][2] = Math.floor(this.PID_DEFAULT[7] * this.sliderPDGain * this.sliderPDRatio);
310329
} else {
311330
FC.ADVANCED_TUNING.dMinRoll = 0;
312331
FC.ADVANCED_TUNING.dMinPitch = 0;
313-
FC.PIDS[0][2] = Math.floor(this.PID_DEFAULT[3] * this.sliderPDGain);
314-
FC.PIDS[1][2] = Math.floor(this.PID_DEFAULT[8] * this.sliderPDGain);
332+
FC.PIDS[0][2] = Math.floor((this.PID_DEFAULT[2] * D_MIN_RATIO) * this.sliderPDGain * this.sliderPDRatio);
333+
FC.PIDS[1][2] = Math.floor((this.PID_DEFAULT[7] * D_MIN_RATIO) * this.sliderPDGain * this.sliderPDRatio);
315334
}
316335

317-
FC.PIDS[0][0] = Math.floor(FC.PIDS[0][2] * this.PID_DEFAULT[0] / this.PID_DEFAULT[2] * this.sliderPDRatio);
318-
FC.PIDS[1][0] = Math.floor(FC.PIDS[1][2] * this.PID_DEFAULT[5] / this.PID_DEFAULT[7] * this.sliderPDRatio);
336+
FC.PIDS[0][0] = Math.floor(this.PID_DEFAULT[0] * this.sliderPDGain);
337+
FC.PIDS[1][0] = Math.floor(this.PID_DEFAULT[5] * this.sliderPDGain);
319338
FC.PIDS[2][0] = Math.floor(this.PID_DEFAULT[10] * this.sliderPDGain);
320-
339+
// ff
340+
FC.ADVANCED_TUNING.feedforwardRoll = Math.round(this.PID_DEFAULT[4] * this.sliderFFGain);
341+
FC.ADVANCED_TUNING.feedforwardPitch = Math.round(this.PID_DEFAULT[9] * this.sliderFFGain);
342+
FC.ADVANCED_TUNING.feedforwardYaw = Math.round(this.PID_DEFAULT[14] * this.sliderFFGain);
321343
// master slider part
322344
// these are not calculated anywhere other than master slider multiplier therefore set at default before every calculation
323345
FC.PIDS[0][1] = this.PID_DEFAULT[1];
@@ -330,13 +352,14 @@ TuningSliders.legacyCalculateNewPids = function() {
330352
//master slider multiplication, max value 200 for main PID values
331353
for (let i = 0; i < 3; i++) {
332354
for (let j = 0; j < 3; j++) {
333-
FC.PIDS[j][i] = Math.min(Math.floor(FC.PIDS[j][i] * this.sliderMasterMultiplier), MAX_PID_GAIN);
355+
FC.PIDS[j][i] = Math.min(Math.round(FC.PIDS[j][i] * this.sliderMasterMultiplier), MAX_PID_GAIN);
334356
}
335357
}
336-
// ff
337-
FC.ADVANCED_TUNING.feedforwardRoll = Math.min(Math.floor(this.PID_DEFAULT[4] * this.sliderFFGain * this.sliderMasterMultiplier), MAX_FF_GAIN);
338-
FC.ADVANCED_TUNING.feedforwardPitch = Math.min(Math.floor(this.PID_DEFAULT[9] * this.sliderFFGain * this.sliderMasterMultiplier), MAX_FF_GAIN);
339-
FC.ADVANCED_TUNING.feedforwardYaw = Math.min(Math.floor(this.PID_DEFAULT[14] * this.sliderFFGain * this.sliderMasterMultiplier), MAX_FF_GAIN);
358+
359+
FC.ADVANCED_TUNING.feedforwardRoll = Math.min(Math.round(FC.ADVANCED_TUNING.feedforwardRoll * this.sliderMasterMultiplier), MAX_FF_GAIN);
360+
FC.ADVANCED_TUNING.feedforwardPitch = Math.min(Math.round(FC.ADVANCED_TUNING.feedforwardPitch * this.sliderMasterMultiplier), MAX_FF_GAIN);
361+
FC.ADVANCED_TUNING.feedforwardYaw = Math.min(Math.round(FC.ADVANCED_TUNING.feedforwardYaw * this.sliderMasterMultiplier), MAX_FF_GAIN);
362+
340363
if (this.dMinFeatureEnabled) {
341364
FC.ADVANCED_TUNING.dMinRoll = Math.min(Math.floor(FC.ADVANCED_TUNING.dMinRoll * this.sliderMasterMultiplier), MAX_DMIN_GAIN);
342365
FC.ADVANCED_TUNING.dMinPitch = Math.min(Math.floor(FC.ADVANCED_TUNING.dMinPitch * this.sliderMasterMultiplier), MAX_DMIN_GAIN);
@@ -368,24 +391,27 @@ TuningSliders.calculateNewPids = function() {
368391
$('output[name="sliderDMinRatio-number"]').val(this.sliderDMinRatio);
369392
$('output[name="sliderFFGain-number"]').val(this.sliderFFGain);
370393

371-
FC.TUNING_SLIDERS.slider_pids_mode = parseInt($('#sliderPidsModeSelect').val());
372-
FC.TUNING_SLIDERS.slider_master_multiplier = TuningSliders.sliderMasterMultiplier * 100;
373-
FC.TUNING_SLIDERS.slider_roll_pitch_ratio = TuningSliders.sliderRollPitchRatio * 100;
374-
FC.TUNING_SLIDERS.slider_i_gain = TuningSliders.sliderIGain * 100;
375-
FC.TUNING_SLIDERS.slider_pd_ratio = TuningSliders.sliderPDRatio * 100;
376-
FC.TUNING_SLIDERS.slider_pd_gain = TuningSliders.sliderPDGain * 100;
377-
FC.TUNING_SLIDERS.slider_dmin_ratio = TuningSliders.sliderDMinRatio * 100;
378-
FC.TUNING_SLIDERS.slider_ff_gain = TuningSliders.sliderFFGain * 100;
379-
380-
FC.TUNING_SLIDERS.slider_dterm_filter = TuningSliders.sliderDTermFilter ? 1 : 0;
381-
FC.TUNING_SLIDERS.slider_dterm_filter_multiplier = TuningSliders.sliderDTermFilterMultiplier * 100;
382-
383-
FC.TUNING_SLIDERS.slider_gyro_filter = TuningSliders.sliderGyroFilter ? 1 : 0;
384-
FC.TUNING_SLIDERS.slider_gyro_filter_multiplier = TuningSliders.sliderGyroFilterMultiplier * 100;
385-
386-
MSP.promise(MSPCodes.MSP_SET_TUNING_SLIDERS, mspHelper.crunch(MSPCodes.MSP_SET_TUNING_SLIDERS));
387-
MSP.send_message(MSPCodes.MSP_PID);
388-
MSP.send_message(MSPCodes.MSP_PID_ADVANCED);
394+
if (semver.gte(FC.CONFIG.apiVersion, API_VERSION_1_44)) {
395+
FC.TUNING_SLIDERS.slider_pids_mode = parseInt($('#sliderPidsModeSelect').val());
396+
FC.TUNING_SLIDERS.slider_master_multiplier = TuningSliders.sliderMasterMultiplier * 100;
397+
FC.TUNING_SLIDERS.slider_roll_pitch_ratio = TuningSliders.sliderRollPitchRatio * 100;
398+
FC.TUNING_SLIDERS.slider_i_gain = TuningSliders.sliderIGain * 100;
399+
FC.TUNING_SLIDERS.slider_pd_ratio = TuningSliders.sliderPDRatio * 100;
400+
FC.TUNING_SLIDERS.slider_pd_gain = TuningSliders.sliderPDGain * 100;
401+
FC.TUNING_SLIDERS.slider_dmin_ratio = TuningSliders.sliderDMinRatio * 100;
402+
FC.TUNING_SLIDERS.slider_ff_gain = TuningSliders.sliderFFGain * 100;
403+
404+
FC.TUNING_SLIDERS.slider_dterm_filter = TuningSliders.sliderDTermFilter ? 1 : 0;
405+
FC.TUNING_SLIDERS.slider_dterm_filter_multiplier = TuningSliders.sliderDTermFilterMultiplier * 100;
406+
407+
FC.TUNING_SLIDERS.slider_gyro_filter = TuningSliders.sliderGyroFilter ? 1 : 0;
408+
FC.TUNING_SLIDERS.slider_gyro_filter_multiplier = TuningSliders.sliderGyroFilterMultiplier * 100;
409+
410+
Promise.resolve(true)
411+
.then(() => { return MSP.promise(MSPCodes.MSP_SET_TUNING_SLIDERS, mspHelper.crunch(MSPCodes.MSP_SET_TUNING_SLIDERS)); })
412+
.then(() => { return MSP.send_message(MSPCodes.MSP_SET_PID); })
413+
.then(() => { return MSP.send_message(MSPCodes.MSP_SET_PID_ADVANCED); });
414+
}
389415

390416
this.updateFormPids();
391417

0 commit comments

Comments
 (0)