TI 官方例程中对3轴加速计获得数据的处理：低通过滤、防抖算法等

TI 官方例程中对3轴加速计获得数据的处理：低通过滤、防抖算法等

摘取自 eZ430-Chronos CC 1_2.tcl

# ----------------------------------------------------------------------------------------
# SimpliciTI acc / ppt -------------------------------------------------------------------

# Zero-set x-y-z values
proc calibrate_sensor {} {
  global simpliciti_on
  global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
  
  # return if SPL is not active  
  if { $simpliciti_on == 0 } { return }
  
  # Wait until new frame has arrived
  after 100
  
  # Clear offset
  set accel_x_offset  0
  set accel_y_offset  0
  set accel_z_offset  0
  set accel_x         0
  set accel_y         0
  set accel_z         0

  # get new data
  get_spl_data
  
  # set new offset
  set accel_x_offset $accel_x
  set accel_y_offset $accel_y
  set accel_z_offset $accel_z
}


# Read received SimpliciTI data from RF access point
proc get_spl_data {} {
  global w
  global simpliciti_on simpliciti_ap_started simpliciti_mode
  global accel_x accel_y accel_z accel_x_offset accel_y_offset accel_z_offset
  global button_event_text button_event previous_button_event button_timeout
    
  # SimpliciTI off?  
  if { $simpliciti_on == 0 } { return }

  # Update status box  
  catch { BM_GetStatus } status

  # Check RF access point status byte  
  if { $status == 2 } {
  
    # Trying to link
    updateStatusSPL "Starting access point."
    return
  
  } elseif { $status == 3 } {
    
    # Read 32-bit SimpliciTI data from dongle
    # Data format is [Z-Y-X-KEY]
    catch { BM_SPL_GetData } data
    
    # Just started? Ignore first data
    if { $simpliciti_ap_started == 0} {

      if { $simpliciti_mode == "acc" } {
        updateStatusSPL "Access point started. Now start watch in acc or ppt mode." 
      } else {
        updateStatusSPL "Access point started. Now start watch in sync mode." 
      }
      set simpliciti_ap_started 1
      return

    } else {

      # if Byte0 is 0xFF, data has already been read from USB buffer, so do nothing
      if { ($data & 0xFF) == 0xFF } { return } 

    }
    
    # Extract watch button event from SimpliciTi data bits 7:0
    set button_event 0
    
    if { [expr ($data & 0xF0) ] != 0 } {
      
      set button_event  [expr ($data & 0xF0) ]
      
      if { $button_event == 0x10 } {
        set button_event_text "Button (*)"
      } elseif { $button_event == 0x20 } {
        set button_event_text "Button (#)"
      } elseif { $button_event == 0x30 } {
        set button_event_text "Button (Up)"
      }

      # Watch can send either key events (2) or mouse clicks (1) - distinguish mode here
      if { [expr ($data & 0x0F) ] == 0x01 } {
        switch $button_event {
          16    { catch { BM_SetMouseClick 1 } res 
                  updateStatusSPL "Left mouse click." }
          32    { catch { BM_SetMouseClick 3 } res
                  updateStatusSPL "Left mouse doubleclick." }
          48    { catch { BM_SetMouseClick 2 } res 
                  updateStatusSPL "Right mouse click." }
        }
      } elseif { [expr ($data & 0x0F) ] == 0x02 } {
        updateStatusSPL "$button_event_text was pressed."
        switch $button_event {
          16    { button_set M1 }
          32    { button_set M2 }
          48    { button_set S1 }
        }
      }
      update
      after 500
      # Dummy read to clear USB buffer
      catch { BM_SPL_GetData } data
      after 20
      catch { BM_SPL_GetData } data
      return
    }
    
    # Keep on drawing X-Y-Z values
    
    # Keep previous values for low pass filtering
    set prev_x  $accel_x
    set prev_y  $accel_y
    set prev_z  $accel_z
    
    # Extract acceleration values from upper data bits
    set accel_x [expr (($data >> 8)  & 0xFF)]
    set accel_y [expr (($data >> 16) & 0xFF)]
    set accel_z [expr (($data >> 24) & 0xFF)]
    
    # Convert raw data to signed integer
    
    # Get sign (1=minus, 0=plus)
    set sign_x  [expr ($accel_x&0x80) >> 7]
    set sign_y  [expr ($accel_y&0x80) >> 7]
    set sign_z  [expr ($accel_z&0x80) >> 7]
    
    # Convert negative 2's complement number to signed decimal
    if { $sign_x == 1 } { 
      set accel_x [ expr (((~$accel_x) & 0xFF ) + 1)*(-1) ]
    }    
    if { $sign_y == 1 } { 
      set accel_y [ expr (((~$accel_y) & 0xFF ) + 1)*(-1) ]
    }    
    if { $sign_z == 1 } { 
      set accel_z [ expr (((~$accel_z) & 0xFF ) + 1)*(-1) ]
    }    
    
    # Low pass filter values from acceleration sensor to avoid jitter
    set accel_x [expr ($accel_x*18*0.5) + $prev_x*0.5 - $accel_x_offset]
    set accel_y [expr ($accel_y*18*0.5) + $prev_y*0.5 - $accel_y_offset]
    set accel_z [expr ($accel_z*18*0.5) + $prev_z*0.5 - $accel_z_offset]
  
    # Display values in status line
    updateStatusSPL "Receiving data from acceleration sensor  X=[format %4.0f $accel_x]  Y=[format %4.0f $accel_y]  Z=[format %4.0f $accel_z]"
    
    # Use values to move mouse cursor 
    move_cursor

    # Update wave graphs
    add_wave_coords
  }
}