%% Tracé des logs du Black_Pearl
clear all
close all

load LOG_BLACK_PEARL_SETE.mat


frame_id=LOG200723120259.frame_id;
trim_com=LOG200723120259.trim_com;
trim_fl=LOG200723120259.trim_fl;
trim_fr=LOG200723120259.trim_fr;
trim_rl=LOG200723120259.trim_rl;
trim_rr=LOG200723120259.trim_rr;
alt_p=LOG200723120259.alt_p;
alt_i=LOG200723120259.alt_i;
alt_d=LOG200723120259.alt_d;
pitch_p=LOG200723120259.pitch_p;
pitch_i=LOG200723120259.pitch_i;
pitch_d=LOG200723120259.pitch_d;
roll_p=LOG200723120259.roll_p;
roll_i=LOG200723120259.roll_i;
roll_d=LOG200723120259.roll_d;
alt_sp=LOG200723120259.alt_sp;
alt_bal=LOG200723120259.alt_bal;
packet_id=LOG200723120259.packet_id;
rc1=LOG200723120259.rc1;
rc2=LOG200723120259.rc2;
pitch=LOG200723120259.pitch;
roll=LOG200723120259.roll;
yaw=LOG200723120259.yaw;
lat=LOG200723120259.lat;
long=LOG200723120259.long;
vel=LOG200723120259.vel;
fl=LOG200723120259.fl;
fr=LOG200723120259.fr;
rl=LOG200723120259.rl;
rr=LOG200723120259.rr;
altl=LOG200723120259.altl;
altr=LOG200723120259.altr;
vbat=LOG200723120259.vbat;
status=LOG200723120259.status;
fr4=LOG200723120259.fr4;
fr5=LOG200723120259.fr5;
fr6=LOG200723120259.fr6;
fr7=LOG200723120259.fr7;
fr8=LOG200723120259.fr8;
fr9=LOG200723120259.fr9;
fr10=LOG200723120259.fr10;
crc=LOG200723120259.crc;

%% Détermination de la plage d'intérêt
i_init = 2200;
i_end = 4210;


Alt_Left=altl(i_init:i_end);
Alt_LRight=altr(i_init:i_end);
Commande_cap = rc1(i_init:i_end);
Commande_poussee = rc2(i_init:i_end);
Cap=yaw(i_init:i_end);
Tangage=pitch(i_init:i_end);
Roulis=roll(i_init:i_end);
Foil_FL=fl(i_init:i_end);
Foil_FR=fr(i_init:i_end);
Foil_RL=rl(i_init:i_end);
Foil_RR=rr(i_init:i_end);
Longitude = long(i_init:i_end);
Lattitude = lat(i_init:i_end);
Rudder_Control = (rc1(i_init:i_end)-1500)/4;
Thrust_Control = (rc2(i_init:i_end)-1500)/4;
Altitude = 0.8*((altl(i_init:i_end) - 380)/max(altl(i_init:i_end)));
%Altitude(1:19) =  Altitude(1:19) - mean(Altitude(1:19)); 
%% Détermination de la trajectoire métrique
[X,Y]=WGS84toLambert('III',Longitude,Lattitude);
X=X-min(X);
Y=Y-min(Y);


[X_CPLT,Y_CPLT]=WGS84toLambert('III',long,lat);
X_CPLT=X_CPLT-min(X_CPLT);
Y_CPLT=Y_CPLT-min(Y_CPLT);

figure()
plot(X_CPLT,Y_CPLT)

%% Détermination de la vitesse
dt=0.1;
for i = 2 : length(X)
Distance(i-1) = sqrt((X(i)-X(i-1))^2+(Y(i)-Y(i-1))^2);
Vitesse(i-1) = Distance(i-1)/dt;
end

XX(1)=0;
YY(1)=0;
%Dessin trajectoire d'apres cap et vitesse
for i =  1 : length(Distance),
    XX(i+1)=XX(i)+Distance(i)*cos(Cap(i)*pi/180);
    YY(i+1)=YY(i)+Distance(i)*sin(Cap(i)*pi/180);
end
distance_cumulee = 0;
for i = 1 : length(Distance)
    distance_cumulee = distance_cumulee + Distance(i);
