function [] = RudderControllerCanCallback(rxCh)

global CircBuff;
global R_idx;
global MaxSeqNum;


persistent jdx;

disp('

SRC_ID = 3;  %Specific to Ruddder Controller

rxMsg = receive(rxCh, Inf);
[srcID, dataID, seq_num] = DecodeCanID(rxMsg);

MaxSeqNum = 0;
%if(~exist('jdx'))
%    disp('jdx doesnt exist');
%    jdx = uint8(0);
%end
if(isempty(jdx))
   jdx = uint8(0); 
end

ID = 0;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),0+1);
    
    CircBuff(seq_num(idx)+1,R_idx.Xvel) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double');
    CircBuff(seq_num(idx)+1,R_idx.Yvel) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double');   
    CircBuff(seq_num(idx)+1,R_idx.Zvel) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double');   
   % CircBuff(seq_num(idx)+1,R_idx.Pressure) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'int16'),'double'); 
    CircBuff(seq_num(idx)+1,R_idx.Pressure) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'uint16'),'double')/200.0; 
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end


ID = 1;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),1+1);
    CircBuff(seq_num(idx)+1,R_idx.BeamAmp1) = cast(unpack(rxMsg(idx), 0, 8, 'LittleEndian', 'uint8'),'double');
    CircBuff(seq_num(idx)+1,R_idx.BeamAmp2) = cast(unpack(rxMsg(idx), 8, 8, 'LittleEndian', 'uint8'),'double');   
    CircBuff(seq_num(idx)+1,R_idx.BeamAmp3) = cast(unpack(rxMsg(idx), 16, 8, 'LittleEndian', 'uint8'),'double');   
    CircBuff(seq_num(idx)+1,R_idx.BeamCorr1) = cast(unpack(rxMsg(idx), 24, 8, 'LittleEndian', 'uint8'),'double');
    CircBuff(seq_num(idx)+1,R_idx.BeamCorr2) = cast(unpack(rxMsg(idx), 32, 8, 'LittleEndian', 'uint8'),'double');   
    CircBuff(seq_num(idx)+1,R_idx.BeamCorr3) = cast(unpack(rxMsg(idx), 40, 8, 'LittleEndian', 'uint8'),'double');       
    CircBuff(seq_num(idx)+1,R_idx.HeadingTarget) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'int16'),'double')*180/32767;
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 2;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),2+1);
    CircBuff(seq_num(idx)+1,R_idx.MeasuredHeading) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')*180/32767+180;
    CircBuff(seq_num(idx)+1,R_idx.FilteredHeading) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')*180/32767+180;   
    CircBuff(seq_num(idx)+1,R_idx.RudderCmd) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double');   
    CircBuff(seq_num(idx)+1,R_idx.RamPosition) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'int16'),'double');   
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 3;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),ID+1);
    CircBuff(seq_num(idx)+1,R_idx.Qtn0) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')/32768;
    CircBuff(seq_num(idx)+1,R_idx.Qtn1) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')/32768;  
    CircBuff(seq_num(idx)+1,R_idx.Qtn2) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double')/32768;   
    CircBuff(seq_num(idx)+1,R_idx.Qtn3) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'int16'),'double')/32768;   
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 4;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),ID+1);
    CircBuff(seq_num(idx)+1,R_idx.Mag0) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')/32768;
    CircBuff(seq_num(idx)+1,R_idx.Mag1) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')/32768;   
    CircBuff(seq_num(idx)+1,R_idx.Mag2) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double')/32768;   
    CircBuff(seq_num(idx)+1,R_idx.Status) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'uint16'),'double');   
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 5;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),ID+1);
    CircBuff(seq_num(idx)+1,R_idx.AngRate0) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')/1000;  %Result is rad/sec
    CircBuff(seq_num(idx)+1,R_idx.AngRate1) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')/1000;   
    CircBuff(seq_num(idx)+1,R_idx.AngRate2) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double')/1000;   
    CircBuff(seq_num(idx)+1,R_idx.VpeStatus) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'uint16'),'double');   
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 6;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),ID+1);
    CircBuff(seq_num(idx)+1,R_idx.Accel0) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')/1000;  %Result is m/sec
    CircBuff(seq_num(idx)+1,R_idx.Accel1) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')/1000;   
    CircBuff(seq_num(idx)+1,R_idx.Accel2) = cast(unpack(rxMsg(idx), 32, 16, 'LittleEndian', 'int16'),'double')/1000;   
    CircBuff(seq_num(idx)+1,R_idx.UnusedP6) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'uint16'),'double');   
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

ID = 7;
idx = find((dataID == ID) & (srcID == SRC_ID));
if(~isempty(idx))
    jdx = bitset(jdx,ID+1);
    CircBuff(seq_num(idx)+1,R_idx.DataIDs) = bitset(CircBuff(seq_num(idx)+1,R_idx.DataIDs),ID+1);
    CircBuff(seq_num(idx)+1,R_idx.Pgain) = cast(unpack(rxMsg(idx), 0, 16, 'LittleEndian', 'int16'),'double')/32768
    CircBuff(seq_num(idx)+1,R_idx.Igain) = cast(unpack(rxMsg(idx), 16, 16, 'LittleEndian', 'int16'),'double')/32768;   
    CircBuff(seq_num(idx)+1,R_idx.UpdateRate) = cast(unpack(rxMsg(idx), 32, 8, 'LittleEndian', 'uint8'),'double');  
    CircBuff(seq_num(idx)+1,R_idx.TimeConstant) = cast(unpack(rxMsg(idx), 40, 8, 'LittleEndian', 'uint8'),'double');  
    CircBuff(seq_num(idx)+1,R_idx.Dgain) = cast(unpack(rxMsg(idx), 48, 16, 'LittleEndian', 'uint16'),'double')/32768;  
    max_sn = max(seq_num(idx));
    if(max_sn > MaxSeqNum)
        MaxSeqNum = max_sn;
    end
