You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
118 lines
3.7 KiB
118 lines
3.7 KiB
% this function handles all the UDP connection to SITL using the TCP/UDP/IP |
|
% Toolbox 2.0.6 by Peter Rydesäter |
|
% https://uk.mathworks.com/matlabcentral/fileexchange/345-tcp-udp-ip-toolbox-2-0-6 |
|
|
|
function SITL_connector(target_ip,state,init_function,physics_function,delta_t) |
|
try |
|
pnet('closeall') % close any connections left open from past runs |
|
catch |
|
warning('Could not execute pnet mex, trying to compile') |
|
if ispc |
|
% running on windows |
|
mex -O -outdir ../tcp_udp_ip_2.0.6 ../tcp_udp_ip_2.0.6/pnet.c ws2_32.lib -DWIN32 |
|
else |
|
% running on unix or mac |
|
mex -O -outdir ../tcp_udp_ip_2.0.6 ../tcp_udp_ip_2.0.6/pnet.c |
|
end |
|
try |
|
pnet('closeall') |
|
catch |
|
error('Failed to compile pnet mex file, see tcp_udp_ip_2.0.6/pnet.c for instructions on manual complication') |
|
end |
|
end |
|
|
|
% init physics |
|
state = init_function(state); |
|
|
|
% Init the UDP port |
|
u = pnet('udpsocket',9002); |
|
pnet(u,'setwritetimeout',1); |
|
pnet(u,'setreadtimeout',0); |
|
|
|
frame_time = tic; |
|
frame_count = 0; |
|
physics_time_us = 0; |
|
last_SITL_frame = -1; |
|
print_frame_count = 1000; |
|
connected = false; |
|
bytes_read = 4 + 4 + 16*2; |
|
time_correction = 1; |
|
while true |
|
mat_time = tic; |
|
|
|
% Wait for data |
|
while true |
|
in_bytes = pnet(u,'readpacket',bytes_read); |
|
if in_bytes > 0 |
|
break; |
|
end |
|
end |
|
|
|
% 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 the current SITL frame and PWM |
|
SITL_frame = pnet(u,'read',1,'UINT32','intel'); |
|
speed_up = double(pnet(u,'read',1,'SINGLE','intel')); |
|
pwm_in = double(pnet(u,'read',16,'UINT16','intel'))'; |
|
% Check if the fame is in expected order |
|
if SITL_frame < last_SITL_frame |
|
% Controller has reset, reset physics also |
|
state = init_function(state); |
|
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; |
|
physics_time_us = physics_time_us + delta_t * 10^6; |
|
|
|
if ~connected |
|
connected = true; |
|
fprintf('Connected\n') |
|
end |
|
frame_count = frame_count + 1; |
|
|
|
% Do a physics time step |
|
state = physics_function(pwm_in,state); |
|
|
|
% build structure representing the JSON string to be sent |
|
JSON.timestamp = physics_time_us; |
|
JSON.imu.gyro = state.gyro; |
|
JSON.imu.accel_body = state.accel; |
|
JSON.position = state.position; |
|
JSON.attitude = state.attitude; |
|
JSON.velocity = state.velocity; |
|
|
|
% Report to AP |
|
pnet(u,'printf',sprintf('\n%s\n',jsonencode(JSON))); |
|
pnet(u,'writepacket',target_ip,9003); |
|
|
|
% print a fps and runtime update |
|
if rem(frame_count,print_frame_count) == 0 |
|
total_time = toc(frame_time); |
|
frame_time = tic; |
|
time_ratio = (print_frame_count*delta_t)/total_time; |
|
fprintf("%0.2f fps, %0.2f%% of realtime\n",print_frame_count/total_time,time_ratio*100) |
|
time_ratio = speed_up / time_ratio; |
|
if time_ratio < 1.1 && time_ratio > 0.9 |
|
time_correction = time_correction * 0.95 + time_ratio * 0.05; |
|
end |
|
end |
|
|
|
while toc(mat_time) < (delta_t / speed_up) / time_correction |
|
end |
|
|
|
end |
|
|
|
|