-- BNO055 Example
-- Hardware Setup:
--  LabJack - BNO055
--    Vs    - Vin
--    GND   - GND
--    EIO0  - SDA
--    EIO1  - SCL

print("")
print("")

-- ======== Functions ========

-- This function will generate the value for the I2C options
function calc_I2C_options(resetAtStart, noStopAtStarting, disableStretching)--Returns a number 0-7
  local optionsVal = 0
  optionsVal = resetAtStart*1+noStopAtStarting*2+disableStretching*4
  return optionsVal
end

-- Prints all values in a table. Use responsibly. 
function print_Table(t)
  for key,value in pairs(t) do 
    print(key,value) 
  end
end

-- Prints all values in a table in hexadecimal. Use responsibly.
function print_Table_Hex(t)
  for key,value in pairs(t) do 
    print(key,string.format("0x%02X",value)) 
  end
end

-- Reads and displays the acks from the last I2C transaction.
function print_I2CAcks()
  acks = MB.R(5114, 1)
  print("Acks: ", acks)
end

-- Converter two bytes that are the lower and upper bytes of a two's 
--  complementU16 to a float.
function Convert_U8toF32(a, b)
  value_u16 = a + b * 256
  if value_u16 >= 32768 then
    value = value_u16 - 65536
  else
    value = value_u16
  end
  return value
end
-- ======== End of Functions ========

--Configure the I2C Utility
sdaPin = 8--FIO0:7 = 0:7, EIO0:7 = 8:15, CIO0:7 = 16:23
sclPin = 9--These are DIO Nums, corresponding to FIO/EIO/CIO/MIO nums
throttleVal = 65300 --0=fastest; 65535=fastest; 65534, 65533, etc. gets slower.
i2cOptions = calc_I2C_options(0, 0, 0)--Calculate the I2C Options value
myAddr = 0x28--I2C address of the slave device
I2C.config(sdaPin, sclPin, throttleVal, i2cOptions, myAddr)

-- Set the register page to 0.
I2C.writeRead({0x0}, 0)      

-- Read chip information.
RX_buff = I2C.writeRead({0x00}, 7)
print("Chip ID: ", string.format("0x%02X",RX_buff[1]))
print("Software Revision: ", RX_buff[5] + RX_buff[6] * 256)

-- BNO055 defaults to config mode. All values will read zero until we turn things on.
-- Turn on the accelerometer and Magnetometer.
I2C.write({0x3D, 4})
print_I2CAcks()

LJ.IntervalConfig(0, 2000)  -- Once every two seconds...
count = 0
while true do
  if LJ.CheckInterval(0) then
    
    -- Read temperature
--    RX_buff = I2C.writeRead({0x34}, 1)
--    print("Temp: ", string.format("%0.1fºC", RX_buff[1]))
    
    -- Read accelerometer and magnetometer data.
    RX_buff = I2C.writeRead({0x08}, 12)
    print("")
    print("Acc: " , string.format("%i,",Convert_U8toF32(RX_buff[1], RX_buff[2]))
                  , string.format("%i,",Convert_U8toF32(RX_buff[3], RX_buff[4]))
                  , string.format("%i" ,Convert_U8toF32(RX_buff[5], RX_buff[6])) )
           
    print("Mag: " , string.format("%i,",Convert_U8toF32(RX_buff[7], RX_buff[8]))
                  , string.format("%i,",Convert_U8toF32(RX_buff[9], RX_buff[10]))
                  , string.format("%i" ,Convert_U8toF32(RX_buff[11], RX_buff[12])) )
    
    count = count + 1
    if count == 10 then
      break
    end
    
  end
end



--Stop the Lua Script
print("--------")
MB.W(6000, 1, 0)