├── README.md ├── Screenshot.png ├── getOscMessages.m ├── gui.fig ├── gui.m ├── quaternionPlot.m └── sensorPlot.m /README.md: -------------------------------------------------------------------------------- 1 | NGIMU-MATLAB-Real-Time-Example 2 | ============================== 3 | 4 | This example demonstrates how to receive NGIMU data in MATLAB in real-time via Wi-Fi. The NGIMU must be configured to be send to the computer IP address before running this example. The computer firewall settings must not be blocking MATLAB from having network access. 5 | 6 | ![](https://github.com/xioTechnologies/NGIMU-MATLAB-Real-Time-Example/blob/master/Screenshot.png) 7 | -------------------------------------------------------------------------------- /Screenshot.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xioTechnologies/NGIMU-MATLAB-Real-Time-Example/a6dc3ebad234bbe037017c1e94ef89bdcc8d6080/Screenshot.png -------------------------------------------------------------------------------- /getOscMessages.m: -------------------------------------------------------------------------------- 1 | function oscMessages = getOscMessages(charArray) 2 | oscMessages = processOscPacket(charArray, NaN, []); 3 | end 4 | 5 | function oscMessages = processOscPacket(charArray, timestamp, oscMessages) 6 | switch charArray(1) 7 | 8 | % OSC contents is an OSC message 9 | case '/' 10 | oscMessage = processOscMessage(charArray); 11 | if isnan(timestamp) == false 12 | oscMessage.timestamp = timestamp; 13 | end 14 | oscMessages = [oscMessages; oscMessage]; 15 | 16 | % OSC contents is an OSC bundle 17 | case '#' 18 | [oscTimeTag, oscContents] = processOscBundle(charArray); 19 | timestamp = double(oscTimeTag) / 2^32; % convert to seconds 20 | for oscContentsIndex = 1:length(oscContents) 21 | oscMessages = processOscPacket(oscContents{oscContentsIndex}, timestamp, oscMessages); % call recursively 22 | end 23 | 24 | % OSC contents is invalid 25 | otherwise 26 | warning('Invalid OSC contents.'); 27 | end 28 | end 29 | 30 | function [oscTimeTag, oscContents] = processOscBundle(charArray) 31 | 32 | % Get OSC time tag 33 | oscTimeTag = typecast(uint8(flip(charArray(9:16))), 'uint64'); 34 | 35 | % Get OSC bundle elements 36 | oscBundleElements = charArray(17:end); 37 | 38 | % Loop through each OSC bundle element 39 | bundleElementIndex = 1; 40 | while isempty(oscBundleElements) == false 41 | 42 | % Get OSC bundle element size 43 | oscBundleElementSize = typecast(uint8(flip(oscBundleElements(1:4))), 'int32'); 44 | 45 | % Get OSC contents 46 | oscContents{bundleElementIndex} = oscBundleElements(5:(oscBundleElementSize + 4)); 47 | bundleElementIndex = bundleElementIndex + 1; 48 | oscBundleElements = oscBundleElements((oscBundleElementSize + 5):end); 49 | end 50 | end 51 | 52 | function oscMessage = processOscMessage(charArray) 53 | 54 | % Get OSC address 55 | [oscMessage.oscAddress, remainder] = strtok(charArray, 0); 56 | 57 | % Get OSC type tag string 58 | [~, remainder] = strtok(remainder, ','); 59 | remainder = remainder(2:end); % trim comma from start 60 | [oscTypeTagString, remainder] = strtok(remainder, 0); 61 | 62 | % Get argument array 63 | oscTypeTagStringSize = 1 + length(oscTypeTagString); % include comma 64 | numberOfNullCharacters = 1; 65 | while mod(oscTypeTagStringSize + numberOfNullCharacters, 4) ~= 0 66 | numberOfNullCharacters = numberOfNullCharacters + 1; 67 | end 68 | argumentsArray = remainder((1 + numberOfNullCharacters):end); 69 | 70 | % Parse each argument 71 | for oscTypeTagStringIndex = 1:length(oscTypeTagString) 72 | switch oscTypeTagString(oscTypeTagStringIndex) 73 | case 'i' 74 | int32 = typecast(uint8(flip(argumentsArray(1:4))), 'int32'); 75 | argumentsArray = argumentsArray(5:end); 76 | oscMessage.arguments{oscTypeTagStringIndex} = int32; 77 | case 'f' 78 | float32 = typecast(uint8(flip(argumentsArray(1:4))), 'single'); 79 | argumentsArray = argumentsArray(5:end); 80 | oscMessage.arguments{oscTypeTagStringIndex} = float32; 81 | case 's' 82 | [string, argumentsArray] = strtok(argumentsArray, 0); 83 | oscMessage.arguments{oscTypeTagStringIndex} = string; 84 | numberOfNullCharacters = 1; 85 | while mod(length(string) + numberOfNullCharacters, 4) ~= 0 86 | numberOfNullCharacters = numberOfNullCharacters + 1; 87 | end 88 | argumentsArray = argumentsArray((length(string) + numberOfNullCharacters):end); 89 | case 'b' 90 | blobSize = typecast(uint8(flip(argumentsArray(1:4))), 'int32'); 91 | oscMessage.arguments{oscTypeTagStringIndex} = argumentsArray(5:(4 + blobSize)); 92 | numberOfNullCharacters = 0; 93 | while mod(blobSize + numberOfNullCharacters, 4) ~= 0 94 | numberOfNullCharacters = numberOfNullCharacters + 1; 95 | end 96 | argumentsArray = argumentsArray((5 + blobSize + numberOfNullCharacters):end); 97 | case 'T' 98 | oscMessage.arguments{oscTypeTagStringIndex} = true; 99 | case 'F' 100 | oscMessage.arguments{oscTypeTagStringIndex} = false; 101 | otherwise 102 | warning('Argument type not supported.'); 103 | break; 104 | end 105 | end 106 | end 107 | -------------------------------------------------------------------------------- /gui.fig: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/xioTechnologies/NGIMU-MATLAB-Real-Time-Example/a6dc3ebad234bbe037017c1e94ef89bdcc8d6080/gui.fig -------------------------------------------------------------------------------- /gui.m: -------------------------------------------------------------------------------- 1 | function varargout = gui(varargin) 2 | % GUI MATLAB code for gui.fig 3 | % GUI, by itself, creates a new GUI or raises the existing 4 | % singleton*. 5 | % 6 | % H = GUI returns the handle to a new GUI or the handle to 7 | % the existing singleton*. 8 | % 9 | % GUI('CALLBACK',hObject,eventData,handles,...) calls the local 10 | % function named CALLBACK in GUI.M with the given input arguments. 11 | % 12 | % GUI('Property','Value',...) creates a new GUI or raises the 13 | % existing singleton*. Starting from the left, property value pairs are 14 | % applied to the GUI before gui_OpeningFcn gets called. An 15 | % unrecognized property name or invalid value makes property application 16 | % stop. All inputs are passed to gui_OpeningFcn via varargin. 17 | % 18 | % *See GUI Options on GUIDE's Tools menu. Choose "GUI allows only one 19 | % instance to run (singleton)". 20 | % 21 | % See also: GUIDE, GUIDATA, GUIHANDLES 22 | 23 | % Edit the above text to modify the response to help gui 24 | 25 | % Last Modified by GUIDE v2.5 14-Nov-2018 17:31:09 26 | 27 | % Begin initialization code - DO NOT EDIT 28 | gui_Singleton = 1; 29 | gui_State = struct('gui_Name', mfilename, ... 30 | 'gui_Singleton', gui_Singleton, ... 31 | 'gui_OpeningFcn', @gui_OpeningFcn, ... 32 | 'gui_OutputFcn', @gui_OutputFcn, ... 33 | 'gui_LayoutFcn', [] , ... 34 | 'gui_Callback', []); 35 | if nargin && ischar(varargin{1}) 36 | gui_State.gui_Callback = str2func(varargin{1}); 37 | end 38 | 39 | if nargout 40 | [varargout{1:nargout}] = gui_mainfcn(gui_State, varargin{:}); 41 | else 42 | gui_mainfcn(gui_State, varargin{:}); 43 | end 44 | % End initialization code - DO NOT EDIT 45 | end 46 | 47 | 48 | % --- Executes just before gui is made visible. 49 | function gui_OpeningFcn(hObject, eventdata, handles, varargin) 50 | % This function has no output args, see OutputFcn. 51 | % hObject handle to figure 52 | % eventdata reserved - to be defined in a future version of MATLAB 53 | % handles structure with handles and user data (see GUIDATA) 54 | % varargin command line arguments to gui (see VARARGIN) 55 | 56 | % Choose default command line output for gui 57 | handles.output = hObject; 58 | 59 | handles.gyroscopePlot = sensorPlot(handles.gyroscopeAxes, 500, 'Gyroscope'); 60 | handles.accelerometerPlot = sensorPlot(handles.accelerometerAxes, 500, 'Accelerometer'); 61 | handles.magnetometerPlot = sensorPlot(handles.magnetometerAxes, 500, 'Magnetometer'); 62 | handles.quaternionPlot = quaternionPlot(handles.quaternionAxes); 63 | 64 | handles.timer = timer('Period', 0.02, 'ExecutionMode', 'fixedRate'); 65 | handles.timer.TimerFcn = {@timer_Callback, handles}; 66 | start(handles.timer); 67 | 68 | % Update handles structure 69 | guidata(hObject, handles); 70 | 71 | % UIWAIT makes gui wait for user response (see UIRESUME) 72 | % uiwait(handles.figure1); 73 | end 74 | 75 | 76 | % --- Outputs from this function are returned to the command line. 77 | function varargout = gui_OutputFcn(hObject, eventdata, handles) 78 | % varargout cell array for returning output args (see VARARGOUT); 79 | % hObject handle to figure 80 | % eventdata reserved - to be defined in a future version of MATLAB 81 | % handles structure with handles and user data (see GUIDATA) 82 | 83 | % Get default command line output from handles structure 84 | varargout{1} = handles.output; 85 | end 86 | 87 | 88 | 89 | function udpPortEditText_Callback(hObject, eventdata, handles) 90 | % hObject handle to udpPortEditText (see GCBO) 91 | % eventdata reserved - to be defined in a future version of MATLAB 92 | % handles structure with handles and user data (see GUIDATA) 93 | 94 | % Hints: get(hObject,'String') returns contents of udpPortEditText as text 95 | % str2double(get(hObject,'String')) returns contents of udpPortEditText as a double 96 | end 97 | 98 | 99 | % --- Executes during object creation, after setting all properties. 100 | function udpPortEditText_CreateFcn(hObject, eventdata, handles) 101 | % hObject handle to udpPortEditText (see GCBO) 102 | % eventdata reserved - to be defined in a future version of MATLAB 103 | % handles empty - handles not created until after all CreateFcns called 104 | 105 | % Hint: edit controls usually have a white background on Windows. 106 | % See ISPC and COMPUTER. 107 | if ispc && isequal(get(hObject,'BackgroundColor'), get(0,'defaultUicontrolBackgroundColor')) 108 | set(hObject,'BackgroundColor','white'); 109 | end 110 | end 111 | 112 | 113 | % --- Executes on button press in openPushButton. 114 | function openPushButton_Callback(hObject, eventdata, handles) 115 | % hObject handle to openPushButton (see GCBO) 116 | % eventdata reserved - to be defined in a future version of MATLAB 117 | % handles structure with handles and user data (see GUIDATA) 118 | 119 | % Close all preivous UDP sockets 120 | try 121 | fclose(instrfindall); 122 | catch 123 | end 124 | 125 | % Open UDP socket 126 | try 127 | udpPort = str2double(get(handles.udpPortEditText, 'String')); 128 | handles.udp = udp('255.255.255.255', 'Localport', udpPort, 'InputBufferSize', 4096); 129 | handles.udp.datagramReceivedFcn = {@processData_Callback, handles}; 130 | fopen(handles.udp); 131 | catch exception 132 | errordlg(exception.message); 133 | end 134 | 135 | % Update handles 136 | guidata(hObject, handles); 137 | end 138 | 139 | 140 | % --- Executes on button press in closePushButton. 141 | function closePushButton_Callback(hObject, eventdata, handles) 142 | % hObject handle to closePushButton (see GCBO) 143 | % eventdata reserved - to be defined in a future version of MATLAB 144 | % handles structure with handles and user data (see GUIDATA) 145 | 146 | try 147 | fclose(instrfindall); 148 | catch 149 | end 150 | end 151 | 152 | 153 | % --- Executes when user attempts to close figure1. 154 | function figure1_CloseRequestFcn(hObject, eventdata, handles) 155 | % hObject handle to figure1 (see GCBO) 156 | % eventdata reserved - to be defined in a future version of MATLAB 157 | % handles structure with handles and user data (see GUIDATA) 158 | 159 | try 160 | fclose(instrfindall); 161 | catch 162 | end 163 | stop(handles.timer); 164 | delete(handles.timer); 165 | 166 | % Hint: delete(hObject) closes the figure 167 | delete(hObject); 168 | end 169 | 170 | 171 | function processData_Callback(hObject, eventdata, handles) 172 | 173 | % Do nothing if socket closed 174 | if strcmp(handles.udp.Status, 'closed') 175 | return; 176 | end 177 | 178 | % Discad input buffer if overrun 179 | if handles.udp.BytesAvailable == handles.udp.InputBufferSize 180 | flushinput(handles.udp); 181 | warning('UDP input buffer overrun.'); 182 | return; 183 | end 184 | 185 | % Read UDP packet 186 | charArray = char(fread(handles.udp))'; 187 | 188 | % Prcess OSC packet 189 | oscMessages = getOscMessages(charArray); 190 | 191 | % Process OSC messages 192 | for oscMessagesIndex = 1:length(oscMessages) 193 | oscMessage = oscMessages(oscMessagesIndex); 194 | 195 | % Filter by OSC address 196 | switch oscMessage.oscAddress 197 | case '/sensors' 198 | handles.gyroscopePlot.updateData([oscMessage.arguments{1}, oscMessage.arguments{2}, oscMessage.arguments{3}]); 199 | handles.accelerometerPlot.updateData([oscMessage.arguments{4}, oscMessage.arguments{5}, oscMessage.arguments{6}]); 200 | handles.magnetometerPlot.updateData([oscMessage.arguments{7}, oscMessage.arguments{8}, oscMessage.arguments{9}]); 201 | case '/quaternion' 202 | quaternionAxes = [oscMessage.arguments{1}, oscMessage.arguments{2}, oscMessage.arguments{3}, oscMessage.arguments{4}]; 203 | handles.quaternionPlot.updateData(quaternionAxes); 204 | case '/temperature' 205 | % This message is currnelty unhandled 206 | case '/humidity' 207 | % This message is currnelty unhandled 208 | case '/battery' 209 | % This message is currnelty unhandled 210 | otherwise 211 | warning(['Unhandled OSC address received: ' oscMessage.oscAddress]); 212 | end 213 | end 214 | end 215 | 216 | 217 | function timer_Callback(hObject, eventdata, handles) 218 | handles.gyroscopePlot.updatePlot(); 219 | handles.accelerometerPlot.updatePlot(); 220 | handles.magnetometerPlot.updatePlot(); 221 | handles.quaternionPlot.updatePlot(); 222 | drawnow; 223 | end 224 | -------------------------------------------------------------------------------- /quaternionPlot.m: -------------------------------------------------------------------------------- 1 | classdef quaternionPlot < handle 2 | properties 3 | teaPot; 4 | size; 5 | teaPotPlot; 6 | xAxisPlot; 7 | yAxisPlot; 8 | zAxisPlot; 9 | quaternion; 10 | end 11 | methods 12 | function obj = quaternionPlot(axesHandle) 13 | 14 | % Draw teapot 15 | obj.teaPot = pcdownsample(pcread('teapot.ply'), 'gridAverage', 0.2); 16 | obj.size = max(max(abs(obj.teaPot.Location))); 17 | obj.teaPotPlot = plot3(axesHandle, ... 18 | obj.teaPot.Location(:, 1), obj.teaPot.Location(:, 2), obj.teaPot.Location(:, 3), ... 19 | '.', 'Color', [0.8, 0.8, 0.8]); 20 | 21 | % Draw Earth axes 22 | hold(axesHandle, 'on'); 23 | red = [0.6350, 0.0780, 0.1840]; 24 | green = [0.4660, 0.6740, 0.1880]; 25 | blue = [0, 0.4470, 0.7410]; 26 | plot3(axesHandle, [0, obj.size], [0, 0], [0, 0], '--', 'Color', red); 27 | plot3(axesHandle, [0, 0], [0, obj.size], [0, 0], '--', 'Color', green); 28 | plot3(axesHandle, [0, 0], [0, 0], [0, obj.size], '--', 'Color', blue); 29 | 30 | % Draw sensor axes 31 | obj.xAxisPlot = plot3(axesHandle, [0, obj.size], [0, 0], [0, 0], 'Color', red); 32 | obj.yAxisPlot = plot3(axesHandle, [0, 0], [0, obj.size], [0, 0], 'Color', green); 33 | obj.zAxisPlot = plot3(axesHandle, [0, 0], [0, 0], [0, obj.size], 'Color', blue); 34 | hold(axesHandle, 'off'); 35 | 36 | % Format axes 37 | set(axesHandle, 'XGrid', 'on'); 38 | set(axesHandle, 'YGrid', 'on'); 39 | set(axesHandle, 'ZGrid', 'on'); 40 | set(axesHandle,'XTickLabel',[]); 41 | set(axesHandle,'YTickLabel',[]); 42 | set(axesHandle,'ZTickLabel',[]); 43 | set(axesHandle, 'xlim', [-obj.size, obj.size]); 44 | set(axesHandle, 'ylim', [-obj.size, obj.size]); 45 | set(axesHandle, 'zlim', [-obj.size, obj.size]); 46 | set(get(axesHandle, 'title'), 'string', 'Quaternion'); 47 | 48 | % Initial quaternion vlaue 49 | obj.quaternion = [1 0 0 0]; 50 | end 51 | function obj = updateData(obj, quaternion) 52 | obj.quaternion = quaternion; 53 | end 54 | function obj = updatePlot(obj) 55 | 56 | % Convert quaternion to rotation matrix 57 | %R = quat2rotm(obj.quaternion); 58 | q = obj.quaternion; 59 | R = eye(3); 60 | R(1,1) = 2.*q(1).^2-1+2.*q(2).^2; 61 | R(1,2) = 2.*(q(2).*q(3)+q(1).*q(4)); 62 | R(1,3) = 2.*(q(2).*q(4)-q(1).*q(3)); 63 | R(2,1) = 2.*(q(2).*q(3)-q(1).*q(4)); 64 | R(2,2) = 2.*q(1).^2-1+2.*q(3).^2; 65 | R(2,3) = 2.*(q(3).*q(4)+q(1).*q(2)); 66 | R(3,1) = 2.*(q(2).*q(4)+q(1).*q(3)); 67 | R(3,2) = 2.*(q(3).*q(4)-q(1).*q(2)); 68 | R(3,3) = 2.*q(1).^2-1+2.*q(4).^2; 69 | 70 | % Udate teapot 71 | T = eye(4); 72 | T(1:3, 1:3) = R; 73 | rotatedTeaPot = pctransform(obj.teaPot, affine3d(T)); 74 | set(obj.teaPotPlot, 'XData', rotatedTeaPot.Location(:, 1)); 75 | set(obj.teaPotPlot, 'YData', rotatedTeaPot.Location(:, 2)); 76 | set(obj.teaPotPlot, 'ZData', rotatedTeaPot.Location(:, 3)); 77 | 78 | % Udate sensor X axis 79 | scaledR = obj.size * R; 80 | set(obj.xAxisPlot, 'XData', [0, scaledR(1, 1)]); 81 | set(obj.xAxisPlot, 'YData', [0, scaledR(1, 2)]); 82 | set(obj.xAxisPlot, 'ZData', [0, scaledR(1, 3)]); 83 | 84 | % Udate sensor Y axis 85 | set(obj.yAxisPlot, 'XData', [0, scaledR(2, 1)]); 86 | set(obj.yAxisPlot, 'YData', [0, scaledR(2, 2)]); 87 | set(obj.yAxisPlot, 'ZData', [0, scaledR(2, 3)]); 88 | 89 | % Udate sensor Z axis 90 | set(obj.zAxisPlot, 'XData', [0, scaledR(3, 1)]); 91 | set(obj.zAxisPlot, 'YData', [0, scaledR(3, 2)]); 92 | set(obj.zAxisPlot, 'ZData', [0, scaledR(3, 3)]); 93 | end 94 | end 95 | end 96 | -------------------------------------------------------------------------------- /sensorPlot.m: -------------------------------------------------------------------------------- 1 | classdef sensorPlot < handle 2 | properties 3 | axesHandle; 4 | xData; 5 | yData; 6 | zData; 7 | xPlot; 8 | yPlot; 9 | zPlot; 10 | end 11 | methods 12 | function obj = sensorPlot(axesHandle, samplesPerPlot, titleString) 13 | obj.axesHandle = axesHandle; 14 | obj.xData = NaN(samplesPerPlot, 1); 15 | obj.yData = NaN(samplesPerPlot, 1); 16 | obj.zData = NaN(samplesPerPlot, 1); 17 | hold(obj.axesHandle, 'on'); 18 | plot(obj.axesHandle, [0 samplesPerPlot], [0 0], 'k:'); 19 | obj.xPlot = plot(obj.axesHandle, obj.xData, 'Color', [0.6350, 0.0780, 0.1840]); 20 | obj.yPlot = plot(obj.axesHandle, obj.yData, 'Color', [0.4660, 0.6740, 0.1880]); 21 | obj.zPlot = plot(obj.axesHandle, obj.zData, 'Color', [ 0, 0.4470, 0.7410]); 22 | hold(obj.axesHandle, 'off'); 23 | set(obj.axesHandle, 'xtick', []); 24 | set(obj.axesHandle, 'YGrid', 'on'); 25 | set(obj.axesHandle, 'YMinorGrid', 'on'); 26 | set(get(obj.axesHandle, 'title'), 'string', titleString); 27 | end 28 | function obj = updateData(obj, xyxValue) 29 | obj.xData = [obj.xData(2:end); xyxValue(1)]; 30 | obj.yData = [obj.yData(2:end); xyxValue(2)]; 31 | obj.zData = [obj.zData(2:end); xyxValue(3)]; 32 | end 33 | function obj = updatePlot(obj) 34 | set(obj.xPlot, 'YData', obj.xData); 35 | set(obj.yPlot, 'YData', obj.yData); 36 | set(obj.zPlot, 'YData', obj.zData); 37 | limit = max(abs([obj.xData; obj.yData; obj.zData])); 38 | if ~isnan(limit) 39 | set(obj.axesHandle, 'ylim', [-limit, limit]); 40 | end 41 | end 42 | end 43 | end 44 | --------------------------------------------------------------------------------