shoes/liangjianbo/matlab/串口数据显示及读取/test_DOWN.m

clear all;
close all;
load imu_data1;

gyr_data = imu_data(1:3,:);
press_raw_data = imu_data(4,:);
press_data = (imu_data(4,:)-9000000*ones(1,size(gyr_data,2)))*0.0000005;
last_move_index = 0;
zupt = zeros(1,size(gyr_data,2));
press_zupt = 0;
mutex_up = 0;
mutex_down =0;
press_index = 0;
down_pass1 = 0;
down_pass2 = 0;

for i = 1:size(gyr_data,2)
    
    if(norm(gyr_data(:,i)) > 1.0)
        last_move_index = i;
    end
    
    if(i> 6 && press_raw_data(1, i) - min(press_raw_data(1, i-6:i-1)) > 100000 )
        press_zupt = 1;
        zupt(i) = 1;
        press_index = i;
        if(down_pass1==2)
            down_pass1 = 0;
        end
        if(abs(gyr_data(2,i)) > 1.5)
            down_pass1 = 1;
        end
    elseif(i> 6 && press_raw_data(1, i) - max(press_raw_data(1, i-6:i-1)) < -100000)
        zupt(i) = 2;
        if(down_pass2==2)
            down_pass2 = 0;
        end
        if(abs(gyr_data(2,i)) > 1.5)
          down_pass2 = 1;
        end
    elseif(i > 1 && zupt(i-1)== 2 )
        if(down_pass1 == 0 && down_pass2 == 0 )
            zupt(i) = 3;
        end
        down_pass1 = 2 ;
        down_pass2 = 2 ;
    end
    
    
end

index = find(zupt ==1);

figure;
hold on
plot(gyr_data(1,:),'r-')
plot(gyr_data(2,:),'g-')
plot(gyr_data(3,:),'b-')
plot(press_data(1,:),'m-')
plot(index,press_data(1,index),'ko')
index = find(zupt == 2);
plot(index,press_data(1,index),'ro')
index = find(zupt == 3);
plot(index,press_data(1,index),'k*')
grid on
axis on

figure
hold on
plot(press_data(1,:),'r-')
grid on
axis on