2024年7月8日发(作者：夹谷鸿朗)
TI 官方例程中对3轴加速计获得数据的处理：低通过滤、防抖算法等
摘取自 eZ430-Chronos CC 1_
view source
?001 # ----------------------------------------------------------------------------------------
002 # SimpliciTI acc / ppt -------------------------------------------------------------------
003
004 # Zero-set x-y-z values
005 proc calibrate_sensor {} {
006 global simpliciti_on
007 global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
008
009 # return if SPL is not active
010 if { $simpliciti_on == 0 } { return }
011
012 # Wait until new frame has arrived
013 after 100
014
015 # Clear offset
016 set accel_x_offset 0
017 set accel_y_offset 0
018 set accel_z_offset 0
019 set accel_x 0
020 set accel_y 0
021 set accel_z 0
022
023 # get new data
024 get_spl_data
025
026 # set new offset
027 set accel_x_offset $accel_x
028 set accel_y_offset $accel_y
029 set accel_z_offset $accel_z
030 }
031
032
033 # Read received SimpliciTI data from RF access point
034 proc get_spl_data {} {
035 global w
036 global simpliciti_on simpliciti_ap_started simpliciti_mode
037 global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
038 global button_event_text button_event previous_button_event button_timeout
039
040 # SimpliciTI off?
041 if { $simpliciti_on == 0 } { return }
042
043 # Update status box
044 catch { BM_GetStatus } status
045
046 # Check RF access point status byte
047 if { $status == 2 } {
048
049 # Trying to link
050 updateStatusSPL "Starting access point."
051 return
052
053 } elseif { $status == 3 } {
054
055 # Read 32-bit SimpliciTI data from dongle
056 # Data format is [Z-Y-X-KEY]
057 catch { BM_SPL_GetData } data
058
059 # Just started? Ignore first data
060 if { $simpliciti_ap_started == 0} {
061
062 if { $simpliciti_mode == "acc" } {
063 updateStatusSPL "Access point started. Now start watch in acc or ppt mode."
064 } else {
065 updateStatusSPL "Access point started. Now start watch in sync mode."
066 }
067 set simpliciti_ap_started 1
068 return
069
070 } else {
071
072 # if Byte0 is 0xFF, data has already been read from USB buffer, so do nothing
073 if { ($data & 0xFF) == 0xFF } { return }
074
075 }
076
077 # Extract watch button event from SimpliciTi data bits 7:0
078 set button_event 0
079
080 if { [expr ($data & 0xF0) ] != 0 } {
081
082 set button_event [expr ($data & 0xF0) ]
083
084 if { $button_event == 0x10 } {
085 set button_event_text "Button (*)"
086 } elseif { $button_event == 0x20 } {
087 set button_event_text "Button (#)"
088 } elseif { $button_event == 0x30 } {
089 set button_event_text "Button (Up)"
090 }
091
092 # Watch can send either key events (2) or mouse clicks (1) - distinguish mode here
093 if { [expr ($data & 0x0F) ] == 0x01 } {
094 switch $button_event {
095 16 { catch { BM_SetMouseClick 1 } res
096 updateStatusSPL "Left mouse click." }
097 32 { catch { BM_SetMouseClick 3 } res
098 updateStatusSPL "Left mouse doubleclick." }
099 48 { catch { BM_SetMouseClick 2 } res
100 updateStatusSPL "Right mouse click." }
101 }
102 } elseif { [expr ($data & 0x0F) ] == 0x02 } {
103 updateStatusSPL "$button_event_text was pressed."
104 switch $button_event {
105 16 { button_set M1 }
106 32 { button_set M2 }
107 48 { button_set S1 }
108 }
109 }
110 update
111 after 500
112 # Dummy read to clear USB buffer
113 catch { BM_SPL_GetData } data
114 after 20
115 catch { BM_SPL_GetData } data
116 return
117 }
118
119 # Keep on drawing X-Y-Z values
120
121 # Keep previous values for low pass filtering
122 set prev_x $accel_x
123 set prev_y $accel_y
124 set prev_z $accel_z
125
126 # Extract acceleration values from upper data bits
127 set accel_x [expr (($data >> 8) & 0xFF)]
128 set accel_y [expr (($data >> 16) & 0xFF)]
129 set accel_z [expr (($data >> 24) & 0xFF)]
130
131 # Convert raw data to signed integer
132
133 # Get sign (1=minus, 0=plus)
134 set sign_x [expr ($accel_x&0x80) >> 7]
135 set sign_y [expr ($accel_y&0x80) >> 7]
136 set sign_z [expr ($accel_z&0x80) >> 7]
137
138 # Convert negative 2's complement number to signed decimal
139 if { $sign_x == 1 } {
140 set accel_x [ expr (((~$accel_x) & 0xFF ) + 1)*(-1) ]
141 }
142 if { $sign_y == 1 } {
143 set accel_y [ expr (((~$accel_y) & 0xFF ) + 1)*(-1) ]
144 }
145 if { $sign_z == 1 } {
146 set accel_z [ expr (((~$accel_z) & 0xFF ) + 1)*(-1) ]
147 }
148
149 # Low pass filter values from acceleration sensor to avoid jitter
150 set accel_x [expr ($accel_x*18*0.5) + $prev_x*0.5 - $accel_x_offset]
151 set accel_y [expr ($accel_y*18*0.5) + $prev_y*0.5 - $accel_y_offset]
152 set accel_z [expr ($accel_z*18*0.5) + $prev_z*0.5 - $accel_z_offset]
153
154 # Display values in status line
155 updateStatusSPL "Receiving data from acceleration sensor X=[format %4.0f $accel_x]
Y=[format %4.0f $accel_y] Z=[format %4.0f $accel_z]"
156
157 # Use values to move mouse cursor
158 move_cursor
159
160 # Update wave graphs
161 add_wave_coords
162 }
163 }
2024年7月8日发(作者：夹谷鸿朗)
TI 官方例程中对3轴加速计获得数据的处理：低通过滤、防抖算法等
摘取自 eZ430-Chronos CC 1_
view source
?001 # ----------------------------------------------------------------------------------------
002 # SimpliciTI acc / ppt -------------------------------------------------------------------
003
004 # Zero-set x-y-z values
005 proc calibrate_sensor {} {
006 global simpliciti_on
007 global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
008
009 # return if SPL is not active
010 if { $simpliciti_on == 0 } { return }
011
012 # Wait until new frame has arrived
013 after 100
014
015 # Clear offset
016 set accel_x_offset 0
017 set accel_y_offset 0
018 set accel_z_offset 0
019 set accel_x 0
020 set accel_y 0
021 set accel_z 0
022
023 # get new data
024 get_spl_data
025
026 # set new offset
027 set accel_x_offset $accel_x
028 set accel_y_offset $accel_y
029 set accel_z_offset $accel_z
030 }
031
032
033 # Read received SimpliciTI data from RF access point
034 proc get_spl_data {} {
035 global w
036 global simpliciti_on simpliciti_ap_started simpliciti_mode
037 global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
038 global button_event_text button_event previous_button_event button_timeout
039
040 # SimpliciTI off?
041 if { $simpliciti_on == 0 } { return }
042
043 # Update status box
044 catch { BM_GetStatus } status
045
046 # Check RF access point status byte
047 if { $status == 2 } {
048
049 # Trying to link
050 updateStatusSPL "Starting access point."
051 return
052
053 } elseif { $status == 3 } {
054
055 # Read 32-bit SimpliciTI data from dongle
056 # Data format is [Z-Y-X-KEY]
057 catch { BM_SPL_GetData } data
058
059 # Just started? Ignore first data
060 if { $simpliciti_ap_started == 0} {
061
062 if { $simpliciti_mode == "acc" } {
063 updateStatusSPL "Access point started. Now start watch in acc or ppt mode."
064 } else {
065 updateStatusSPL "Access point started. Now start watch in sync mode."
066 }
067 set simpliciti_ap_started 1
068 return
069
070 } else {
071
072 # if Byte0 is 0xFF, data has already been read from USB buffer, so do nothing
073 if { ($data & 0xFF) == 0xFF } { return }
074
075 }
076
077 # Extract watch button event from SimpliciTi data bits 7:0
078 set button_event 0
079
080 if { [expr ($data & 0xF0) ] != 0 } {
081
082 set button_event [expr ($data & 0xF0) ]
083
084 if { $button_event == 0x10 } {
085 set button_event_text "Button (*)"
086 } elseif { $button_event == 0x20 } {
087 set button_event_text "Button (#)"
088 } elseif { $button_event == 0x30 } {
089 set button_event_text "Button (Up)"
090 }
091
092 # Watch can send either key events (2) or mouse clicks (1) - distinguish mode here
093 if { [expr ($data & 0x0F) ] == 0x01 } {
094 switch $button_event {
095 16 { catch { BM_SetMouseClick 1 } res
096 updateStatusSPL "Left mouse click." }
097 32 { catch { BM_SetMouseClick 3 } res
098 updateStatusSPL "Left mouse doubleclick." }
099 48 { catch { BM_SetMouseClick 2 } res
100 updateStatusSPL "Right mouse click." }
101 }
102 } elseif { [expr ($data & 0x0F) ] == 0x02 } {
103 updateStatusSPL "$button_event_text was pressed."
104 switch $button_event {
105 16 { button_set M1 }
106 32 { button_set M2 }
107 48 { button_set S1 }
108 }
109 }
110 update
111 after 500
112 # Dummy read to clear USB buffer
113 catch { BM_SPL_GetData } data
114 after 20
115 catch { BM_SPL_GetData } data
116 return
117 }
118
119 # Keep on drawing X-Y-Z values
120
121 # Keep previous values for low pass filtering
122 set prev_x $accel_x
123 set prev_y $accel_y
124 set prev_z $accel_z
125
126 # Extract acceleration values from upper data bits
127 set accel_x [expr (($data >> 8) & 0xFF)]
128 set accel_y [expr (($data >> 16) & 0xFF)]
129 set accel_z [expr (($data >> 24) & 0xFF)]
130
131 # Convert raw data to signed integer
132
133 # Get sign (1=minus, 0=plus)
134 set sign_x [expr ($accel_x&0x80) >> 7]
135 set sign_y [expr ($accel_y&0x80) >> 7]
136 set sign_z [expr ($accel_z&0x80) >> 7]
137
138 # Convert negative 2's complement number to signed decimal
139 if { $sign_x == 1 } {
140 set accel_x [ expr (((~$accel_x) & 0xFF ) + 1)*(-1) ]
141 }
142 if { $sign_y == 1 } {
143 set accel_y [ expr (((~$accel_y) & 0xFF ) + 1)*(-1) ]
144 }
145 if { $sign_z == 1 } {
146 set accel_z [ expr (((~$accel_z) & 0xFF ) + 1)*(-1) ]
147 }
148
149 # Low pass filter values from acceleration sensor to avoid jitter
150 set accel_x [expr ($accel_x*18*0.5) + $prev_x*0.5 - $accel_x_offset]
151 set accel_y [expr ($accel_y*18*0.5) + $prev_y*0.5 - $accel_y_offset]
152 set accel_z [expr ($accel_z*18*0.5) + $prev_z*0.5 - $accel_z_offset]
153
154 # Display values in status line
155 updateStatusSPL "Receiving data from acceleration sensor X=[format %4.0f $accel_x]
Y=[format %4.0f $accel_y] Z=[format %4.0f $accel_z]"
156
157 # Use values to move mouse cursor
158 move_cursor
159
160 # Update wave graphs
161 add_wave_coords
162 }
163 }