zhengxuelong/未整理/测试/read_dual_acc_gyr_mag_RawData.m

clc
close all;
clear all;

dt = 1;
t=[0];
% m=[0;0;0;0]
m=[0;0;0;0;0;0;0;0;]
p = plot(t,m)

data = [];
buf = [];

x=0;
%legend('左脚气压','右脚气压','r2','r3','b0','b1','b2','b3')
legend('左脚气压','右脚气压')
grid on;
set(gcf,'unit','normalized','position',[0.2,0.2,0.64,0.7]);

delete(instrfindall('Type','serial'));%清理串口
object = serial('com3','BaudRate',115200);%配置串口
fopen(object);

state = 0;
Len = 0;
Lenf = 0;

Zarr = [];
Zarr(1) = 0;
Zarr_en  = 0;
run = 0;
left = 0;
right = 0;
acc3_b = 0;
acc3_bb = 0;
press_data = [];
press_var = [];
gyr_var = [];
pos_data = [];

imu_data = [];

is_down = 0;
is_up = 0;
is_left = 0;
is_right = 0;
 
count = 0;

mag_last = [0;0;0];

transfer =[0 -1 0; -1 0 0; 0 0 1];
press = zeros(1,18);
gyr_data = zeros(1, 3);
mag_data = zeros(1, 3);

rotate_matrix = eye(3);

left_acc = [0,0,0];
left_gyr = [0,0,0];
left_front_mag  = [0,0,0];
left_back_mag  = [0,0,0];

right_acc = [0,0,0];
right_gyr = [0,0,0];
right_front_mag  = [0,0,0];
right_back_mag  = [0,0,0];

down = 0;
jump = 0;

s_rssi = 0;

rssi_buff =[];
last_rssi = 0;

press_buff = [];
gyr_buff = [];
h_acc_buff = [];
h_acc_norm_buff = [];
s_acc_buff = [];

h_mag_buff = [];
s_mag_buff = [];

time_stamp = 0;

relate_val = 0;
buff_record = [];

