143 lines
5.0 KiB
Matlab
Executable File
143 lines
5.0 KiB
Matlab
Executable File
% Funzione che converte i dati grezzi dei Klino Link in dati di
|
|
% angoli
|
|
|
|
function [ang_KL,temp_KL,ris_acc,ErrKlinoLink] = conv_grezziKL(ang_KL,temp_KL,...
|
|
rKL,DCalKLTot,IDcentralina,DTcatena,NodoKlinoLink,ErrKlinoLink,FileName)
|
|
|
|
text = 'conv_grezziKL function started';
|
|
fileID = fopen(FileName,'a');
|
|
fmt = '%s \r';
|
|
fprintf(fileID,fmt,text);
|
|
|
|
if strcmp(IDcentralina,'ID0231') == 1 && strcmp(DTcatena,'DT0251') == 1
|
|
angoloKL = real(asind(ang_KL(:,1:3))); %calcolo angolo relativo
|
|
ang_KL(:,1:3) = angoloKL;
|
|
|
|
%% Accelerometri
|
|
cont = 1; % contatore
|
|
cn = 0;
|
|
[rAcc,cAcc] = size(ang_KL);
|
|
ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
|
|
|
|
for ii = 1:(cAcc/3) % colonne
|
|
for i = 1:rAcc % righe
|
|
ris_acc(i,cont) = (ang_KL(i,cn*3+1)^2+ang_KL(i,cn*3+2)^2+ang_KL(i,cn*3+3)^2)^0.5;
|
|
end
|
|
cn = cn+1;
|
|
cont = cont+1;
|
|
end
|
|
ris_acc = sind(ris_acc);
|
|
|
|
elseif strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
|
|
|| isempty(NodoKlinoLink{1,4}) == 1 || strcmp(NodoKlinoLink(1,4),'g')
|
|
|
|
if strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
|
|
|| isempty(NodoKlinoLink{1,4}) == 1
|
|
caX = DCalKLTot(:,1);
|
|
caY = DCalKLTot(:,4);
|
|
caZ = DCalKLTot(:,7);
|
|
pIntX = DCalKLTot(:,2);
|
|
pIntY = DCalKLTot(:,5);
|
|
pIntZ = DCalKLTot(:,8);
|
|
iIntX = DCalKLTot(:,3);
|
|
iIntY = DCalKLTot(:,6);
|
|
iIntZ = DCalKLTot(:,9);
|
|
caT = DCalKLTot(:,10);
|
|
intT = DCalKLTot(:,11);
|
|
end
|
|
|
|
if strcmp(NodoKlinoLink(1,4),'g') == 0
|
|
%% Accelerometri
|
|
cont = 1;
|
|
cn = 1;
|
|
n = 3;
|
|
% Contatore dei nodi, corregge le accelerazioni con le calibrazioni
|
|
for ii=1:n*rKL
|
|
if cont==1
|
|
ang_KL(:,ii) = ang_KL(:,ii)*caX(cn)+(temp_KL(:,cn)*pIntX(cn)+iIntX(cn));
|
|
cont = cont+1;
|
|
elseif cont==2
|
|
ang_KL(:,ii) = ang_KL(:,ii)*caY(cn)+(temp_KL(:,cn)*pIntY(cn)+iIntY(cn));
|
|
cont = cont+1;
|
|
else
|
|
ang_KL(:,ii) = ang_KL(:,ii)*caZ(cn)+(temp_KL(:,cn)*pIntZ(cn)+iIntZ(cn));
|
|
cont = 1;
|
|
cn = cn+1;
|
|
end
|
|
end
|
|
end
|
|
|
|
angoloKL = real(asind(ang_KL)); %calcolo angolo relativo
|
|
|
|
% % Calcolo e correggo angoli relativi in base al segno dell'asse Z
|
|
% cont = 1;
|
|
% contACC = 1;
|
|
% angoloKL = ang_KL;
|
|
% for ii=1:3*rKL
|
|
% if cont == 1 % X
|
|
% angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); %calcolo angolo relativo
|
|
% if angoloKL(:,contACC) > 0 %check quando angolo relativo > 0
|
|
% if ang_KL(:,ii+2) < 0 % check quando asse z < 0 --> il montante si è capovolto!
|
|
% angoloKL(:,contACC) = pi - angoloKL(:,contACC);
|
|
% end
|
|
% elseif angoloKL(:,ii) < 0 % check quando angolo relativo < 0
|
|
% if ang_KL(:,ii+2) < 0 % check quando asse z < 0 --> il montante si è capovolto!
|
|
% angoloKL(:,contACC) = -pi - angoloKL(:,contACC);
|
|
% end
|
|
% end
|
|
% cont = cont+1;
|
|
% contACC = contACC+1;
|
|
% elseif cont == 2 % Y
|
|
% angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); % calcolo angolo relativo
|
|
% if angoloKL(:,contACC) > 0 %check quando angolo relativo > 0
|
|
% if ang_KL(:,ii+1) < 0 % check quando asse z < 0 --> il montante si è capovolto!
|
|
% angoloKL(:,contACC) = pi - angoloKL(:,contACC);
|
|
% end
|
|
% elseif angoloKL(:,ii) < 0 %check quando angolo relativo < 0
|
|
% if ang_KL(:,ii+1) < 0 % check quando asse z < 0 --> il montante si è capovolto!
|
|
% angoloKL(:,contACC) = -pi - angoloKL(:,contACC);
|
|
% end
|
|
% end
|
|
% cont = cont+1;
|
|
% contACC = contACC+1;
|
|
% elseif cont == 3 % Z
|
|
% angoloKL(:,contACC) = real(asind(ang_KL(:,ii))); % calcolo angolo relativo
|
|
% cont = 1;
|
|
% contACC = contACC+1;
|
|
% end
|
|
% end
|
|
|
|
%% Conversione delle temperature
|
|
if strcmp(NodoKlinoLink(1,4),'g') == 0
|
|
for t = 1:rKL
|
|
temp_KL(:,t) = temp_KL(:,t)*caT(t,1) + intT(t,1);
|
|
end
|
|
end
|
|
|
|
%% Risultanti
|
|
clear ii
|
|
clear cont
|
|
clear cn
|
|
cont = 1; % contatore
|
|
cn = 0;
|
|
[rAcc,cAcc] = size(ang_KL);
|
|
ris_acc = zeros(rAcc,cAcc/3); % matrice risultante accelerazioni
|
|
|
|
for ii = 1:(cAcc/3) % colonne
|
|
for i = 1:rAcc % righe
|
|
ris_acc(i,cont) = (ang_KL(i,cn*3+1)^2+ang_KL(i,cn*3+2)^2+ang_KL(i,cn*3+3)^2)^0.5;
|
|
end
|
|
cn = cn+1;
|
|
cont = cont+1;
|
|
end
|
|
|
|
ang_KL = angoloKL;
|
|
else
|
|
ris_acc = [];
|
|
end
|
|
|
|
text = 'Raw Data of Klino Link converted into physical units correctly. conv_grezziKL function ended';
|
|
fprintf(fileID,fmt,text);
|
|
fclose(fileID);
|
|
|
|
end |