From f5e98a6d69487d9dc17cc00862357b9feb10f54a Mon Sep 17 00:00:00 2001 From: Iampete1 Date: Fri, 29 May 2020 15:22:19 +0100 Subject: [PATCH] SITL: update MATLAB example --- .../SITL/examples/JSON/MATLAB/Copter/Copter.m | 4 +- .../examples/JSON/MATLAB/Copter/Hexsoon.mat | Bin 738 -> 742 bytes .../JSON/MATLAB/Copter/SIM_multicopter.m | 134 +++++++++++------- .../examples/JSON/MATLAB/SITL_connector.m | 47 +++--- 4 files changed, 111 insertions(+), 74 deletions(-) diff --git a/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m b/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m index 22a7e217f0..905edf9d7e 100644 --- a/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m +++ b/libraries/SITL/examples/JSON/MATLAB/Copter/Copter.m @@ -61,8 +61,8 @@ copter.battery = battery; copter.mass = 2; % (kg) inertia = (2/5) * copter.mass * (0.45*0.2)^2; % (sphere) copter.inertia = diag(ones(3,1)*inertia); % rotational inertia matrix (kgm^2) -copter.cd = [0.5,0.5,0.5]; -copter.cd_ref_area = [1,1,1] * pi * (0.45*0.5)^2; +copter.cd = [0.5;0.5;0.5]; +copter.cd_ref_area = [1;1;1] * pi * (0.45*0.5)^2; save('Hexsoon','copter') diff --git a/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat b/libraries/SITL/examples/JSON/MATLAB/Copter/Hexsoon.mat index 4848175d807532a0e029828fa0ec23d44ba046cc..5ea3572c81e3a2caa3cb88f6db617992c89bcf36 100644 GIT binary patch delta 612 zcmV-q0-OEf1?B~iI21~CZXh5rATc*OF*G_fIUq7HGBA-*BavVQk#rCNUXg_>f5pLZ zD_VrafDm*kE3EAN1)vHj1L9LGNK7!m2oe%ght`grBZP9-^WEqtS^oU_JwLzqb5aL@ z`^NxAM14RM2bAp7{aUHWHJGNHFgfqRVLg69ls4s*WePisxLC82x7#SwqX1Dt7>`ii z#vybBhXEsV0b&F}UG#4q?QV~!e`_`F!6DtB5+$Y>@rWjJ+Xa*)B$lhfh!L!YNi13{ z!YBrk;lhNJ(G<#3*XJti7}>=Rw`9ktD95G#i+*e7>ZV`R<^3jl>{l!8=ZIF#0Otzi z_O`-}*YvSZ&>*HL4Db?TB;mkF7G)eIn3$c<6Bebef5J6;;GBw| zrJZxReAoUx-IV)G_Mv}Nk6$}`@YAF8sW0(&p+A2WaVP8D{9$6xjmG2iUsrGE_Dniv z_Dq9}c+@6&ra~HVyr5PkOYLAbMj^@I(m9%BJRiOCr&&tZa<-bEr?vk4_jv2QZ;Ib~ z{o~b(DRn>n=8rRg-T5^%e|{P1E%Mex@52XD?iu^*=acy}N5V0GNc^>-r;Yf_GsG4C zv;1ZG3oL(G{`wET3x914gTEXDoc;c?{AKye^4E~~YoPx}*AQpFzbt=Q{<8davd{c4 z;^&ko%QWU_J`d(nXk(Nj9Pt(Ch^d`#-$yyeI227`c_1=1ATc#MFgZFjI3O}GGBA-*BavVQk#rCNT9Jh-f7!ut zD_T^k146(8E3EAN1)vHj1L9LGNK7!m2oe$#L$PD$2uHd0d^h?@mhET%{GOlveopED zaQ6tnh^+U?;(?YureCWRH3zdy5U$1pIB4|eWN9;j^cq-s{2aDug(q_V{m zj8Y&uE?vkt%b+TCv#-*Rk!}3&D}Id1a#YzrAGT&LZ-!-EU2kHC^=g&%JlU#Q;CzYP z+S0i3+V0M^jN_Q;D|?H}e}w}mi&BQ-2rqF?r~pp#Bo~NcVz<9QdX)JZf7jdr=XJ$d z`8ij}R}th7A0A|nLxsNy{l%-icdFhl9yb15Ydk#rdG&hX&!lJP z&ornTkJ_ZjRLBy67xXN;)Q)CT6q6h-oMAKe}|RbGH-3{-oK~hmUX{%k#9td`*owabOI#U0 z$6t=W!10&kum9kC@YlvL_{+1vx$iH>Uyi>Ve+`Mh2Ks;WEphJq%kh`vFUMcU`^^9H z{+y6ynWX~F7niwmwK2*NPQ(gy#MaN(uarz!=WVq2&9RFg7p>!@Htjc>gWRjDJ#aq! u`#z}m= 0 && accel_ef(3) > 0 + accel_ef(3) = 0; +end + +% work out acceleration as seen by the accelerometers. It sees the kinematic +% acceleration (ie. real movement), plus gravity +state.accel = state.dcm' * (accel_ef + [0; 0; -state.gravity_mss]); + +state.velocity = state.velocity + accel_ef * state.delta_t; +state.position = state.position + state.velocity * state.delta_t; % make sure we can't go underground (NED so underground is positive) if state.position(3) >= 0 state.position(3) = 0; - state.velocity = [0,0,0]; - state.accel = [0,0,-state.gravity_mss]; - state.gyro = [0,0,0]; + state.velocity = [0;0;0]; + state.gyro = [0;0;0]; end -% caculate the body frame velocity and resulting drag -bf_velo = (state.velocity / dcm); -state.drag = sign(bf_velo) .* state.copter.cd .* state.copter.cd_ref_area .* 0.5 .* state.environment.density .* bf_velo.^2; - -% estimate rotational drag (mostly for yaw) -state.rotational_drag = 0.3 * sign(state.gyro) .* state.gyro.^2; % estimated to give a reasonable max rotation rate +% calculate the body frame velocity for drag calculation +state.bf_velo = state.dcm' * state.velocity; end + +function [dcm, euler] = rotate_dcm(dcm, ang) + +% rotate +delta = [dcm(1,2) * ang(3) - dcm(1,3) * ang(2), dcm(1,3) * ang(1) - dcm(1,1) * ang(3), dcm(1,1) * ang(2) - dcm(1,2) * ang(1); + dcm(2,2) * ang(3) - dcm(2,3) * ang(2), dcm(2,3) * ang(1) - dcm(2,1) * ang(3), dcm(2,1) * ang(2) - dcm(2,2) * ang(1); + dcm(3,2) * ang(3) - dcm(3,3) * ang(2), dcm(3,3) * ang(1) - dcm(3,1) * ang(3), dcm(3,1) * ang(2) - dcm(3,2) * ang(1)]; + +dcm = dcm + delta; + +% normalise +a = dcm(1,:); +b = dcm(2,:); +error = a * b'; +t0 = a - (b *(0.5 * error)); +t1 = b - (a *(0.5 * error)); +t2 = cross(t0,t1); +dcm(1,:) = t0 * (1/norm(t0)); +dcm(2,:) = t1 * (1/norm(t1)); +dcm(3,:) = t2 * (1/norm(t2)); + +% calculate euler angles +euler = [atan2(dcm(3,2),dcm(3,3)); -asin(dcm(3,1)); atan2(dcm(2,1),dcm(1,1))]; + +end + diff --git a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m index d7107bda5b..bd0568d82f 100644 --- a/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m +++ b/libraries/SITL/examples/JSON/MATLAB/SITL_connector.m @@ -2,7 +2,7 @@ % 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) +function SITL_connector(state,init_function,physics_function,max_timestep) try pnet('closeall') % close any connections left open from past runs catch @@ -31,17 +31,15 @@ pnet(u,'setreadtimeout',0); frame_time = tic; frame_count = 0; -physics_time_us = 0; +physics_time_s = 0; last_SITL_frame = -1; -print_frame_count = 1000; +print_frame_count = 1000; % print the fps every x frames connected = false; -bytes_read = 4 + 4 + 16*2; -time_correction = 1; +bytes_read = 4 + 4 + 16*2; % the number of bytes received in each packet while true - mat_time = tic; % Wait for data - while true + while true in_bytes = pnet(u,'readpacket',bytes_read); if in_bytes > 0 break; @@ -59,14 +57,23 @@ while true continue; end - % read in the current SITL frame and PWM + % read in data from AP + magic = pnet(u,'read',1,'UINT16','intel'); + frame_rate = double(pnet(u,'read',1,'UINT16','intel')); 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 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, reset physics also state = init_function(state); + connected = false; fprintf('Controller reset\n') elseif SITL_frame == last_SITL_frame % duplicate frame, skip @@ -76,19 +83,21 @@ while true 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; + state.delta_t = min(1/frame_rate,max_timestep); + physics_time_s = physics_time_s + state.delta_t; if ~connected connected = true; - fprintf('Connected\n') + [ip, port] = pnet(u,'gethost'); + fprintf('Connected to %i.%i.%i.%i:%i\n',ip,port) end frame_count = frame_count + 1; - % Do a physics time step + % 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.timestamp = physics_time_s; JSON.imu.gyro = state.gyro; JSON.imu.accel_body = state.accel; JSON.position = state.position; @@ -97,22 +106,14 @@ while true % Report to AP pnet(u,'printf',sprintf('\n%s\n',jsonencode(JSON))); - pnet(u,'writepacket',target_ip,9003); + pnet(u,'writepacket'); % 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; + time_ratio = (print_frame_count*state.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