Skip to content

Commit a2b84fd

Browse files
Tinker-TwinsTinker-Twins
committed
Add Simulink API for Polaris RZR and Environment
1 parent 1ee8352 commit a2b84fd

File tree

5 files changed

+547
-0
lines changed

5 files changed

+547
-0
lines changed

‎autodrive_simulink_api/autodrive_rzr.m‎

Lines changed: 364 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,364 @@
1+
function autodrive_rzr(block)
2+
% AutoDRIVE RZR Simulink API
3+
% This is a Level-2 MATLAB S-Function wrapper for AutoDRIVE RZR API
4+
5+
% Copyright 2024 AutoDRIVE Ecosystem
6+
7+
%% The setup method is used to set up the basic attributes of the
8+
%% S-function such as ports, parameters, etc. Do not add any other
9+
%% calls to the main body of the function.
10+
11+
setup(block);
12+
13+
%endfunction
14+
15+
%% Function: setup
16+
%% Abstract:
17+
%% Set up the basic characteristics of the S-function block such as:
18+
%% - Input ports
19+
%% - Output ports
20+
%% - Dialog parameters
21+
%% - Options
22+
%%
23+
%% Required: Yes
24+
%% C MEX counterpart: mdlInitializeSizes
25+
26+
function setup(block)
27+
28+
% Allow more than 2D signals
29+
block.AllowSignalsWithMoreThan2D = 1;
30+
31+
% Register number of ports
32+
block.NumInputPorts = 21;
33+
block.NumOutputPorts = 15;
34+
35+
% Setup port properties to be inherited or dynamic
36+
block.SetPreCompInpPortInfoToDynamic;
37+
block.SetPreCompOutPortInfoToDynamic;
38+
39+
% Override input port properties
40+
% Co-Simulation Mode
41+
block.InputPort(1).Dimensions = 1;
42+
block.InputPort(1).DatatypeID = 3; % uint8
43+
block.InputPort(1).Complexity = 'Real';
44+
block.InputPort(1).DirectFeedthrough = true;
45+
% Position X Component
46+
block.InputPort(2).Dimensions = 1;
47+
block.InputPort(2).DatatypeID = 0; % double
48+
block.InputPort(2).Complexity = 'Real';
49+
block.InputPort(2).DirectFeedthrough = true;
50+
% Position Y Component
51+
block.InputPort(3).Dimensions = 1;
52+
block.InputPort(3).DatatypeID = 0; % double
53+
block.InputPort(3).Complexity = 'Real';
54+
block.InputPort(3).DirectFeedthrough = true;
55+
% Position Z Component
56+
block.InputPort(4).Dimensions = 1;
57+
block.InputPort(4).DatatypeID = 0; % double
58+
block.InputPort(4).Complexity = 'Real';
59+
block.InputPort(4).DirectFeedthrough = true;
60+
% Orientation Quaternion X Component
61+
block.InputPort(5).Dimensions = 1;
62+
block.InputPort(5).DatatypeID = 0; % double
63+
block.InputPort(5).Complexity = 'Real';
64+
block.InputPort(5).DirectFeedthrough = true;
65+
% Orientation Quaternion Y Component
66+
block.InputPort(6).Dimensions = 1;
67+
block.InputPort(6).DatatypeID = 0; % double
68+
block.InputPort(6).Complexity = 'Real';
69+
block.InputPort(6).DirectFeedthrough = true;
70+
% Orientation Quaternion Z Component
71+
block.InputPort(7).Dimensions = 1;
72+
block.InputPort(7).DatatypeID = 0; % double
73+
block.InputPort(7).Complexity = 'Real';
74+
block.InputPort(7).DirectFeedthrough = true;
75+
% Orientation Quaternion W Component
76+
block.InputPort(8).Dimensions = 1;
77+
block.InputPort(8).DatatypeID = 0; % double
78+
block.InputPort(8).Complexity = 'Real';
79+
block.InputPort(8).DirectFeedthrough = true;
80+
% Throttle Command
81+
block.InputPort(9).Dimensions = 1;
82+
block.InputPort(9).DatatypeID = 0; % double
83+
block.InputPort(9).Complexity = 'Real';
84+
block.InputPort(9).DirectFeedthrough = true;
85+
% Steering Command
86+
block.InputPort(10).Dimensions = 1;
87+
block.InputPort(10).DatatypeID = 0; % double
88+
block.InputPort(10).Complexity = 'Real';
89+
block.InputPort(10).DirectFeedthrough = true;
90+
% Brake Command
91+
block.InputPort(11).Dimensions = 1;
92+
block.InputPort(11).DatatypeID = 0; % double
93+
block.InputPort(11).Complexity = 'Real';
94+
block.InputPort(11).DirectFeedthrough = true;
95+
% Handbrake Command
96+
block.InputPort(12).Dimensions = 1;
97+
block.InputPort(12).DatatypeID = 0; % double
98+
block.InputPort(12).Complexity = 'Real';
99+
block.InputPort(12).DirectFeedthrough = true;
100+
% Headlights Command
101+
block.InputPort(13).Dimensions = 1;
102+
block.InputPort(13).DatatypeID = 3; % uint8
103+
block.InputPort(13).Complexity = 'Real';
104+
block.InputPort(13).DirectFeedthrough = true;
105+
% Auto-Time Command
106+
block.InputPort(14).Dimensions = 1;
107+
block.InputPort(14).DatatypeID = 8; % boolean
108+
block.InputPort(14).Complexity = 'Real';
109+
block.InputPort(14).DirectFeedthrough = true;
110+
% Time Scale Command
111+
block.InputPort(15).Dimensions = 1;
112+
block.InputPort(15).DatatypeID = 0; % double
113+
block.InputPort(15).Complexity = 'Real';
114+
block.InputPort(15).DirectFeedthrough = true;
115+
% Time of Day Command
116+
block.InputPort(16).Dimensions = 1;
117+
block.InputPort(16).DatatypeID = 0; % double
118+
block.InputPort(16).Complexity = 'Real';
119+
block.InputPort(16).DirectFeedthrough = true;
120+
% Weather ID Command
121+
block.InputPort(17).Dimensions = 1;
122+
block.InputPort(17).DatatypeID = 3; % uint8
123+
block.InputPort(17).Complexity = 'Real';
124+
block.InputPort(17).DirectFeedthrough = true;
125+
% Cloud Intensity Command
126+
block.InputPort(18).Dimensions = 1;
127+
block.InputPort(18).DatatypeID = 0; % double
128+
block.InputPort(18).Complexity = 'Real';
129+
block.InputPort(18).DirectFeedthrough = true;
130+
% Fog Intensity Command
131+
block.InputPort(19).Dimensions = 1;
132+
block.InputPort(19).DatatypeID = 0; % double
133+
block.InputPort(19).Complexity = 'Real';
134+
block.InputPort(19).DirectFeedthrough = true;
135+
% Rain Intensity Command
136+
block.InputPort(20).Dimensions = 1;
137+
block.InputPort(20).DatatypeID = 0; % double
138+
block.InputPort(20).Complexity = 'Real';
139+
block.InputPort(20).DirectFeedthrough = true;
140+
% Snow Intensity Command
141+
block.InputPort(21).Dimensions = 1;
142+
block.InputPort(21).DatatypeID = 0; % double
143+
block.InputPort(21).Complexity = 'Real';
144+
block.InputPort(21).DirectFeedthrough = true;
145+
146+
% Override output port properties
147+
% Vehicle ID
148+
block.OutputPort(1).Dimensions = 1;
149+
block.OutputPort(1).DatatypeID = 5; % uint16
150+
block.OutputPort(1).Complexity = 'Real';
151+
% Collision Count
152+
block.OutputPort(2).Dimensions = 1;
153+
block.OutputPort(2).DatatypeID = 5; % uint16
154+
block.OutputPort(2).Complexity = 'Real';
155+
% Throttle Feedback
156+
block.OutputPort(3).Dimensions = 1;
157+
block.OutputPort(3).DatatypeID = 0; % double
158+
block.OutputPort(3).Complexity = 'Real';
159+
% Steering Feedback
160+
block.OutputPort(4).Dimensions = 1;
161+
block.OutputPort(4).DatatypeID = 0; % double
162+
block.OutputPort(4).Complexity = 'Real';
163+
% Brake Feedback
164+
block.OutputPort(5).Dimensions = 1;
165+
block.OutputPort(5).DatatypeID = 0; % double
166+
block.OutputPort(5).Complexity = 'Real';
167+
% Handbrake Feedback
168+
block.OutputPort(6).Dimensions = 1;
169+
block.OutputPort(6).DatatypeID = 0; % double
170+
block.OutputPort(6).Complexity = 'Real';
171+
% Encoder Ticks
172+
block.OutputPort(7).Dimensions = [2 1];
173+
block.OutputPort(7).DatatypeID = 6; % int32
174+
block.OutputPort(7).Complexity = 'Real';
175+
% Encoder Angles
176+
block.OutputPort(8).Dimensions = [2 1];
177+
block.OutputPort(8).DatatypeID = 0; % double
178+
block.OutputPort(8).Complexity = 'Real';
179+
% Position
180+
block.OutputPort(9).Dimensions = [3 1];
181+
block.OutputPort(9).DatatypeID = 0; % double
182+
block.OutputPort(9).Complexity = 'Real';
183+
% Orientation (Quaternion)
184+
block.OutputPort(10).Dimensions = [4 1];
185+
block.OutputPort(10).DatatypeID = 0; % double
186+
block.OutputPort(10).Complexity = 'Real';
187+
% Orientation (Euler Angles)
188+
block.OutputPort(11).Dimensions = [3 1];
189+
block.OutputPort(11).DatatypeID = 0; % double
190+
block.OutputPort(11).Complexity = 'Real';
191+
% Angular Velocity
192+
block.OutputPort(12).Dimensions = [3 1];
193+
block.OutputPort(12).DatatypeID = 0; % double
194+
block.OutputPort(12).Complexity = 'Real';
195+
% Linear Acceleration
196+
block.OutputPort(13).Dimensions = [3 1];
197+
block.OutputPort(13).DatatypeID = 0; % double
198+
block.OutputPort(13).Complexity = 'Real';
199+
% LIDAR Pointcloud
200+
% block.OutputPort(14).DimensionsMode = 'Variable';
201+
% block.OutputPort(14).Dimensions = [57600,3]; % max. size
202+
% block.OutputPort(14).DatatypeID = 1; % single
203+
% block.OutputPort(14).Complexity = 'Real';
204+
% Camera 01
205+
block.OutputPort(14).Dimensions = [720,1280,3];
206+
block.OutputPort(14).DatatypeID = 3; % uint8
207+
block.OutputPort(14).Complexity = 'Real';
208+
% Camera 02
209+
block.OutputPort(15).Dimensions = [720,1280,3];
210+
block.OutputPort(15).DatatypeID = 3; % uint8
211+
block.OutputPort(15).Complexity = 'Real';
212+
213+
% Register parameters
214+
block.NumDialogPrms = 1;
215+
216+
% Register sample times
217+
% [0 offset] : Continuous sample time
218+
% [positive_num offset] : Discrete sample time
219+
% [-1, 0] : Inherited sample time
220+
% [-2, 0] : Variable sample time
221+
block.SampleTimes = [1 0];
222+
223+
% Specify the block simStateCompliance. The allowed values are:
224+
% 'UnknownSimState', < The default setting; warn and assume DefaultSimState
225+
% 'DefaultSimState', < Same sim state as a built-in block
226+
% 'HasNoSimState', < No sim state
227+
% 'CustomSimState', < Has GetSimState and SetSimState methods
228+
% 'DisallowSimState' < Error out when saving or restoring the model sim state
229+
block.SimStateCompliance = 'DefaultSimState';
230+
231+
%% The MATLAB S-function uses an internal registry for all
232+
%% block methods. You should register all relevant methods
233+
%% (optional and required) as illustrated below. You may choose
234+
%% any suitable name for the methods and implement these methods
235+
%% as local functions within the same file. See comments
236+
%% provided for each function for more information.
237+
238+
% block.RegBlockMethod('PostPropagationSetup', @DoPostPropSetup);
239+
block.RegBlockMethod('InitializeConditions', @InitializeConditions);
240+
% block.RegBlockMethod('Start', @Start);
241+
block.RegBlockMethod('Outputs', @Outputs); % Required
242+
% block.RegBlockMethod('Update', @Update);
243+
% block.RegBlockMethod('Derivatives', @Derivatives);
244+
block.RegBlockMethod('Terminate', @Terminate); % Required
245+
246+
%end setup
247+
248+
%% PostPropagationSetup:
249+
%% Functionality: Setup work areas and state variables. Can
250+
%% also register run-time methods here
251+
%% Required: No
252+
%% C MEX counterpart: mdlSetWorkWidths
253+
254+
% function DoPostPropSetup(block)
255+
256+
%end DoPostPropSetup(block)
257+
258+
%% InitializeConditions:
259+
%% Functionality: Called at the start of simulation and if it is
260+
%% present in an enabled subsystem configured to reset
261+
%% states, it will be called when the enabled subsystem
262+
%% restarts execution to reset the states.
263+
%% Required: No
264+
%% C MEX counterpart: mdlInitializeConditions
265+
266+
function InitializeConditions(block)
267+
global autodrive
268+
autodrive = server_rzr(block.DialogPrm(1).Data);
269+
270+
%end InitializeConditions
271+
272+
%% Start:
273+
%% Functionality: Called once at start of model execution. If you
274+
%% have states that should be initialized once, this
275+
%% is the place to do it.
276+
%% Required: No
277+
%% C MEX counterpart: mdlStart
278+
279+
% function Start(block)
280+
281+
%end Start
282+
283+
%% Outputs:
284+
%% Functionality: Called to generate block outputs in
285+
%% simulation step
286+
%% Required: Yes
287+
%% C MEX counterpart: mdlOutputs
288+
289+
function Outputs(block)
290+
global autodrive
291+
% Parse inputs
292+
autodrive.rzr_1.cosim_mode = block.InputPort(1).Data;
293+
autodrive.rzr_1.posX_command = block.InputPort(2).Data;
294+
autodrive.rzr_1.posY_command = block.InputPort(3).Data;
295+
autodrive.rzr_1.posZ_command = block.InputPort(4).Data;
296+
autodrive.rzr_1.rotX_command = block.InputPort(5).Data;
297+
autodrive.rzr_1.rotY_command = block.InputPort(6).Data;
298+
autodrive.rzr_1.rotZ_command = block.InputPort(7).Data;
299+
autodrive.rzr_1.rotW_command = block.InputPort(8).Data;
300+
autodrive.rzr_1.throttle_command = block.InputPort(9).Data;
301+
autodrive.rzr_1.steering_command = block.InputPort(10).Data;
302+
autodrive.rzr_1.brake_command = block.InputPort(11).Data;
303+
autodrive.rzr_1.handbrake_command = block.InputPort(12).Data;
304+
autodrive.rzr_1.headlights_command = block.InputPort(13).Data;
305+
autodrive.rzr_1.env.auto_time = block.InputPort(14).Data;
306+
autodrive.rzr_1.env.time_scale = block.InputPort(15).Data;
307+
autodrive.rzr_1.env.time_of_day = block.InputPort(16).Data;
308+
autodrive.rzr_1.env.weather_id = block.InputPort(17).Data;
309+
autodrive.rzr_1.env.cloud_intensity = block.InputPort(18).Data;
310+
autodrive.rzr_1.env.fog_intensity = block.InputPort(19).Data;
311+
autodrive.rzr_1.env.rain_intensity = block.InputPort(20).Data;
312+
autodrive.rzr_1.env.snow_intensity = block.InputPort(21).Data;
313+
% Configure outputs
314+
block.OutputPort(1).Data = autodrive.rzr_1.id;
315+
block.OutputPort(2).Data = autodrive.rzr_1.collision_count;
316+
block.OutputPort(3).Data = autodrive.rzr_1.throttle;
317+
block.OutputPort(4).Data = autodrive.rzr_1.steering;
318+
block.OutputPort(5).Data = autodrive.rzr_1.brake;
319+
block.OutputPort(6).Data = autodrive.rzr_1.handbrake;
320+
block.OutputPort(7).Data = autodrive.rzr_1.encoder_ticks;
321+
block.OutputPort(8).Data = autodrive.rzr_1.encoder_angles;
322+
block.OutputPort(9).Data = autodrive.rzr_1.position;
323+
block.OutputPort(10).Data = autodrive.rzr_1.orientation_quaternion;
324+
block.OutputPort(11).Data = autodrive.rzr_1.orientation_euler_angles;
325+
block.OutputPort(12).Data = autodrive.rzr_1.angular_velocity;
326+
block.OutputPort(13).Data = autodrive.rzr_1.linear_acceleration;
327+
% block.OutputPort(14).CurrentDimensions = [size(autodrive.rzr_1.lidar_pointcloud,1),3];
328+
% block.OutputPort(14).Data = autodrive.rzr_1.lidar_pointcloud;
329+
block.OutputPort(14).Data = autodrive.rzr_1.left_camera_image;
330+
block.OutputPort(15).Data = autodrive.rzr_1.right_camera_image;
331+
332+
%end Outputs
333+
334+
%% Update:
335+
%% Functionality: Called to update discrete states
336+
%% during simulation step
337+
%% Required: No
338+
%% C MEX counterpart: mdlUpdate
339+
340+
% function Update(block)
341+
342+
%end Update
343+
344+
%% Derivatives:
345+
%% Functionality: Called to update derivatives of
346+
%% continuous states during simulation step
347+
%% Required: No
348+
%% C MEX counterpart: mdlDerivatives
349+
350+
% function Derivatives(block)
351+
352+
%end Derivatives
353+
354+
%% Terminate:
355+
%% Functionality: Called at the end of simulation for cleanup
356+
%% Required: Yes
357+
%% C MEX counterpart: mdlTerminate
358+
359+
function Terminate(block)
360+
global autodrive
361+
autodrive.stop;
362+
autodrive.delete;
363+
364+
%end Terminate

0 commit comments

Comments
 (0)