Skip to content

Commit 782d11b

Browse files
akpamososuryoakpamososuryo
committed
[PitchGain.m] Major update -- see description
- Fixed the error showing up when gain or phase margins cannot be calculated; made notification for the user to increase gain if necessary - Included gain and phase margin cutoff frequencies in the Bode Plot legend - Refactored some parts of the code
1 parent 56de88e commit 782d11b

1 file changed

Lines changed: 77 additions & 33 deletions

File tree

‎subfunctions/PitchGain.m‎

Lines changed: 77 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -503,6 +503,7 @@ function UndockBode_pushbutton_Callback(hObject, eventdata, handles)
503503
function BodePlot(handles, undock, exportData)
504504
cla(handles.BodeMag_axes,'reset')
505505
cla(handles.BodePhase_axes,'reset')
506+
clc; % to clear all warnings
506507

507508
% Create vector of plot colors
508509
plotCol = linspace(0.8, 0, length(handles.SelectedListboxContents));
@@ -534,17 +535,15 @@ function BodePlot(handles, undock, exportData)
534535
LoopGain(1,i) = series(ControllerLG(:,i), Plant(:,i));
535536
end
536537

537-
w_llimit = -2; % change this limit if one of the margins can't be found
538+
w_llimit = -2;
538539
w_ulimit = 2;
539540
w = logspace(w_llimit, w_ulimit, 1000);
540541

541-
[ControllerLG_FRF, Controller_MagResponse, Controller_PhaseResponse] = ...
542+
[~, Controller_MagResponse, Controller_PhaseResponse] = ...
542543
calculateFreqResp(Controller, w);
543-
Controller_FRF = freqresp(Controller, w);
544544

545545
[LoopGainFRF, LoopGainMagResponse, LoopGainPhaseResponse] = ...
546546
calculateFreqResp(LoopGain, w);
547-
LoopGainMagResponseAbs = db2mag(LoopGainMagResponse); % for margins calculation
548547

