SITL: examples: add Simulink libary and functions

libraries/SITL/examples/JSON/MATLAB/AP_receve.m

@@ -0,0 +1,92 @@
+function [pwm, reset] = AP_receve(time)
+global u
+persistent connected frame_time last_sim_time frame_count last_SITL_frame past_time
+
+if isempty(u) || time == 0
+    pnet('closeall') % close any connections left open from past runs
+    u = pnet('udpsocket',9002);
+    pnet(u,'setwritetimeout',1);
+    pnet(u,'setreadtimeout',0);
+    
+    connected = false;
+    frame_time = tic;
+    last_sim_time = 0;
+    frame_count = 0;
+    last_SITL_frame = -1;
+    past_time = -1;
+end
+if past_time == time
+    error('Receve repeat time');
+end
+past_time = time;
+
+print_frame_count = 1000; % print the fps every x frames
+bytes_read =  4 + 4 + 16*2; % the number of bytes received in each packet
+reset = false;
+
+% Wait for data  
+while true
+    in_bytes = pnet(u,'readpacket',bytes_read);
+    if in_bytes > 0
+        % if there is another frame waiting, read it straight away
+        if in_bytes > bytes_read
+            if in_bytes == u.InputBufferSize
+                % buffer got full, reset
+                % should only happen if we have been paused in Matlab for some time
+                fprintf('Buffer reset\n')
+                continue;
+            end
+            continue;
+        end
+        
+        % read in data from AP
+        magic = pnet(u,'read',1,'UINT16','intel');
+        double(pnet(u,'read',1,'UINT16','intel')); % Cant set frame rate in Simulink
+        SITL_frame = pnet(u,'read',1,'UINT32','intel');
+        pwm = double(pnet(u,'read',16,'UINT16','intel'))';
+        
+        % check the magic value is what expect
+        if magic ~= 18458
+            warning('incorrect magic value')
+            continue;
+        end
+        
+        % Check if the fame is in expected order
+        if SITL_frame < last_SITL_frame
+            % Controller has reset
+            connected = false;
+            reset = true;
+            fprintf('Controller reset\n')
+        elseif SITL_frame == last_SITL_frame
+            % duplicate frame, skip
+            fprintf('Duplicate input frame\n')
+            continue;
+        elseif SITL_frame ~= last_SITL_frame + 1 && connected
+            fprintf('Missed %i input frames\n',SITL_frame - last_SITL_frame - 1)
+        end
+        last_SITL_frame = SITL_frame;
+        break;
+    end
+end
+
+if ~connected
+    % use port -1 to indicate connection to address of last recv pkt
+    connected = true;
+    [ip, port] = pnet(u,'gethost');
+    fprintf('Connected to %i.%i.%i.%i:%i\n',ip,port)
+end
+frame_count = frame_count + 1;
+
+%print a fps and runtime update
+if rem(frame_count,print_frame_count) == 0
+    total_time = toc(frame_time);
+    frame_time = tic;
+    sim_time = time - last_sim_time;
+    last_sim_time = time;
+    time_ratio = sim_time/total_time;
+    fprintf("%0.2f fps, %0.2f%% of realtime\n",print_frame_count/total_time,time_ratio*100)
+end
+
+end
+
+

libraries/SITL/examples/JSON/MATLAB/AP_send.m

@@ -0,0 +1,27 @@
+function AP_send(gyro, attitude, accel, velocity, position, time)
+global u
+if isempty(u)
+   return 
+end
+persistent past_time
+if isempty(past_time)
+    past_time = -1;
+end
+if past_time == time
+    error('Send repeat time');
+end
+past_time = time;
+
+% build structure representing the JSON string to be sent
+JSON.timestamp = time;
+JSON.imu.gyro = gyro;
+JSON.imu.accel_body = accel;
+JSON.position = position;
+JSON.attitude = attitude;
+JSON.velocity = velocity;
+
+% Report to AP
+pnet(u,'printf',sprintf('\n%s\n',jsonencode(JSON)));
+pnet(u,'writepacket');
+
+end