TI-Nspire & SensorTag / 重力加速度から角度を求める / 2 軸を使う

The output of an accelerometer can be converted to an angle of inclination.
In this note, a Dual-Axis approach will be used to calculate an angle of inclination.



.lua

require "bleCentral"

bleState        = ""
peripheralState = ""
myPeripheral    = nil
peripheralName  = ""

-------------
-- MPU9250 --
-------------
AngleX = nil


dataUUID   = "F000AA81-0451-4000-B000-000000000000"
confUUID   = "F000AA82-0451-4000-B000-000000000000"
periodUUID = "F000AA83-0451-4000-B000-000000000000"
Period     = 100 -- 100 ～ 2550〔msec〕
GyroX , GyroY , GyroZ = nil , nil , nil 
AccX  , AccY  , AccZ  = nil , nil , nil
MagX  , MagY  , MagZ  = nil , nil , nil 
convert = function(value)
             local GyroX , GyroY , GyroZ , AccX , AccY , AccZ , MagX , MagY , MagZ  = string.unpack("s16s16s16s16s16s16s16s16s16", value)
             local factor1 = 500 / 65536
             local factor2 = 8 / 32768 
             return tonumber(GyroX) * factor1 , tonumber(GyroY) * factor1 , tonumber(GyroZ) * factor1 ,
                    tonumber(AccX)  * factor2 , tonumber(AccY)  * factor2 , tonumber(AccZ)  * factor2 ,
                    tonumber(MagX)            , tonumber(MagY)            , tonumber(MagZ)  
          end 
--------------------------------------------------------
--------------------------------------------------------

function on.resize()
   W, H = platform.window:width(), platform.window:height()
   leftMargin = W/100
   fontSize   = W/23
   lineSpace  = H/9
   platform.window:invalidate()
end
function on.construction()
   refreshMenu()
   ble.addStateListener(stateListener)
end
function on.paint(gc)
   gc:setFont("sansserif", "r", fontSize)
   gc:drawString(bleState        , leftMargin, lineSpace * 0)
   gc:drawString(peripheralName  , leftMargin, lineSpace * 1)
   gc:drawString(peripheralState , leftMargin, lineSpace * 2)
   
   
   if GyroX or AccX or MagX then
      gc:drawString("Acc_XYZ (g) : "                                                   , leftMargin , lineSpace * 3)
      gc:drawString("   "..string.format("%.1f , %.1f , %.1f" , AccX  , AccY  , AccZ)  , leftMargin , lineSpace * 4)
      gc:drawString("AngleX (°) : "                                                   , leftMargin , lineSpace * 5)
      gc:drawString("   "..string.format("%.0f" , AngleX)                              , leftMargin , lineSpace * 6)
      
      drawClock(W * 0.8, lineSpace * 4, W / 7, AngleX, gc)

   end
   
   
end

------------------------------
-- Menu, Keyboard and Mouse --
------------------------------
function refreshMenu()
   Menu = {
      {"Controls",
         {"Scan and Connect", function() peripheralOn()  end},
         {"Disconnect"      , function() peripheralOff() end},
      }
   }
   toolpalette.register(Menu)
end

--------------------------
-- Define state listner --
--------------------------
function stateListener(state)
   if state == "on" then
      peripheralOn()
   end 
   bleState = "BLE: "..state
   platform.window:invalidate()
end

----------------------------
-- Start or stop scanning --
----------------------------
function peripheralOn()
   if peripheralState ~= "connected" then
      bleCentral.startScanning(scanner)
   end 
   platform.window:invalidate()
end
function peripheralOff()
   bleCentral.stopScanning()
   if myPeripheral then
      myPeripheral:disconnect()
   end
   peripheralName = "" 
   platform.window:invalidate()
end

--------------------
-- Define scanner --
--------------------
function scanner(peripheral)
   local name = peripheral:getName() or ""
   if peripheral and name:find("SensorTag") then
      myPeripheral   = peripheral
      peripheralName = name
      peripheral:connect(connector) 
   end
   platform.window:invalidate()
end

----------------------
-- Define connector --
----------------------
function connector(peripheral, event)
   local name = peripheral:getName() or ""
   if event == bleCentral.CONNECTED and name:find("SensorTag") then
      bleCentral.stopScanning()
      myPeripheral    = peripheral
      peripheralName  = name
      peripheral:discoverServices(servicesDiscoverer)
   elseif event == bleCentral.DISCONNECTED then 
      myPeripheral    = nil
      peripheralName  = ""
   end
   peripheralState = peripheral:getState()
   platform.window:invalidate()
end

--------------------------------
-- Define services discoverer --
--------------------------------
function servicesDiscoverer(peripheral)
   if peripheral:getState() == bleCentral.CONNECTED then
      local servicesList = peripheral:getServices()
      for _, v in ipairs(servicesList) do
         v:discoverCharacteristics(characteristicsDiscoverer)
      end
   end
   platform.window:invalidate()
end

---------------------------------------
-- Define characteristics discoverer --
---------------------------------------
function characteristicsDiscoverer(service)
   local characteristicsList = service:getCharacteristics()
   for _, characteristic in ipairs(characteristicsList) do
      local UUID = characteristic:getUUID() 


      if UUID == dataUUID then
         characteristic:setValueUpdateListener(valueUpdateListener)
         characteristic:setNotify(true)
      elseif UUID == confUUID then
         local msg = string.pack("u16", 0x007F)
         characteristic:setWriteCompleteListener(valueUpdateListener)
         characteristic:write(msg, true)
      elseif UUID == periodUUID then
         characteristic:setWriteCompleteListener(valueUpdateListener)
         characteristic:write(string.uchar( ((Period - 100) / 10) + 0x0A ), true)  
      end


   end
end

----------------------------------
-- Define value-update listener --
----------------------------------
function valueUpdateListener(characteristic)
   local UUID = characteristic:getUUID()
   if UUID == dataUUID then
      local value = characteristic:getValue()


      if value then
         GyroX , GyroY , GyroZ , 
         AccX  , AccY  , AccZ  ,
         MagX  , MagY  , MagZ  = convert(value)
         
         
         AngleX = math.deg(math.atan2(AccY, AccZ))


      end


   end
   platform.window:invalidate()
end

-----------------------
-- General functions --
-----------------------
function constrain(value, min, max)
   return math.max(min, math.min(value, max))
end
function drawClock(centerX, centerY, radius, degree, gc)
   gc:setPen("thick")
   local diameter = radius + radius
   local radian = math.rad(degree)
   gc:drawArc(centerX - radius, centerY - radius, diameter, diameter, 0, 360)
   gc:drawLine(centerX, centerY, centerX + radius * math.cos(radian), centerY - radius * math.sin(radian))
end