Movatterモバイル変換

[0]ホーム
URL:

Skip to content

Navigation Menu

Sign in
Appearance settings

Search code, repositories, users, issues, pull requests...

Provide feedback

We read every piece of feedback, and take your input very seriously.

Saved searches

Use saved searches to filter your results more quickly

 Sign up
Appearance settings

Commitec87ada

Browse files
committed
fixes missing type definitions for new style SPI.
Also adds the start of an IMU sensor framework and fusion math.
1 parentdb0d39d commitec87ada

File tree

6 files changed

+747
-0
lines changed

6 files changed

+747
-0
lines changed
Lines changed: 119 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,119 @@
1+
# Copyright (c) 2024 - 2025 Kevin G. Schlosser
2+
3+
importlvglaslv
4+
5+
6+
classAutoRotation:
7+
8+
def__init__(self,device,delay,lock_rotation=False,adjustment=0.0):
9+
10+
ifadjustment>180oradjustment<-180:
11+
raiseValueError('adjustment range is -180.0 to +180.0')
12+
13+
scrn=lv.screen_active()
14+
disp=scrn.get_display()
15+
16+
self._device=device
17+
18+
self._timer=lv.timer_create(self._timer_cb,delay)
19+
self._timer.set_repeat_count(-1)
20+
21+
self._fusion=fusion.Fusion()
22+
self._lock_rotation=lock_rotation
23+
24+
self._last_rotation=disp.get_rotation()
25+
self._last_free_rotation=0
26+
self._adjustment=adjustment
27+
28+
@property
29+
defadjustment(self):
30+
returnself._adjustment
31+
32+
@adjustment.setter
33+
defadjustment(self,value):
34+
self._adjustment=value
35+
self._timer_cb(None)
36+
37+
@property
38+
deflock_rotation(self):
39+
returnself._lock_rotation
40+
41+
@lock_rotation.setter
42+
deflock_rotation(self,value):
43+
self._lock_rotation=value
44+
self._timer_cb(None)
45+
46+
def__del__(self):
47+
ifself._timerisnotNone:
48+
self._timer.delete()
49+
50+
def_timer_cb(self,_):
51+
roll,pitch,yaw=self._device.read()
52+
53+
roll+=self._adjustment
54+
55+
ifroll>180.00:
56+
roll-=360.0
57+
58+
ifroll<-180.0:
59+
roll+=360.0
60+
61+
ifroll<0.0:
62+
roll+=360.0
63+
64+
scrn=lv.screen_active()
65+
disp=scrn.get_display()
66+
67+
ifself._lock_rotation:
68+
69+
if45<=roll<135:
70+
new_rotation=lv.DISPLAY_ROTATION._90
71+
72+
elif135<=roll<225:
73+
new_rotation=lv.DISPLAY_ROTATION._180
74+
75+
elif225<=roll<315:
76+
new_rotation=lv.DISPLAY_ROTATION._270
77+
78+
else:
79+
new_rotation=lv.DISPLAY_ROTATION._0
80+
81+
ifnew_rotation!=self._last_rotation:
82+
self._last_rotation=new_rotation
83+
disp.set_rotation(new_rotation)
84+
else:
85+
roll=int(roll*10.0)
86+
87+
ifself._last_rotation!=lv.DISPLAY_ROTATION._0:
88+
self._last_rotation=lv.DISPLAY_ROTATION._0
89+
disp.set_rotation(lv.DISPLAY_ROTATION._0)
90+
91+
ifroll!=self._last_free_rotation:
92+
self._last_free_rotation=roll
93+
top_layer=disp.get_layer_top()
94+
sys_layer=disp.get_layer_sys()
95+
bottom_layer=disp.get_layer_bottom()
96+
97+
top_layer_pivot_x=int(top_layer.get_width()/2)
98+
top_layer_pivot_y=int(top_layer.get_height()/2)
99+
top_layer.set_style_transform_pivot_x(top_layer_pivot_x,lv.PART.ANY)
100+
top_layer.set_style_transform_pivot_y(top_layer_pivot_y,lv.PART.ANY)
101+
top_layer.set_style_transform_rotation(roll,lv.PART.ANY)
102+
103+
sys_layer_pivot_x=int(sys_layer.get_width()/2)
104+
sys_layer_pivot_y=int(sys_layer.get_height()/2)
105+
sys_layer.set_style_transform_pivot_x(sys_layer_pivot_x,lv.PART.ANY)
106+
sys_layer.set_style_transform_pivot_y(sys_layer_pivot_y,lv.PART.ANY)
107+
sys_layer.set_style_transform_rotation(roll,lv.PART.ANY)
108+
109+
bottom_layer_pivot_x=int(bottom_layer.get_width()/2)
110+
bottom_layer_pivot_y=int(bottom_layer.get_height()/2)
111+
bottom_layer.set_style_transform_pivot_x(bottom_layer_pivot_x,lv.PART.ANY)
112+
bottom_layer.set_style_transform_pivot_y(bottom_layer_pivot_y,lv.PART.ANY)
113+
bottom_layer.set_style_transform_rotation(roll,lv.PART.ANY)
114+
115+
scrn_pivot_x=int(scrn.get_width()/2)
116+
scrn_pivot_y=int(scrn.get_height()/2)
117+
scrn.set_style_transform_pivot_x(scrn_pivot_x,lv.PART.ANY)
118+
scrn.set_style_transform_pivot_y(scrn_pivot_y,lv.PART.ANY)
119+
scrn.set_style_transform_rotation(roll,lv.PART.ANY)
Lines changed: 186 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,186 @@
1+
frommicropythonimportconst# NOQA
2+
importimu_sensor_framework# NOQA
3+
4+
5+
_VERSION_REG=const(0x0)
6+
_REVISION_REG=const(0x1)
7+
8+
_TIME_REG=const(0x30)
9+
_TEMP_REG=const(0x33)
10+
_ACCEL_REG=const(0x35)
11+
_GYRO_REG=const(0x3B)
12+
13+
_CONFIG2_REG=const(0x02)
14+
_CONFIG5_REG=const(0x05)
15+
_CONFIG6_REG=const(0x06)
16+
_CONFIG7_REG=const(0x07)
17+
18+
_ACCEL_SETTING_REG=const(0x03)
19+
_GYRO_SETTING_REG=const(0x04)
20+
_ENABLE_REG=const(0x08)
21+
22+
# STANDARD_GRAVITY = 9.80665
23+
24+
25+
def_decode_settings(value):
26+
rnge=value>>4
27+
rate=value&0xF
28+
29+
returnrnge,rate
30+
31+
32+
def_encode_setting(rnge,rate):
33+
return ((rnge&0x7)<<4)| (rate&0xF)
34+
35+
36+
ACCEL_RANGE_2=const(0)# +/- 2g
37+
ACCEL_RANGE_4=const(0x10)# +/- 4g
38+
ACCEL_RANGE_8=const(0x20)# +/- 8g (default value)
39+
ACCEL_RANGE_16=const(0x30)# +/- 16g
40+
41+
GYRO_RANGE_16=const(0)# +/- 16 deg/s
42+
GYRO_RANGE_32=const(0x10)# +/- 32 deg/s
43+
GYRO_RANGE_64=const(0x20)# +/- 64 deg/s
44+
GYRO_RANGE_128=const(0x30)# +/- 128 deg/s
45+
GYRO_RANGE_256=const(0x40)# +/- 256 deg/s (default value)
46+
GYRO_RANGE_512=const(0x50)# +/- 512 deg/s
47+
GYRO_RANGE_1024=const(0x60)# +/- 1024 deg/s
48+
GYRO_RANGE_2048=const(0x70)# +/- 2048 deg/s
49+
50+
ACCEL_RATE_8000_HZ=const(0)
51+
ACCEL_RATE_4000_HZ=const(1)
52+
ACCEL_RATE_2000_HZ=const(2)
53+
ACCEL_RATE_1000_HZ=const(3)
54+
ACCEL_RATE_500_HZ=const(4)
55+
ACCEL_RATE_250_HZ=const(5)
56+
ACCEL_RATE_125_HZ=const(6)# (default value)
57+
ACCEL_RATE_62_HZ=const(7)
58+
ACCEL_RATE_31_HZ=const(8)
59+
ACCEL_RATE_LP_128_HZ=const(12)
60+
ACCEL_RATE_LP_21_HZ=const(13)
61+
ACCEL_RATE_LP_11_HZ=const(14)
62+
ACCEL_RATE_LP_3_HZ=const(15)
63+
64+
65+
GYRO_RATE_8000_HZ=const(0)
66+
GYRO_RATE_4000_HZ=const(1)
67+
GYRO_RATE_2000_HZ=const(2)
68+
GYRO_RATE_1000_HZ=const(3)
69+
GYRO_RATE_500_HZ=const(4)
70+
GYRO_RATE_250_HZ=const(5)
71+
GYRO_RATE_125_HZ=const(6)# (default value)
72+
GYRO_RATE_62_HZ=const(7)
73+
GYRO_RATE_31_HZ=const(8)
74+
75+
I2C_ADDR=0x6B
76+
BITS=8
77+
78+
79+
classQMI8658C(imu_sensor_framework.IMUSensorFramework):
80+
81+
def_read_reg(self,reg):
82+
self._device.read_mem(reg,self._rx_mv[:1])
83+
returnself._rx_buf[0]
84+
85+
def_write_reg(self,reg,data):
86+
self._tx_buf[0]=data
87+
self._device.write_mem(reg,self._tx_mv[:1])
88+
89+
def__init__(self,device,delay_between_samples=100):
90+
91+
super().__init__(device,0.0,delay_between_samples)
92+
self._device=device
93+
self._tx_buf=bytearray(1)
94+
self._tx_mv=memoryview(self._tx_buf)
95+
self._rx_buf=bytearray(6)
96+
self._rx_mv=memoryview(self._rx_buf)
97+
98+
ifself._read_reg(_VERSION_REG)!=0x05:
99+
raiseRuntimeError("Failed to find QMI8658C")
100+
101+
self._accel_range=ACCEL_RANGE_8
102+
self._accel_rate=ACCEL_RATE_125_HZ
103+
104+
self._gyro_range=GYRO_RANGE_256
105+
self._gyro_rate=GYRO_RATE_125_HZ
106+
107+
self._write_reg(_CONFIG2_REG,0x60)
108+
self._write_reg(_ACCEL_SETTING_REG,_encode_setting(ACCEL_RANGE_8,ACCEL_RATE_125_HZ))
109+
self._write_reg(_GYRO_SETTING_REG,_encode_setting(GYRO_RANGE_256,GYRO_RATE_125_HZ))
110+
111+
self._write_reg(_CONFIG5_REG,0x00)# No magnetometer
112+
self._write_reg(_CONFIG6_REG,0x00)# Disables Gyroscope And Accelerometer Low-Pass Filter
113+
self._write_reg(_CONFIG7_REG,0x00)# Disables Motion on Demand.
114+
self._write_reg(_ENABLE_REG,0x03)# enable accel and gyro
115+
116+
@property
117+
defaccel_range(self):
118+
returnself._accel_range
119+
120+
@accel_range.setter
121+
defaccel_range(self,value):
122+
self._accel_range=value
123+
self._write_reg(_ACCEL_SETTING_REG,_encode_setting(value,self._accel_rate))
124+
125+
@property
126+
defaccel_rate(self):
127+
returnself._accel_rate
128+
129+
@accel_rate.setter
130+
defaccel_rate(self,value):
131+
self._accel_rate=value
132+
self._write_reg(_ACCEL_SETTING_REG,_encode_setting(self._accel_range,value))
133+
134+
@property
135+
defgyro_range(self):
136+
returnself._gyro_range
137+
138+
@gyro_range.setter
139+
defgyro_range(self,value):
140+
self._gyro_range=value
141+
self._write_reg(_ACCEL_SETTING_REG,_encode_setting(value,self._gyro_rate))
142+
143+
@property
144+
defgyro_rate(self):
145+
returnself._gyro_rate
146+
147+
@gyro_rate.setter
148+
defgyro_rate(self,value):
149+
self._gyro_rate=value
150+
self._write_reg(_ACCEL_SETTING_REG,_encode_setting(self._gyro_range,value))
151+
152+
@property
153+
deftimestamp(self)->int:
154+
self._device.read_mem(_TIME_REG,self._rx_mv[:3])
155+
returnself._rx_buf[0]+ (self._rx_buf[1]<<8)+ (self._rx_buf[2]<<16)
156+
157+
@property
158+
deftemperature(self)->float:
159+
"""Chip temperature"""
160+
self._device.read_mem(_TEMP_REG,self._rx_mv[:2])
161+
temp=self._rx_buf[0]/256+self._rx_buf[1]
162+
returntemp
163+
164+
def_get_accelerometer(self):
165+
self._device.read_mem(_ACCEL_REG,self._rx_mv[:6])
166+
167+
buf=self._rx_buf
168+
169+
x=buf[0]<<8|buf[1]
170+
y=buf[2]<<8|buf[3]
171+
z=buf[4]<<8|buf[5]
172+
173+
returnx,y,z
174+
175+
def_get_gyrometer(self):
176+
self._device.read_mem(_GYRO_REG,self._rx_mv[:6])
177+
buf=self._rx_buf
178+
179+
x=buf[0]<<8|buf[1]
180+
y=buf[2]<<8|buf[3]
181+
z=buf[4]<<8|buf[5]
182+
183+
returnx,y,z
184+
185+
def_get_magnetometer(self):# NOQA
186+
returnNone
Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
importfusion
2+
importtime
3+
4+
5+
classIMUSensorFramework:
6+
# delay between samples is nanosecond resolution. 1000 nanoseconds = 1 millisecond
7+
8+
def__init__(self,device,declination_adjustment=0.0,delay_between_samples=100):
9+
self._device=device
10+
self._fusion=fusion.Fusion(declination=declination_adjustment)
11+
self._delay_between_samples=delay_between_samples
12+
self._roll=0.0
13+
self._pitch=0.0
14+
self._yaw=0.0
15+
16+
@property
17+
defroll(self):
18+
returnself._roll
19+
20+
@property
21+
defpitch(self):
22+
returnself._pitch
23+
24+
@property
25+
defyaw(self):
26+
returnself._yaw
27+
28+
def_get_gyrometer(self):
29+
raiseNotImplementedError
30+
31+
def_get_accelerometer(self):
32+
raiseNotImplementedError
33+
34+
def_get_magnetometer(self):
35+
raiseNotImplementedError
36+
37+
defcalibrate(self,sample_count=5):
38+
ts= [time.ticks_ns()]
39+
count= [0]
40+
41+
def_stop_func():
42+
ifcount[0]==sample_count:
43+
returnTrue
44+
45+
now_ts=time.ticks_ns()
46+
diff=time.ticks_diff(now_ts,ts[0])
47+
remaining=self._delay_between_samples-diff
48+
ifremaining>0:
49+
time.sleep_ns(remaining)
50+
51+
ts[0]=time.ticks_ns()
52+
count[0]+=1
53+
returnFalse
54+
55+
self._fusion.calibrate(self._get_magnetometer,_stop_func)
56+
57+
defread(self):
58+
accel=self._get_accelerometer()
59+
gyro=self._get_gyrometer()
60+
61+
try:
62+
mag=self._get_magnetometer()
63+
exceptNotImplementedError:
64+
mag=None
65+
66+
self._roll,self._pitch,self._yaw=self._fusion.update(accel,gyro,mag)
67+
68+
returnself._roll,self._pitch,self._yaw

0 commit comments

Comments
 (0)
[8]ページ先頭
©2009-2025 Movatter.jp