SITL: add Simulink rover example

libraries/SITL/SIM_JSON.cpp

@@ -232,6 +232,16 @@ void JSON::recv_fdm(const struct sitl_input &input)
                            state.velocity[1],
                            state.velocity[2]);
 
+
+    // velocity relative to airmass in body frame
+    velocity_air_bf = dcm.transposed() * velocity_ef;
+
+    // airspeed
+    airspeed = velocity_air_bf.length();
+
+    // airspeed as seen by a fwd pitot tube (limited to 120m/s)
+    airspeed_pitot = constrain_float(velocity_air_bf * Vector3f(1.0f, 0.0f, 0.0f), 0.0f, 120.0f);
+
     position = Vector3f(state.position[0],
                             state.position[1],
                             state.position[2]);
@@ -251,7 +261,7 @@ void JSON::recv_fdm(const struct sitl_input &input)
     }
     time_now_us += deltat * 1.0e6;
 
-    if (deltat > 0 && deltat < 0.1) {
+    if (is_positive(deltat) && deltat < 0.1) {
         // time in us to hz
         adjust_frame_time(1.0 / deltat);
 
@@ -264,56 +274,55 @@ void JSON::recv_fdm(const struct sitl_input &input)
 #if 0
 // @LoggerMessage: JSN1
 // @Description: Log data received from JSON simulator
-// @Field: TimeUS: Time since system startup
-// @Field: TUS: Simulation's timestamp
-// @Field: R: Simulation's roll
-// @Field: P: Simulation's pitch
-// @Field: Y: Simulation's yaw
-// @Field: GX: Simulated gyroscope, X-axis
-// @Field: GY: Simulated gyroscope, Y-axis
-// @Field: GZ: Simulated gyroscope, Z-axis
-    AP::logger().Write("JSN1", "TimeUS,TUS,R,P,Y,GX,GY,GZ",
-                       "QQffffff",
+// @Field: TimeUS: Time since system startup (us)
+// @Field: TStamp: Simulation's timestamp (s)
+// @Field: R: Simulation's roll (rad)
+// @Field: P: Simulation's pitch (rad)
+// @Field: Y: Simulation's yaw (rad)
+// @Field: GX: Simulated gyroscope, X-axis (rad/sec)
+// @Field: GY: Simulated gyroscope, Y-axis (rad/sec)
+// @Field: GZ: Simulated gyroscope, Z-axis (rad/sec)
+    AP::logger().Write("JSN1", "TimeUS,TStamp,R,P,Y,GX,GY,GZ",
+                       "ssrrrEEE",
+                       "F???????",
+                       "Qfffffff",
                        AP_HAL::micros64(),
-                       state.timestamp,
-                       degrees(state.pose.roll),
-                       degrees(state.pose.pitch),
-                       degrees(state.pose.yaw),
-                       degrees(gyro.x),
-                       degrees(gyro.y),
-                       degrees(gyro.z));
+                       state.timestamp_s,
+                       state.attitude[0],
+                       state.attitude[1],
+                       state.attitude[2],
+                       gyro.x,
+                       gyro.y,
+                       gyro.z);
 
-    Vector3f velocity_bf = dcm.transposed() * velocity_ef;
-    position = home.get_distance_NED(location);
+    Vector3f accel_ef = dcm.transposed() * accel_body;
 
 // @LoggerMessage: JSN2
 // @Description: Log data received from JSON simulator
-// @Field: TimeUS: Time since system startup
-// @Field: AX: simulation's acceleration, X-axis
-// @Field: AY: simulation's acceleration, Y-axis
-// @Field: AZ: simulation's acceleration, Z-axis
-// @Field: VX: simulation's velocity, X-axis
-// @Field: VY: simulation's velocity, Y-axis
-// @Field: VZ: simulation's velocity, Z-axis
-// @Field: PX: simulation's position, X-axis
-// @Field: PY: simulation's position, Y-axis
-// @Field: PZ: simulation's position, Z-axis
-// @Field: Alt: simulation's gps altitude
-// @Field: SD: simulation's earth-frame speed-down
-    AP::logger().Write("JSN2", "TimeUS,AX,AY,AZ,VX,VY,VZ,PX,PY,PZ,Alt,SD",
-                       "Qfffffffffff",
+// @Field: TimeUS: Time since system startup (us)
+// @Field: VN: simulation's velocity, North-axis (m/s)
+// @Field: VE: simulation's velocity, East-axis (m/s)
+// @Field: VD: simulation's velocity, Down-axis (m/s)
+// @Field: AX: simulation's acceleration, X-axis (m/s^2)
+// @Field: AY: simulation's acceleration, Y-axis (m/s^2)
+// @Field: AZ: simulation's acceleration, Z-axis (m/s^2)
+// @Field: AN: simulation's acceleration, North (m/s^2)
+// @Field: AE: simulation's acceleration, East (m/s^2)
+// @Field: AD: simulation's acceleration, Down (m/s^2)
+    AP::logger().Write("JSN2", "TimeUS,VN,VE,VD,AX,AY,AZ,AN,AE,AD",
+                       "snnnoooooo",
+                       "F?????????",
+                       "Qfffffffff",
                        AP_HAL::micros64(),
+                       velocity_ef.x,
+                       velocity_ef.y,
+                       velocity_ef.z,
                        accel_body.x,
                        accel_body.y,
                        accel_body.z,
-                       velocity_bf.x,
-                       velocity_bf.y,
-                       velocity_bf.z,
-                       position.x,
-                       position.y,
-                       position.z,
-                       state.gps.alt,
-                       velocity_ef.z);
+                       accel_ef.x,
+                       accel_ef.y,
+                       accel_ef.z);
 #endif
 
 }

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

libraries/SITL/examples/JSON/MATLAB/Heli/heli_init.m

@@ -0,0 +1,128 @@
+addpath(genpath('../../MATLAB'))
+
+% init the globalr varables required by the heli SITL example
+
+global A
+global B
+
+A=zeros(6,6); %#ok<*PREALL>
+B=zeros(6,7);
+
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% initialize higher order model variables
+DCM_0 = zeros(3,3);
+ic_vel = zeros(3,1);
+ic_angrate = zeros(3,1);
+ic_controls = zeros(4,1);
+ic_euler_ang = zeros(3,1);
+
+% direction IC's
+h_0=0; h=0;
+North_0=0;%
+East_0=0;
+Psi = 353*pi/180.0;
+
+%Derivatives accountfor mass 
+M=1;
+
+Xu=-0.02;     %   barn door
+Zw=-2.0;
+Mq=-2.0;
+Mu=0.001;
+Yv=-0.4; 
+Lp=-6.0;
+Nr=-8.0;
+Lv=-0.02;
+
+
+
+%     [u     v      w     p     q      r]'
+
+A = [   Xu    0     0     0     0      0;...
+        0     Yv    0     0     0      0;...
+        0     0     Zw    0     0      0;...
+        0     Lv    0     Lp    0      0;...
+        Mu    0     0     0     Mq     0;...
+        0     0     0     0     0      Nr];
+
+
+%      lat long coll ped '
+
+Zcoll=-60;
+Mlong = 1;
+Llat=1;
+Nped=2;		 
+B =   [ 0     0     0     0;...
+        0     0     0     0;...
+        0     0    Zcoll  0 ;...
+        Llat  0     0     0 ;...
+        0    Mlong  0     0 ;...
+        0     0     0    Nped ];
+
+max_saved=300000;   % limits no. of data points to the last max_saved
+
+%%  Re-order rows of  A for aerospace blk based sim - columns to be ordered in u.
+% Pare A down for Forces only 
+A_Fxyz_uvw_pqr=A([1 2 3],[1 2 3 4 5 6]); 
+% Pare A down for Moments only
+A_Mxyz_uvw_pqr=A([4 5 6],[1 2 3 4 5 6]); 
+
+%%  B rows = X,Y,Z,L,M,N,      B cols  =  u=[lat long coll ped]
+% Pare B down for Forces only
+B_Fxyz_u_cont=B([1 2 3],[1 2 3 4]); 
+% Pare B down for Moments only
+B_Mxyz_u_cont=B([4 5 6],[1 2 3 4]); 
+
+ic_u=0.0000; ic_v=0.0000;  ic_w=0.0000; zPsi=306*pi/180; % Points down RWY32
+
+%ic_u=0.00001; ic_v=0.00001;  ic_w=0.00001; zPsi=306*pi/180; % Points down RWY32
+ic_U=sqrt(ic_u^2+ic_v^2+ic_w^2);
+
+
+%% WIND MODEL - NOTE ; SET Vw = 0 @ h_ic,  Vw units in braces are KTS
+h_deck=21;  h_island=h_deck+130;
+h_contact=h_deck; % h_contact is used in landing gear model (same for fixed and rotary model)
+
+%% TURBULENCE
+W_GUST=.8; % BW in rad/sec
+K_gust=0 ;  % 80; % Gust Gain Factor
+Th_wind_limit=40;  % Limits on wind swing in deg
+hw_gust=[0 1 h_island h_island+1 5000];  % 5 break points
+K_gust_factor=[1.1 1 .5 0.25 0];
+%% WIND
+% hw=[0, h_deck+1, h_deck+1, 150 151 20000];   % FOR SHIP.. 6 BREAK POINTS
+hw=[0 200 5000 10000 15000 20000];   % 6 BREAK POINTS, MONO INCREASING
+Vw=15*[1 1 1 1 1 1];       % wind magnitude in fps
+Dir_w=0*[1 1 1 1 1 1];    % wind direction in deg
+
+Ftrim_grav=-32.0757;
+Roll_ic=0;  Pit_ic=0;  
+
+%Make the trim direction cosine matrix 
+phi_0 = 3; 
+tht_0 = Pit_ic;
+psi_0 = Psi*180/pi;
+
+cphi = cosd(phi_0);
+sphi = sind(phi_0);
+ctht = cosd(tht_0);
+stht = sind(tht_0);
+cpsi = cosd(psi_0);
+spsi = sind(psi_0);
+
+DCM_0 = [1 0 0;
+    0 cphi sphi;
+    0 -sphi cphi] * ...
+    [ctht 0 -stht;
+    0     1  0;
+    stht  0  ctht]*...
+    [cpsi spsi 0;
+    -spsi cpsi 0
+    0     0    1];
+
+%% A short run of the sim to sample cockpit the current control positions 
+U_CONT_O=[0 0 0 0]; 
+
+
+Ts=2000;     % set Ts for real time sim run
+disp('Init Loaded')

libraries/SITL/examples/JSON/MATLAB/Heli/readme.md

@@ -0,0 +1,3 @@
+This is a example Helicopter Simulink model. This has accurate physics and can be run using the EKF. Blocks from this example could be used to make other vehicles without having to work out the conversions to the ArduPilot reference frames. Note that ground interaction is neglected, once you take off you can't land! heli_init.m must be run before the module will run.
+
+Many thanks to [Bill Geyer](https://github.com/bnsgeyer) for providing this model.

libraries/SITL/examples/JSON/MATLAB/Rover/readme.md

@@ -0,0 +1 @@
+This is a very basic Simulink skid steer rover example, this the smallest possible model that can be used to try out the Simuliunk connector functionality.