mag_norm = 0;
h_mag_norm_buff = [];
s_mag_norm_buff = [];
% for i=1:shuliang
while true
    data=fread(object);%读取数据
    while ~isempty(data)
        buf = [buf data(1)];
        data(1) = [];
        switch state
            case 0
               if length(buf)>=5
                   if  buf(1)==170
                       Len = buf(2);
                       Lenf = 255 - buf(2);
                       if buf(3) == Lenf
                          state = 1;
                       else
                           buf(1) = [];
                       end
                   else
                       buf(1) = [];
                   end
               end
            case 1
                if length(buf)>=Len
                  ver = 0;
                   for i=1:(Len-1)
                       ver = ver + buf(i);
                   end
                   ver = mod(ver,256);
    %                disp(ver);
    %                disp(buf(Len));
                   if ver==buf(Len) && buf(4) == 4 && Len>52
                       
                       
                       left_acc(1) =  double(bitshift(int16(buf(5)),8)+bitshift(int16(buf(6)),0)) ;
                       left_acc(2) =  double(bitshift(int16(buf(7)),8)+bitshift(int16(buf(8)),0)) ;
                       left_acc(3) =  double(bitshift(int16(buf(9)),8)+bitshift(int16(buf(10)),0)) ;
                       
                       left_acc_norm = norm(left_acc);
                       
                       
                       left_gyr(1) =  double(bitshift(int16(buf(11)),8)+bitshift(int16(buf(12)),0)) ;
                       left_gyr(2) =  double(bitshift(int16(buf(13)),8)+bitshift(int16(buf(14)),0)) ;
                       left_gyr(3) =  double(bitshift(int16(buf(15)),8)+bitshift(int16(buf(16)),0)) ;
                       
                       left_gyr_norm = norm(left_gyr);
                       
                       left_front_mag(1) =   double(bitshift(int16(buf(17)),8)+bitshift(int16(buf(18)),0));
                       left_front_mag(2) =   double(bitshift(int16(buf(19)),8)+bitshift(int16(buf(20)),0));
                       left_front_mag(3) =   double(bitshift(int16(buf(21)),8)+bitshift(int16(buf(22)),0));
                       
                       left_front_mag_norm = norm(left_front_mag);
                       
                       left_back_mag(1) =   double(bitshift(int16(buf(23)),8)+bitshift(int16(buf(24)),0))
                       left_back_mag(2) =   double(bitshift(int16(buf(25)),8)+bitshift(int16(buf(26)),0));
                       left_back_mag(3) =   double(bitshift(int16(buf(27)),8)+bitshift(int16(buf(28)),0));
                       
                       left_back_mag_norm = norm(left_back_mag);
                       
                       right_acc(1) =  double(bitshift(int16(buf(29)),8)+bitshift(int16(buf(30)),0)) ;
                       right_acc(2) =  double(bitshift(int16(buf(31)),8)+bitshift(int16(buf(32)),0)) ;
                       right_acc(3) =  double(bitshift(int16(buf(33)),8)+bitshift(int16(buf(34)),0)) ;
                       
                       right_acc_norm = norm(right_acc);
                       
                       right_gyr(1) =  double(bitshift(int16(buf(35)),8)+bitshift(int16(buf(36)),0)) ;
                       right_gyr(2) =  double(bitshift(int16(buf(37)),8)+bitshift(int16(buf(38)),0)) ;
                       right_gyr(3) =  double(bitshift(int16(buf(39)),8)+bitshift(int16(buf(40)),0)) ;
                       
                       right_gyr_norm = norm(right_gyr);
                       
                       right_front_mag(1) =   double(bitshift(int16(buf(41)),8)+bitshift(int16(buf(42)),0));
                       right_front_mag(2) =   double(bitshift(int16(buf(43)),8)+bitshift(int16(buf(44)),0));
                       right_front_mag(3) =   double(bitshift(int16(buf(45)),8)+bitshift(int16(buf(46)),0));
                       
                       right_front_mag_norm = norm(right_front_mag);
                       
                       right_back_mag(1) =   double(bitshift(int16(buf(47)),8)+bitshift(int16(buf(48)),0));
                       right_back_mag(2) =   double(bitshift(int16(buf(49)),8)+bitshift(int16(buf(50)),0));
                       right_back_mag(3) =   double(bitshift(int16(buf(51)),8)+bitshift(int16(buf(52)),0));

                       right_back_mag_norm = norm(right_back_mag);
                       
                       buff_record = [buff_record;buf(1:Len)];
                        
                       
                        dt = dt + 1;
                        t=[t dt];
                        if dt>250 
                            x=x+1; 
                        end

%                     if((size(h_acc_norm_buff,2) > 9) && (max(h_acc_norm_buff(end-9:end)) - 1 < 0.1) && (min(h_acc_norm_buff(end-9:end)) - 1 > -0.1))
%                         acc_zupt = 1;
%                     else
%                         acc_zupt = 0;
%                     end

%                     %测左右鞋地磁
%                     m=[m, [ left_front_mag_norm;left_back_mag_norm; right_front_mag_norm ; right_back_mag_norm; 0; 0; 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:))
%                             set(p(2),'XData',t,'YData',m(2,:))
%                             set(p(3),'XData',t,'YData',m(3,:))
%                             set(p(4),'XData',t,'YData',m(4,:))
% 

%                     %测左右鞋地磁
%                     m=[m, [ left_front_mag_norm;left_back_mag_norm; right_front_mag_norm ; right_back_mag_norm; 0; 0; 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:))
%                             set(p(2),'XData',t,'YData',m(2,:))
%                             set(p(3),'XData',t,'YData',m(3,:))
%                             set(p(4),'XData',t,'YData',m(4,:))
                            