end
distance_cumulee

figure()
plot(XX,YY)

%% traitement des outliers sur la vitesse
for i = 2 : length(Vitesse)
    if Vitesse(i)>8,
        Vitesse(i)=Vitesse(i-1);
    end
end
for i = 2 : length(Vitesse)
    if Vitesse(i)>6,
        Vitesse(i)=Vitesse(i-1);
    end
end


TIME = [1:length(Foil_FL)]/10;%Avec un dt de 0.1;

figure()
hold on
load data_vitesse.mat
plot(X_VITESSE_FIT_2,Y_VITESSE_FIT2,'b')
title('u velocity (m/s)')
xlabel('Time (s)')

figure()
hold on
plot(TIME,Altitude,'b')
title('Altitude Above Water (m)')
xlabel('Time (s)')

figure()
hold on
plot(TIME,Roulis,'r')
plot(TIME,Tangage,'g')
title('Roll (°,r) and Pitch (°,g) angles')
xlabel('Time (s)')

figure()
hold on
plot(TIME,Cap,'b')
title('Yaw angle (°,b)')
xlabel('Time (s)')

figure()
plot(X,Y)
title('Trajectory')
hold on
plot(X(1),Y(1),'r*')
xlabel('North (m)')
xlabel('East (m)')

figure()
Foil_FL(1:38)=-500;
Foil_FR(1:38)=-500;
Foil_RL(1:38)=-500;
Foil_RR(1:38)=-500;

% Conversion en angle
valeur_max_deg=15;%degres
val_statique_deg=7;%degres
valeur_max_pwm=500;%degres
val_statique_pwm=200;%degres

alpha=(valeur_max_deg+val_statique_deg)/(valeur_max_pwm-val_statique_pwm);
beta=-valeur_max_deg+valeur_max_pwm*alpha;

for i = 1 : length(Foil_FL)
Foil_FL_DEG(i) = alpha*Foil_FL(i)+beta;
if Foil_FL_DEG(i)>10, Foil_FL_DEG(i)=Foil_FL_DEG(i)*0.8;end
if Foil_FL_DEG(i)>15, Foil_FL_DEG(i)=15;end
Foil_FR_DEG(i) = alpha*Foil_FR(i)+beta;
if Foil_FR_DEG(i)>10, Foil_FR_DEG(i)=Foil_FR_DEG(i)*0.8;end
if Foil_FR_DEG(i)>15, Foil_FR_DEG(i)=15;end
Foil_RL_DEG(i) = alpha*Foil_RL(i)+beta;
if Foil_RL_DEG(i)>10, Foil_RL_DEG(i)=Foil_RL_DEG(i)*0.8;end
if Foil_RL_DEG(i)>15, Foil_RL_DEG(i)=15;end
Foil_RR_DEG(i) = alpha*Foil_RR(i)+beta;
if Foil_RR_DEG(i)>10, Foil_RR_DEG(i)=Foil_RR_DEG(i)*0.8;end
if Foil_RR_DEG(i)>15, Foil_RR_DEG(i)=15;end
end




hold on
plot(TIME,Foil_FL_DEG,'r')
plot(TIME,Foil_FR_DEG,'g')
plot(TIME,Foil_RL_DEG,'b')
plot(TIME,Foil_RR_DEG,'k')
xlabel('Time (s)')
title('Foils Activity FL(r,deg),FR(g,deg),RL(b,deg),RR(k,deg)')

figure()
hold on
plot(TIME,Foil_FL,'r')
plot(TIME,Foil_FR,'g')
plot(TIME,Foil_RL,'b')
plot(TIME,Foil_RR,'k')
xlabel('Time (s)')
title('Foils Activity FL(r,PWM),FR(g,PWM),RL(b,PWM),RR(k,PWM)')

figure()
Rudder_Control(1:38)=0;
hold on
plot(TIME,Rudder_Control,'r')
xlabel('Time (s)')
title('rudder CTRL (%,PWM)')

figure()
Thrust_Control(1:38)=0;
hold on
plot(X_VITESSE_FIT_2,Thrust_Control(1:2010),'r')
xlabel('Time (s)')
title('Thrust CTRL (%,PWM)')

