MPU-9250 MotionTracking device in Nerves/Elixir

mpu9250.ex

defmodule Mpu9250 do
  @moduledoc """
  Documentation for Mpu9250.
  
  Ref.
    1) Lulu's blog/MPU-9250 and Arduino (9-Axis IMU)
       https://lucidar.me/en/inertial-measurement-unit/mpu-9250-and-arduino-9-axis-imu/
    2) Data sheet: https://strawberry-linux.com/pub/PS-MPU-9250A-01.pdf
    3) Regs. map: https://strawberry-linux.com/pub/RM-MPU-9250A-00.pdf
    4) AK8963 Data sheet: https://strawberry-linux.com/pub/AK8963.pdf
  """
  alias Circuits.I2C
  
  @mpu9250 0x68
  @ak8963  0x0C

  @doc """
  Setup MPU9250/65 sensor
  """
  def setup() do
    {:ok, ref} = I2C.open("i2c-1")

    [ 
      {29,  6},  # Set accelerometers low pass filter at 5Hz
      {26,  6},  # Set gyroscope low pass filter at 5Hz
      {27, 16},  # Configure gyroscope range 1000dps
      {28,  8},  # Configure accelerometers range 4G
      {55,  2}   # Set by pass mode for the magnetometers
    ] |> Enum.each(fn {reg, val} -> I2C.write(ref, @mpu9250, <<reg, val>>) end)
    
    [
      {10, 0x16}
    ] |> Enum.each(fn {reg, val} -> I2C.write(ref, @ak8963, <<reg, val>>) end)

    {:ok, ref}
  end

  # helper macro: bitstring pattern / big-endian signed 16bit integer
  defmacrop b_int16(var) do
    quote do
      <<unquote(var)::signed-integer-16>>
    end
  end

  # helper macro: bitstring pattern / little-endian signed 16bit integer
  defmacrop l_int16(var) do
    quote do
      <<unquote(var)::little-signed-integer-16>>
    end
  end

  @doc """
  Read out accelerometer
  """
  def get_acc(ref) do
    <<b_int16(x), b_int16(y), b_int16(z)>> = I2C.write_read!(ref, @mpu9250, <<59>>, 6)
    [x, y, z]
  end
  
  @doc """
  Read out gyroscope
  """
  def get_gyr(ref) do
    <<b_int16(x), b_int16(y), b_int16(z)>> = I2C.write_read!(ref, @mpu9250, <<67>>, 6)
    [x, y, z]
  end

  @doc """
  Read out Magnetometer
  """
  def get_mag(ref) do
    with {:ok, _} <- ready_mag?(ref, 50)
    do
      <<l_int16(y), l_int16(x), l_int16(z), _st2>> = I2C.write_read!(ref, @ak8963, <<3>>, 7)
      [-x, -y, -z]
    else
      _ -> [0, 0, 0]
    end
  end
  
  defp ready_mag?(ref, 0), do: {:error, "over times"}
  defp ready_mag?(ref, retry) do
    <<_::6, dor::1, drdy::1>> = I2C.write_read!(ref, @ak8963, <<2>>, 1)
    case drdy do
      1 -> {:ok, dor}
      _ -> ready_mag?(ref, retry-1)
    end
  end

  @doc """
  Print Measurements
  """
  def print(ref, 0), do: :ok
  def print(ref, count) do
    [ax,ay,az] = get_acc(ref)
    [gx,gy,gz] = get_gyr(ref)
    [mx,my,mz] = get_mag(ref)
    IO.puts "acc-xyz: #{ax}, #{ay}, #{az} / gyr-xyz: #{gx}, #{gy}, #{gz} / mag-xyz: #{mx}, #{my}, #{mz}"
    :timer.sleep(2000)
    print(ref, count-1)
  end
end