%                     %测左鞋前后地磁原始数据
%                     m=[m, [ left_front_mag(1);left_front_mag(2); left_front_mag(3) ; left_back_mag(1); left_back_mag(2); left_back_mag(3); 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:),'Color','red')
%                             set(p(2),'XData',t,'YData',m(2,:),'Color','green')
%                             set(p(3),'XData',t,'YData',m(3,:),'Color','blue')
%                             set(p(4),'XData',t,'YData',m(4,:),'Color','yellow')
%                             set(p(5),'XData',t,'YData',m(5,:),'Color','black')
%                             set(p(6),'XData',t,'YData',m(6,:),'Color','cyan')
                            
%                     %测右鞋前后地磁原始数据
%                     m=[m, [ right_front_mag(1);right_front_mag(2); right_front_mag(3) ; right_back_mag(1); right_back_mag(2); right_back_mag(3); 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:))
%                             set(p(2),'XData',t,'YData',m(2,:))
%                             set(p(3),'XData',t,'YData',m(3,:))
%                             set(p(4),'XData',t,'YData',m(4,:))
%                             set(p(5),'XData',t,'YData',m(5,:))
%                             set(p(6),'XData',t,'YData',m(6,:))
                            
                            
                    %测左鞋六轴        
                    m=[m, [ left_acc_norm+4000;left_gyr_norm+3000; left_front_mag_norm+2000 ; left_back_mag_norm+1000;right_acc_norm-1000;right_gyr_norm-2000; right_front_mag_norm-3000 ; right_back_mag_norm-4000; ]];
                            set(p(1),'XData',t,'YData',m(1,:),'Color','red')
                            set(p(2),'XData',t,'YData',m(2,:),'Color','green')
                            set(p(3),'XData',t,'YData',m(3,:),'Color','blue')
                            set(p(4),'XData',t,'YData',m(4,:),'Color','black')
                            set(p(5),'XData',t,'YData',m(5,:),'Color','red')
                            set(p(6),'XData',t,'YData',m(6,:),'Color','green')
                            set(p(7),'XData',t,'YData',m(7,:),'Color','blue')
                            set(p(8),'XData',t,'YData',m(8,:),'Color','black')
                            
%                      m=[m, [ right_back_mag(1);left_back_mag(1); 0 ; 0; 0;0; 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:))
%                             set(p(2),'XData',t,'YData',m(2,:))  


%                     %测右鞋六轴        
%                     m=[m, [ right_acc(1);right_acc(2); right_acc(3) ; right_gyr(1); right_gyr(2); right_gyr(3); 0; 0; ]];
%                             set(p(1),'XData',t,'YData',m(1,:))
%                             set(p(2),'XData',t,'YData',m(2,:))
%                             set(p(3),'XData',t,'YData',m(3,:))
%                             set(p(4),'XData',t,'YData',m(4,:))
%                             set(p(5),'XData',t,'YData',m(5,:))
%                             set(p(6),'XData',t,'YData',m(6,:))
                           
                            
                            
                            
                            
                            
                            
                            
%                             set(p(6),'XData',t,'YData', m(6,:))
%                             set(p(7),'XData',t,'YData',m(7,:))
%                             set(p(8),'XData',t,'YData',m(8,:))
%                                    set(p(3),'XData',t,'YData',m(7,:))
%                                  set(p(8),'XData',t,'YData',m(8,:))
%                                 set(p(9),'XData',t,'YData',m(9,:))
%                                set(p(10),'XData',t,'YData',m(10,:))
                   
                        buf = buf(Len:end);
                   else
                       buf(1) = [];
                   end
                   state = 0;
                end
            otherwise
                state = 0;
        end
    end
    drawnow limitrate nocallbacks
% 	axis([x x+300 -0.1 0.1]); 
    axis([x x+300 -10000 10000]);
end

fclose(object);
delete(object);
clear object;