1'''
2AHRS - AHRS_visualize.py with synchronous mode.
3
4This example visualize AHRS by using 3D model.
5
6-------------------------------------------------------------------------------------
7Please change correct serial number or IP and port number BEFORE you run example code.
8
9For other examples please check:
10 https://github.com/WPC-Systems-Ltd/WPC_Python_driver_release/tree/main/examples
11See README.md file to get detailed usage of this example.
12
13Copyright (c) 2022-2025 WPC Systems Ltd. All rights reserved.
14'''
15
16## Python
17import numpy as np
18import stl.mesh as mesh
19import mpl_toolkits.mplot3d as mplot3d
20from PIL import Image
21import matplotlib.pyplot as plt
22import matplotlib.gridspec as gridspec
23import matplotlib.image as mpimg
24import matplotlib.font_manager as font_manager
25from matplotlib import rcParams
26from matplotlib.transforms import Affine2D
27
28## WPC
29from wpcsys import pywpc
30
31################################################################################
32## Configuration
33
34DATA_PATH = 'Material/viz_data/'
35IMG_PATH = 'Material/viz_data/avion_'
36
37for font in font_manager.findSystemFonts(DATA_PATH):
38 font_manager.fontManager.addfont(font)
39
40## Set font family globally
41rcParams['font.family'] = 'Digital-7 Mono'
42
43## Style
44plt.style.use("bmh")
45BMH_BURGUNDY = "#a70f34"
46BMH_BLUE = "#3d90be"
47BMH_PURPLE = "#7c6ca4"
48
49for font in font_manager.findSystemFonts(DATA_PATH):
50 font_manager.fontManager.addfont(font)
51
52## Set font family globally
53rcParams['font.family'] = 'Digital-7 Mono'
54
55IMG_WPC = plt.imread('Material/trademark.jpg')
56IMG_WPC_PIL = Image.open('Material/trademark.jpg')
57
58################################################################################
59## Plane Images and Constants
60
61IMG_YAW = mpimg.imread(f'{IMG_PATH}yaw.png')
62IMG_PITCH = mpimg.imread(f'{IMG_PATH}pitch.png')
63IMG_ROLL = mpimg.imread(f'{IMG_PATH}roll.png')
64
65SIZE_YAW = np.array([IMG_YAW.shape[0], IMG_YAW.shape[1]])
66CENTER_YAW = SIZE_YAW / 2
67DIAG_YAW = np.sqrt(np.sum(SIZE_YAW ** 2))
68ALPHA_YAW = (DIAG_YAW - SIZE_YAW) / 2
69
70
71SIZE_PITCH = np.array([IMG_PITCH.shape[0], IMG_PITCH.shape[1]])
72CENTER_PITCH = SIZE_PITCH / 2
73DIAG_PITCH = np.sqrt(np.sum(SIZE_PITCH ** 2))
74ALPHA_PITCH = (DIAG_PITCH - SIZE_PITCH) / 2
75
76SIZE_ROLL = np.array([IMG_ROLL.shape[0], IMG_ROLL.shape[1]])
77CENTER_ROLL = SIZE_ROLL / 2
78DIAG_ROLL = np.sqrt(np.sum(SIZE_ROLL ** 2))
79ALPHA_ROLL = (DIAG_ROLL - SIZE_ROLL) / 2
80
81## Dictionaries
82PLANE_DICT = {
83 'yaw': dict(img=IMG_YAW, size=SIZE_YAW, center=CENTER_YAW, diag=DIAG_YAW, alpha=ALPHA_YAW),
84 'pitch': dict(img=IMG_PITCH, size=SIZE_PITCH, center=CENTER_PITCH, diag=DIAG_PITCH, alpha=ALPHA_PITCH),
85 'roll': dict(img=IMG_ROLL, size=SIZE_ROLL, center=CENTER_ROLL, diag=DIAG_ROLL, alpha = ALPHA_ROLL),
86}
87
88MODEL_DICT = {
89 'cat': dict(label='Cat', view=400),
90 'rat': dict(label='Rat', view=50)
91}
92DEFAULT_MODEL = 'rat'
93
94################################################################################
95## Constants
96
97DEGREE_TO_RADIAN = np.pi / 180.0
98RADIAN_TO_DEGREE = 180.0 / np.pi
99
100################################################################################
101## Functions - utilities
102
103def WPC_initializeFigure(nb_rows=1, nb_col=1, share_x='all', share_y='all', option='none'):
104 fig = plt.figure(figsize=(10, 6))
105 fig.clf()
106 gs = gridspec.GridSpec(3, 3, width_ratios=[3, 1, 1], height_ratios=[1, 1, 1])
107
108 ## If `option` is `3D`, return a 3-D axis.
109 if option == '3D':
110 ax_main = fig.add_subplot(gs[:, 0], projection='3d')
111 ax_main.set_title('Main 3D Plot')
112 ax_plane_yaw = fig.add_subplot(gs[0, 1])
113 ax_plane_pitch = fig.add_subplot(gs[1, 1])
114 ax_plane_roll = fig.add_subplot(gs[2, 1])
115 ax_value_yaw = fig.add_subplot(gs[0, 2])
116 ax_value_pitch = fig.add_subplot(gs[1, 2])
117 ax_value_roll = fig.add_subplot(gs[2, 2])
118
119 Pil_width = IMG_WPC_PIL.width
120 Pil_height = IMG_WPC_PIL.height
121 new_pil_image = IMG_WPC_PIL.resize((int(Pil_width//3), int(Pil_height//3)))
122 fig.figimage(new_pil_image, xo=fig.bbox.xmin, yo=fig.bbox.ymin, alpha=0.8, zorder = 0)
123
124 ## fig.figimage(IMG_WPC, xo=fig.bbox.xmin, yo=fig.bbox.ymin, alpha=1)
125 ax_dict = {
126 'main': ax_main,
127 'plane_yaw': ax_plane_yaw,
128 'plane_pitch': ax_plane_pitch,
129 'plane_roll': ax_plane_roll,
130 'value_yaw': ax_value_yaw,
131 'value_pitch': ax_value_pitch,
132 'value_roll': ax_value_roll,
133 }
134
135 return fig, ax_dict
136
137 ## If `option` is `grid`, return gridspec for further usage.
138 if option == 'grid':
139 ax_grid = gridspec.GridSpec(nb_rows, nb_col, figure=fig)
140 return fig, None, None, ax_grid
141
142 ## If empty rows or columns, return `None`.
143 if nb_rows == 0 or nb_col == 0:
144 return fig, None, None, None
145
146 ## Return subplots
147 ax_mat = fig.subplots(nb_rows, nb_col, sharex=share_x, sharey=share_y, squeeze=False)
148
149 ## sharex/sharey if the subplot would share same axis, squeeze=False to ensure ax_mat is 2D array
150 ax_arr = ax_mat.flatten()
151 ax = ax_arr[0]
152
153 ## Add grid to the figure
154 fig.grid(True)
155 return fig, ax, ax_arr, ax_mat
156
157def WPC_initialize_plane(angle_type, fig, ax):
158 ## Load image
159 ax.set_axis_off()
160 d_plane = PLANE_DICT[angle_type]
161 img_plane = d_plane['img']
162 im = ax.imshow(img_plane)
163
164 size_plane = d_plane['size']
165 center_plane = d_plane['center']
166 diag_plane = d_plane['diag']
167 alpha_plane = d_plane['alpha']
168 tt = 1.15 ## Tolerance threshold of 20 percent to not have cut circle
169
170 ax.set_xlim(-alpha_plane[1]*tt, (size_plane[1] + alpha_plane[1])*tt)
171 ax.set_ylim(-alpha_plane[0]*tt, (size_plane[0] + alpha_plane[0])*tt)
172
173 ## display circle
174 cc = plt.Circle((center_plane[1], center_plane[0]), diag_plane/2, fill=False, color='black', linewidth=2, linestyle='dashdot')
175 ax.add_artist(cc)
176 return im
177
178def WPC_showFigure(fig):
179 w, h = fig.get_size_inches()
180 fig.set_size_inches(w, h, forward=True)
181 plt.ion() ## Turn on interactive mode
182 plt.show()
183 return
184
185def WPC_getRotMat(roll, pitch, yaw, use_deg=False):
186 if use_deg:
187 roll *= DEGREE_TO_RADIAN
188 pitch *= DEGREE_TO_RADIAN
189 yaw *= DEGREE_TO_RADIAN
190 rot_mat_x = np.array([[1, 0, 0], [0, np.cos(roll), np.sin(roll)], [0, -np.sin(roll), np.cos(roll)]])
191 rot_mat_y = np.array([[np.cos(pitch), 0, np.sin(pitch)], [0, 1, 0], [-np.sin(pitch), 0, np.cos(pitch)]])
192 rot_mat_z = np.array([[np.cos(yaw), -np.sin(yaw), 0], [np.sin(yaw), np.cos(yaw), 0], [0, 0, 1]])
193 rot_mat = rot_mat_x.dot(rot_mat_y).dot(rot_mat_z) ## Dot product from the back RxRyRz
194 return rot_mat
195
196def WPC_showEmpty(tag=DEFAULT_MODEL, save=0):
197 fig, ax_dict = WPC_initializeFigure(option='3D')
198 im_yaw = WPC_initialize_plane('yaw', fig, ax_dict.get('plane_yaw'))
199 im_pitch = WPC_initialize_plane('pitch', fig, ax_dict.get('plane_pitch'))
200 im_roll = WPC_initialize_plane('roll', fig, ax_dict.get('plane_roll'))
201
202 im_dict = {
203 'yaw': im_yaw,
204 'pitch': im_pitch,
205 'roll': im_roll,
206 }
207
208 d = MODEL_DICT[tag] #model dict dictionary
209
210 ## Load
211 model = mesh.Mesh.from_file(f'{DATA_PATH}{tag}.stl')
212 data_orig = model.vectors ## 3D array
213
214 ## Plot
215 poly = mplot3d.art3d.Poly3DCollection([], color=BMH_BLUE, edgecolor='k', lw=0.2) #or lightsteelblue
216 ax_dict.get('main').add_collection3d(poly)
217
218 ## Settings
219 scale = [-0.6*d['view'], 0.6*d['view']]
220 ax_dict.get('main').view_init(elev=0, azim=0, roll=0)
221 ax_dict.get('main').auto_scale_xyz(scale, scale, scale)
222 ax_dict.get('main').set_axis_off()
223 ax_dict.get('main').set_title(f'Sensor fusion {tag}', weight='bold')
224
225 ## Save
226 fig.set_size_inches(6, 6)
227 fig.subplots_adjust(left=0.05, right=0.95, bottom=0.05, top=0.95)
228 fig.canvas.manager.set_window_title('WPC AHRS visualization')
229
230 WPC_showFigure(fig)
231 return fig, ax_dict, im_dict, data_orig, poly
232
233def WPC_draw3DModel(fig, ax, data_orig, poly, roll, pitch, yaw):
234 ## Rotate
235 rot_mat = WPC_getRotMat(roll, pitch, yaw, use_deg=True)
236 data = data_orig.dot(rot_mat)
237
238 ## Plot
239 poly.set_verts(data)
240 return
241
242def WPC_plot_plane(fig, ax, angle_type, im, center):
243 ## Display image
244 im.remove()
245
246 ## Calculate rotation angle in radians
247 angle = np.deg2rad(angle_type) #Convert in en radians
248
249 ## Apply rotation to the image
250 rotation_matrix = Affine2D().rotate_deg_around(center[1], center[0], np.rad2deg(angle)+180)
251 im.set_transform(rotation_matrix + ax.transData)
252
253 ## Show the image again
254 ax.add_artist(im)
255 return
256
257def WPC_text_button(fig, Roll, Pitch, Yaw, ax_value_roll, ax_value_pitch, ax_value_yaw):
258 for ax in [ax_value_roll, ax_value_pitch, ax_value_yaw]:
259 ax.clear()
260 ax.axis('off')
261 ax.add_patch(plt.Rectangle((0, 0), 1, 1, facecolor='#ebebeb', transform=ax.transAxes, zorder=-1))
262 ax.grid(False)
263 roll = "{:7.2f}".format(Roll)
264 pitch = "{:7.2f}".format(Pitch)
265 yaw = "{:7.2f}".format(Yaw)
266 ax_value_roll.text(0.5, 0.5, f' Roll:\n{roll} deg', fontsize=32, weight='bold', color=BMH_BLUE,ha='center', va='center')
267 ax_value_pitch.text(0.5, 0.5, f' Pitch:\n{pitch} deg', fontsize=32, weight='bold', color=BMH_BURGUNDY, ha='center', va='center')
268 ax_value_yaw.text(0.5, 0.5, f' Yaw:\n{yaw} deg', fontsize=32, weight='bold', color=BMH_PURPLE, ha='center', va='center')
269 return
270
271def main():
272 ## Get Python driver version
273 print(f'{pywpc.PKG_FULL_NAME} - Version {pywpc.__version__}')
274
275 ## Create device handle
276 dev = pywpc.WifiDAQE3AH()
277
278 ## Show empty
279 fig, ax_dict, im_dict, data_orig, poly = WPC_showEmpty()
280
281 ## Connect to device
282 try:
283 dev.connect("192.168.5.38") ## Depend on your device
284 except Exception as err:
285 pywpc.printGenericError(err)
286 ## Release device handle
287 dev.close()
288 return
289
290 try:
291 ## Parameters setting
292 port = 0 ## Depend on your device
293 mode = 0 ## 0: Orientation, 1: Acceleration, 2: Orientation + Acceleration
294 timeout = 3 ## [sec]
295
296 ## Get firmware model & version
297 driver_info = dev.Sys_getDriverInfo(timeout)
298 print("Model name: " + driver_info[0])
299 print("Firmware version: " + driver_info[-1])
300
301 ## Open AHRS and update rate is 333 HZ
302 err = dev.AHRS_open(port, timeout)
303 print(f"AHRS_open in port {port}, status: {err}")
304
305 ## Start AHRS
306 err = dev.AHRS_start(port, timeout)
307 print(f"AHRS_start in port {port}, status: {err}")
308
309 while plt.fignum_exists(fig.number):
310 ahrs_list = dev.AHRS_getEstimate(port, mode, timeout)
311 if len(ahrs_list) > 0:
312 WPC_draw3DModel(fig, ax_dict.get('main'), data_orig, poly, ahrs_list[0], ahrs_list[1], ahrs_list[2])
313 WPC_plot_plane(fig, ax_dict.get('plane_roll'), ahrs_list[0], im_dict.get('roll'), CENTER_ROLL)
314 WPC_plot_plane(fig, ax_dict.get('plane_pitch'), ahrs_list[1], im_dict.get('pitch'), CENTER_PITCH)
315 WPC_plot_plane(fig, ax_dict.get('plane_yaw'), ahrs_list[2], im_dict.get('yaw'), CENTER_YAW)
316 WPC_text_button(fig, ahrs_list[0], ahrs_list[1], ahrs_list[2], ax_dict.get('value_roll'), ax_dict.get('value_pitch'), ax_dict.get('value_yaw'))
317 plt.tight_layout()
318 plt.pause(2**-5)
319
320 except KeyboardInterrupt:
321 print("Press keyboard")
322
323 except Exception as err:
324 pywpc.printGenericError(err)
325
326 finally:
327 ## Stop AHRS
328 err = dev.AHRS_stop(port, timeout)
329 print(f"AHRS_stop in port {port}, status: {err}")
330
331 ## Close AHRS
332 err = dev.AHRS_close(port, timeout)
333 print(f"AHRS_close in port {port}, status: {err}")
334
335 ## Disconnect device
336 dev.disconnect()
337
338 ## Release device handle
339 dev.close()
340
341 return
342if __name__ == '__main__':
343 main()