549548
[PlantFRF, PlantMagResponse, PlantPhaseResponse] = ...
550549
calculateFreqResp(Plant, w);
@@ -553,7 +552,6 @@ function BodePlot(handles, undock, exportData)
553552
if size(Controller_MagResponse, 1) == 1
554553
Controller_MagResponse = Controller_MagResponse(:);
555554
Controller_PhaseResponse = Controller_PhaseResponse(:);
556-
LoopGainMagResponseAbs = LoopGainMagResponseAbs(:);
557555
LoopGainMagResponse = LoopGainMagResponse(:);
558556
LoopGainPhaseResponse = LoopGainPhaseResponse(:);
559557
PlantMagResponse = PlantMagResponse(:);
@@ -569,50 +567,96 @@ function BodePlot(handles, undock, exportData)
569567
'DelayMargin', cell_prealloc, ...
570568
'DMFrequency', cell_prealloc, ...
571569
'Stable', cell_prealloc);
572-
GM = zeros(1, length(handles.SelectedListboxContents));
570+
GM = nan(1, length(handles.SelectedListboxContents));
573571
PM = GM;
574572
GMFreq = GM;
575573
PMFreq = GM;
576-
for i = 1:length(handles.SelectedListboxContents)
577-
S(i) = allmargin(LoopGainMagResponseAbs(:,i),LoopGainPhaseResponse(:,i),w');
578-
GM(i) = mag2db(S(i).GainMargin(1));
579-
PM(i) = S(i).PhaseMargin(1);
580-
GMFreq(i) = S(i).GMFrequency(1);
581-
PMFreq(i) = S(i).PMFrequency(1);
574+
IsGMFreqWithinLimit = true(1, length(handles.SelectedListboxContents));
575+
IsPMFreqWithinLimit = IsGMFreqWithinLimit;
576+
warning('off','backtrace') % suppress warning
577+
if get(handles.PlotGMPM_checkbox,'value')
578+
LoopGainMagResponseAbs = db2mag(LoopGainMagResponse); % for margins calculation
579+
for i = 1:length(handles.SelectedListboxContents)
580+
S(i) = allmargin(LoopGainMagResponseAbs(:,i),LoopGainPhaseResponse(:,i),w');
581+
try
582+
GM(i) = mag2db(S(i).GainMargin(1));
583+
PM(i) = S(i).PhaseMargin(1);
584+
catch
585+
S(i) = allmargin(LoopGain(1,i));
586+
try
587+
GM(i) = mag2db(S(i).GainMargin(1));
588+
PM(i) = S(i).PhaseMargin(1);
589+
catch
590+
quest_msg = sprintf("Cannot obtain GM and/or PM for the model with pitch angle %.3f [deg]! Please try to increase the gain (Kp or Ki) and try again!", ...
591+
string(handles.SelectedListboxContents{i}));
592+
questdlg(quest_msg, 'Error', 'OK', 'OK');
593+
continue;
594+
end
595+
end
596+
597+
msg = "";
598+
GMFreq(i) = S(i).GMFrequency(1);
599+
IsGMFreqWithinLimit(i) = (log10(GMFreq(i)) >= w_llimit) && (log10(GMFreq(i)) <= w_ulimit);
600+
if ~IsGMFreqWithinLimit(i)
601+
msg = sprintf("Gain margin frequency is outside the figure axis limit: %.5f! [rad/s] ", ...
602+
GMFreq(i));
603+
end
604+
605+
PMFreq(i) = S(i).PMFrequency(1);
606+
IsPMFreqWithinLimit(i) = (log10(PMFreq(i)) >= w_llimit) && (log10(PMFreq(i)) <= w_ulimit);
607+
if ~IsPMFreqWithinLimit(i)
608+
msg = msg + sprintf("Phase margin frequency is outside the figure axis limit: %.5f! [rad/s] ", ...
609+
PMFreq(i));
610+
end
611+
612+
if ~IsGMFreqWithinLimit(i) || ~IsPMFreqWithinLimit(i)
613+
msg = "Cannot draw one or more stability margins! " + msg;
614+
warning(msg);
615+
end
616+
end
582617
end
618+
warning('on','backtrace')
583619

584620
if get(handles.PlotGMPM_checkbox,'value')
585-
legendMargins = cell(length(handles.SelectedListboxContents), 6);
621+
legendMargins = cell(length(handles.SelectedListboxContents), 12);
586622
legendMargins(:,1) = {' GM: '};
587623
legendMargins(:,3) = {' [dB] '};
588-
legendMargins(:,4) = {' PM: '};
589-
legendMargins(:,6) = {' [deg] '};
590-
591-
if length(handles.SelectedListboxContents) == 1
592-
legendMargins{2} = num2str(GM,'%.2f');
593-
legendMargins{5} = num2str(PM,'%.2f');
594-
legendMargins = strcat(legendMargins{1}, legendMargins{2}, ...
595-
legendMargins{3}, legendMargins{4}, legendMargins{5}, ...
596-
legendMargins{6});
597-
else
598-
for i = 1:length(handles.SelectedListboxContents)
599-
legendMargins{i,2} = num2str(GM(i));
600-
legendMargins{i,5} = num2str(PM(i));
601-
end
602-
legendMargins = strcat(legendMargins(:,1), legendMargins(:,2), ...
603-
legendMargins(:,3), legendMargins(:,4), legendMargins(:,5), ...
604-
legendMargins(:,6));
624+
legendMargins(:,4) = {' \omega_{gc}: '};
625+
legendMargins(:,6) = {' [rad/s] '};
626+
legendMargins(:,7) = {' PM: '};
627+
legendMargins(:,9) = {' [deg] '};
628+
legendMargins(:,10) = {' \omega_{pc}: '};
629+
legendMargins(:,12) = {' [rad/s] '};
630+
631+
for i = 1:length(handles.SelectedListboxContents)
632+
legendMargins{i,2} = num2str(GM(i),'%.2f');
633+
legendMargins{i,5} = num2str(GMFreq(i),'%.4f');
634+
legendMargins{i,8} = num2str(PM(i), '%.2f');
635+
legendMargins{i,11} = num2str(PMFreq(i),'%.4f');
605636
end
637+
legendMargins = strcat(legendMargins(:,1), legendMargins(:,2), ...
638+
legendMargins(:,3), legendMargins(:,4), legendMargins(:,5), ...
639+
legendMargins(:,6), legendMargins(:,7), legendMargins(:,8), ...
640+
legendMargins(:,9), legendMargins(:,10), legendMargins(:,11), ...
641+
legendMargins(:,12));
606642
end
607643

608644
if exportData
645+
ControllerLG_FRF = freqresp(ControllerLG, w);
646+
609647
frd_Plant = frd(PlantFRF, w);
610648
frd_Controller = frd(ControllerLG_FRF, w);
611649
frd_LoopGain = frd(LoopGainFRF, w);
612650

613651
PitchAngles = str2double(handles.SelectedListboxContents);
614652

615-
uisave({'frd_Plant', 'frd_Controller', 'frd_LoopGain', 'PitchAngles', 'GM', 'PM', 'GMFreq', 'PMFreq'}, 'ExportPlotData')
653+
variablesToSave = {'frd_Plant', 'frd_Controller', 'frd_LoopGain', 'PitchAngles'};
654+
655+
if get(handles.PlotGMPM_checkbox,'value')
656+
variablesToSave = [variablesToSave {'GM', 'PM', 'GMFreq', 'PMFreq'}];
657+
end
658+
659+
uisave(variablesToSave, 'ExportPlotData');
616660
end
617661

618662
if undock
@@ -633,7 +677,7 @@ function BodePlot(handles, undock, exportData)
633677
hold on
634678
end
635679
if get(handles.PlotLoopGain_checkbox, 'Value')
636-
if get(handles.PlotGMPM_checkbox,'value') && (log10(GMFreq(i)) <= w_ulimit) && (log10(GMFreq(i)) >= w_llimit)
680+
if get(handles.PlotGMPM_checkbox,'value') && IsGMFreqWithinLimit(i)
637681
h(i) = semilogx([GMFreq(i) GMFreq(i)], [0 -GM(i)], 'Color', [0 0.8 0], 'LineStyle', plotLineStyle{2}, 'LineWidth', plotLineWidth(1));
638682
h(i) = semilogx(GMFreq(i), -GM(i), 'o', 'Color', [0 0.8 0], 'LineStyle', plotLineStyle{1}, 'LineWidth', plotLineWidth(1));
639683

@@ -673,7 +717,7 @@ function BodePlot(handles, undock, exportData)
673717
if get(handles.PlotLoopGain_checkbox, 'Value')
674718
semilogx(w, LoopGainPhaseResponse(:,i), 'Color', ones(1,3)*plotCol(i), 'LineStyle', plotLineStyle{3}, 'LineWidth', plotLineWidth(3));
675719
hold on
676-
if get(handles.PlotGMPM_checkbox,'value') && (log10(PMFreq(i)) <= w_ulimit) && (log10(PMFreq(i)) >= w_llimit)
720+
if get(handles.PlotGMPM_checkbox,'value') && IsPMFreqWithinLimit(i)
677721
if PM(i) >= 0
678722
dotPM = -180 + PM(i);
679723
else

0 commit comments

Comments
 (0)