end

%R_idx.HeadingTarget = 13;

%R_idx.MeasuredHeading = 14;
%R_idx.FilteredHeading = 15;
%R_idx.RudderCmd = 16;
%R_idx.RamPosition = 17;


idx = find(CircBuff(:,R_idx.DataIDs) >= 127);

if(jdx == bin2dec('1111111'));
 jdx = uint8(0);
 %disp(num2str(CircBuff(idx(end),[R_idx.HeadingTarget R_idx.FilteredHeading R_idx.RudderCmd R_idx.RamPosition])))
 %disp([num2str(bitand(CircBuff(idx(end),R_idx.Status),3)) '  ' num2str(bitand(CircBuff(idx(end),R_idx.Status),4)) '  ' num2str(bitand(CircBuff(idx(end),R_idx.Status),8))  '    ' num2str(CircBuff(idx(end),R_idx.RamPosition)) '  ' num2str(bitshift(bitand(CircBuff(idx(end),R_idx.Status),24),-3))  '    ' num2str(CircBuff(idx(end),R_idx.UnusedP6)) '   '  num2str(CircBuff(idx(end),R_idx.FilteredHeading))   '   ' num2str(CircBuff(idx(end),R_idx.Xvel)) '   ' num2str(CircBuff(idx(end),R_idx.Yvel)) '   ' num2str(CircBuff(idx(end),R_idx.Zvel)) '   ' num2str(CircBuff(idx(end),R_idx.Pressure))])
 str{1} =(['  ' num2str(CircBuff(idx(end),R_idx.Xvel),4) '   ' num2str(CircBuff(idx(end),R_idx.Yvel),4)  '   ' num2str(CircBuff(idx(end),R_idx.Zvel),4) '   ' num2str(CircBuff(idx(end),R_idx.Pressure),4)]);
 str{2} =(['  ' num2str(CircBuff(idx(end),R_idx.BeamAmp1)) '   ' num2str(CircBuff(idx(end),R_idx.BeamAmp2))  '   ' num2str(CircBuff(idx(end),R_idx.BeamAmp3)) '   ' num2str(CircBuff(idx(end),R_idx.BeamCorr1)) '   ' num2str(CircBuff(idx(end),R_idx.BeamCorr2))  '   ' num2str(CircBuff(idx(end),R_idx.BeamCorr3)) '   ' num2str(CircBuff(idx(end),R_idx.HeadingTarget))]);
 str{3} =(['  ' num2str(CircBuff(idx(end),R_idx.MeasuredHeading),4) '   ' num2str(CircBuff(idx(end),R_idx.FilteredHeading),4)  '   ' num2str(CircBuff(idx(end),R_idx.RudderCmd),4) '   ' num2str(CircBuff(idx(end),R_idx.RamPosition),4)]);
 str{4} =(['  ' num2str(CircBuff(idx(end),R_idx.Qtn0),4) '   ' num2str(CircBuff(idx(end),R_idx.Qtn1),4)  '   ' num2str(CircBuff(idx(end),R_idx.Qtn2),4) '   ' num2str(CircBuff(idx(end),R_idx.Qtn3),4)]);
 str{5} =(['  ' num2str(CircBuff(idx(end),R_idx.Mag0),4) '   ' num2str(CircBuff(idx(end),R_idx.Mag1),4)  '   ' num2str(CircBuff(idx(end),R_idx.Mag2),4) '   ' num2str(CircBuff(idx(end),R_idx.Status),4)]);
 str{6} =(['  ' num2str(CircBuff(idx(end),R_idx.AngRate0),4) '   ' num2str(CircBuff(idx(end),R_idx.AngRate1),4)  '   ' num2str(CircBuff(idx(end),R_idx.AngRate2),4) '   ' num2str(CircBuff(idx(end),R_idx.VpeStatus),4)]);
 str{7} =(['  ' num2str(CircBuff(idx(end),R_idx.Accel0),4) '   ' num2str(CircBuff(idx(end),R_idx.Accel1),4)  '   ' num2str(CircBuff(idx(end),R_idx.Accel2),4) '   ' num2str(CircBuff(idx(end),R_idx.UnusedP6),4)]);
 %disp(['ID7  ' num2str(CircBuff(idx(end),R_idx.Pgain),4) '   ' num2str(CircBuff(idx(end),R_idx.Igain),4)  '   ' num2str(CircBuff(idx(end),R_idx.UpdateRate),4) '   ' num2str(CircBuff(idx(end),R_idx.TimeConstant),4) '   ' num2str(CircBuff(idx(end),R_idx.Dgain),4)])
 
 fileID = fopen('TestLog1.txt','a'); 
 fprintf(fileID,'%s ',datestr(now));
 disp(['ID2: ' str{3}]);
 for i = 1:length(str)
    %disp(str{i}); 
    fprintf(fileID,'%s ',str{i});
 end
 fprintf(fileID,'\n');
 fclose(fileID);
 
 CircBuff(idx,R_idx.DataIDs) = 0;
end