256 lines
8.6 KiB
Matlab
Executable File
256 lines
8.6 KiB
Matlab
Executable File
function [ANGdefKL,TdefKL,ErrKlinoLink,ARRAYdateKL] = Klino(ANGdefKL,TdefKL,...
|
|
ARRAYdateKL,ErrKlinoLink,NuovoZeroKL,NdatiMedia,Ndatidespike,ris_acc,tolleranzaAcc,...
|
|
Tmax,Tmin,datainiKL,rKL,IDcentralina,DTcatena,margine,NodoKlinoLink,FileName)
|
|
|
|
fileID = fopen(FileName,'a');
|
|
fmt = '%s \r';
|
|
text = 'Klino function started';
|
|
fprintf(fileID,fmt,text);
|
|
|
|
if NuovoZeroKL == 1
|
|
if NdatiMedia > Ndatidespike
|
|
Ndati = NdatiMedia;
|
|
else
|
|
Ndati = Ndatidespike;
|
|
end
|
|
ini = round(Ndati/2)+1;
|
|
if rem(Ndati,2) == 0
|
|
ini = ini+1;
|
|
end
|
|
clear NDati
|
|
ini = ini + margine;
|
|
if ini < 6
|
|
ini = 6;
|
|
end
|
|
[letture,~]=size(ANGdefKL);
|
|
if ini > letture
|
|
ini = 1;
|
|
end
|
|
ErrKlinoLink = ErrKlinoLink(ini:end,:);
|
|
ANGdefKL = ANGdefKL(ini:end,:);
|
|
TdefKL = TdefKL(ini:end,:);
|
|
ARRAYdateKL = ARRAYdateKL(ini:end,1);
|
|
ris_acc = ris_acc(ini:end,:);
|
|
end
|
|
|
|
if strcmp(NodoKlinoLink{1,4} ,'Gradi')
|
|
TempDef = TdefKL';
|
|
Angolo = ANGdefKL';
|
|
else
|
|
%% Controllo delle risultanti di accelerazione
|
|
clear r
|
|
clear rr
|
|
clear c
|
|
clear cc
|
|
[r,c] = size(ris_acc);
|
|
[rr,cc] = size(ANGdefKL);
|
|
% controllo che le matrici con le risultanti delle accelerazioni e
|
|
% le matrici con i dati di angolo di inclinazione
|
|
% abbiano le stesse dimensioni (r con cc e c con rr perchè matrice
|
|
% ris_acc è trasposta!)
|
|
|
|
if c~=cc/3
|
|
text = '--- Warning! Number of columns of angle data do not correspond to the number of acceleration cosine vector! ---';
|
|
fprintf(fileID,fmt,text);
|
|
end
|
|
if r~=rr
|
|
text = '--- Warning! Number of rows of angle data do not correspond to the number of acceleration cosine vector! ---';
|
|
fprintf(fileID,fmt,text);
|
|
end
|
|
clear i
|
|
clear j
|
|
cont = 1; % contatore
|
|
cont3 = 1; % contatore
|
|
ris_acc = ris_acc'; % Nodi in riga, date in colonna
|
|
TempDef = TdefKL';
|
|
Angolo = ANGdefKL';
|
|
textA = 'There are not correction of Klino Link based on acceleration vectors filter';
|
|
textA2 = 'There are not correction of Klino Link based on uncalibrated acceleration vectors';
|
|
textT = 'There are not correction of Klino Link based on temperature filter';
|
|
for j = 2:r % Data
|
|
nodo = 1;
|
|
for i = 1:c % Nodo
|
|
% se il valore assoluto della differenza è maggiore della
|
|
% tolleranza, pongo l'angolo pari al valore precendete
|
|
if abs(ris_acc(i,j)-ris_acc(i,j-1)) > tolleranzaAcc
|
|
Angolo(nodo:nodo+2,j) = Angolo(nodo:nodo+2,j-1);
|
|
textA = ['' num2str(cont) ' correction executed for Klino Link - Acceleration vector filter!'];
|
|
cont = cont+1;
|
|
end
|
|
if ris_acc(i,j) < 0.9 || ris_acc(i,j) > 1.1 % Il nodo è fuori taratura!
|
|
Angolo(nodo:nodo+2,j) = Angolo(nodo:nodo+2,j-1);
|
|
TempDef(i,j) = TempDef(i,j-1);
|
|
textA2 = ['' num2str(cont) ' correction executed for Klino Link - uncalibrated Acceleration vector!'];
|
|
cont3 = cont3+1;
|
|
end
|
|
nodo = nodo+3;
|
|
end
|
|
end
|
|
fprintf(fileID,fmt,textA);
|
|
fprintf(fileID,fmt,textA2);
|
|
end
|
|
|
|
%% Controllo della temperatura
|
|
FileTemperature = ['' IDcentralina '-' DTcatena '-KL-Therm.csv'];
|
|
if isfile(FileTemperature) == 1
|
|
DatiRaw = csvread(FileTemperature);
|
|
[rDR,cDR] = size(DatiRaw);
|
|
DatiRaw(:,1) = DatiRaw(:,1) + 730000;
|
|
else
|
|
rDR = 1;
|
|
cDR = 1;
|
|
end
|
|
[~,cEr] = size(ErrKlinoLink);
|
|
[rr,c] = size(TdefKL);
|
|
col = cEr/rKL;
|
|
cont2 = 1;
|
|
for j = 1:rr % Data
|
|
jj = col;
|
|
nodo = 1;
|
|
for i = 1:c % Nodo
|
|
% NON considero i dati al di sopra di Tmax o al di sotto di Tmin
|
|
if TempDef(i,j) > Tmax || TempDef(i,j) < Tmin
|
|
cont2 = cont2+1;
|
|
if j == 1
|
|
if isfile(FileTemperature) == 1
|
|
RawDate = find(DatiRaw(:,1)<=datenum(datainiKL));
|
|
if isempty(RawDate) == 1
|
|
cc = 2;
|
|
while cc <= Ndate
|
|
if TempDef(i,cc) > Tmax || TempDef(i,cc) < Tmin
|
|
cc = cc+1;
|
|
else
|
|
break
|
|
end
|
|
end
|
|
TempDef(i,j) = TempDef(i,cc);
|
|
else
|
|
if isnan(DatiRaw(RawDate(end),i)) == 0
|
|
TempDef(i,j) = DatiRaw(RawDate(end),i+1);
|
|
ErrKlinoLink(j,jj) = 0.5;
|
|
wardat = 'Temperature data of Klino Link nodes corrected using Raw Data of reference Csv file.';
|
|
fprintf(fileID,fmt,wardat);
|
|
else
|
|
cc = 2;
|
|
while cc <= c
|
|
if TempDef(i,cc) > Tmax || TempDef(i,cc) < Tmin
|
|
cc = cc+1;
|
|
else
|
|
break
|
|
end
|
|
end
|
|
TempDef(i,j) = TempDef(i,cc);
|
|
end
|
|
end
|
|
else
|
|
cc = 2;
|
|
while cc <= c
|
|
if TempDef(i,cc) > Tmax || TempDef(i,cc) < Tmin
|
|
cc = cc+1;
|
|
else
|
|
break
|
|
end
|
|
end
|
|
TempDef(i,j) = TempDef(i,cc);
|
|
end
|
|
else
|
|
if col == 3
|
|
Angolo(nodo:nodo+1,j) = Angolo(nodo:nodo+1,j-1);
|
|
nodo = nodo+2;
|
|
elseif col == 4
|
|
Angolo(nodo:nodo+2,j) = Angolo(nodo:nodo+2,j-1);
|
|
nodo = nodo+3;
|
|
end
|
|
TempDef(i,j) = TempDef(i,j-1);
|
|
ErrKlinoLink(j,jj) = 0.5;
|
|
end
|
|
end
|
|
textT = ['' num2str(cont2) ' correction executed for Klino Link - Temperature filter!'];
|
|
jj = jj+cEr/rKL;
|
|
end
|
|
end
|
|
|
|
if rDR~=1 && cDR~=1 && isempty(DatiRaw) == 0
|
|
RawDate1 = find(DatiRaw(:,1)<=ARRAYdateKL(1));
|
|
if isempty(RawDate1) == 1
|
|
RawDate2 = 1;
|
|
elseif RawDate1(end) == rDR
|
|
RawDate2 = find(ARRAYdateKL(:,1)>DatiRaw(end,1));
|
|
else
|
|
RawDate2 = find(ARRAYdateKL(:,1)>DatiRaw(RawDate1(end)+1,1));
|
|
end
|
|
else
|
|
RawDate1 = [];
|
|
RawDate2 = 1;
|
|
end
|
|
if isempty(RawDate1) == 0 && isempty(RawDate2) == 0
|
|
Dati = [DatiRaw(1:RawDate1(end),:); ARRAYdateKL(RawDate2(1):end,1) TempDef(:,RawDate2(1):end)'];
|
|
elseif isempty(RawDate1) == 1 && isempty(RawDate2) == 0
|
|
Dati = [ARRAYdateKL TempDef'];
|
|
else
|
|
Dati = DatiRaw;
|
|
end
|
|
% Elimino appoggio più vecchio di un mese
|
|
RawDate3 = find(Dati(:,1)<now-30);
|
|
if isempty(RawDate3) == 0
|
|
[rDati,~] = size(Dati);
|
|
if RawDate3(end) == rDati
|
|
else
|
|
Dati = Dati(RawDate3(end)+1:end,:);
|
|
end
|
|
end
|
|
if isfile(FileTemperature) == 1
|
|
delete(FileTemperature);
|
|
end
|
|
Dati(:,1) = Dati(:,1) - 730000;
|
|
csvwrite(FileTemperature,Dati);
|
|
|
|
TdefKL = TempDef';
|
|
ANGdefKL = Angolo';
|
|
fprintf(fileID,fmt,textT);
|
|
|
|
%% Calcolo differenziali
|
|
NomeFile = ['' IDcentralina '-' DTcatena '-RifKL.csv'];
|
|
if NuovoZeroKL == 0 % prima elaborazione
|
|
csvwrite(NomeFile,ANGdefKL(1,:));
|
|
ANGdefKL = ANGdefKL - ANGdefKL(1,:);
|
|
else % Ci sono già dei dati elaborati
|
|
RIF = csvread(NomeFile);
|
|
ANGdefKL = ANGdefKL - RIF;
|
|
end
|
|
|
|
%% Matrice Errori
|
|
[r,~] = size(ErrKlinoLink);
|
|
Matrice_err = zeros(r,rKL);
|
|
for i = 1:r % date
|
|
d = 1;
|
|
if strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
|
|
|| isempty(NodoKlinoLink{1,4}) == 1 || strcmp(NodoKlinoLink{1,4},'g') == 1
|
|
off = 3;
|
|
elseif strcmp(NodoKlinoLink(1,4),'Gradi')
|
|
off = 2;
|
|
end
|
|
for n = 1:rKL % nodi
|
|
j = 1;
|
|
err = ErrKlinoLink(i,d:d+off);
|
|
while j <= off+1
|
|
if err(1,j) == 1
|
|
Matrice_err(i,n) = 1;
|
|
end
|
|
j = j+1;
|
|
end
|
|
if strcmp(NodoKlinoLink(1,4),'ADC') || strcmp(NodoKlinoLink{1,4} ,'null') ...
|
|
|| isempty(NodoKlinoLink{1,4}) == 1 || strcmp(NodoKlinoLink{1,4},'g') == 1
|
|
off = off+4;
|
|
elseif strcmp(NodoKlinoLink(1,4),'Gradi')
|
|
off = off+3;
|
|
end
|
|
end
|
|
end
|
|
ErrKlinoLink = Matrice_err';
|
|
|
|
text = 'Klino Link elaborated correctly. Klino function closed';
|
|
fprintf(fileID,fmt,text);
|
|
fclose(fileID);
|
|
|
|
end |