Sync from remote server: 2025-10-12 18:56:41
This commit is contained in:
256
Tilt/Klino.m
Executable file
256
Tilt/Klino.m
Executable file
@@ -0,0 +1,256 @@
|
||||
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
|
||||
Reference in New Issue
Block a user