Accelerometer, magnetometer, and gyroscope

projects/bin/build.gradle.kts

@@ -13,6 +13,7 @@ dependencies {
     compile("org.jetbrains.kotlin:kotlin-stdlib-jre8")
     compile(project(":core"))
     compile(project(":gps"))
+    compile(project(":imu"))
     compile(project(":microcontrollers"))
     compile("com.fazecast:jSerialComm:1.3.11")
     compile("org.tinylog:tinylog:1.1")

projects/bin/src/main/kotlin/bin/Main.kt

@@ -2,86 +2,26 @@
 
 package bin
 
-import core.Boat
-import core.PropulsionSystem
-import core.broadcast.*
-import core.values.GpsValue
-import core.values.ValueSerializer
-import gps.Gps
-import gps.PMTK
-import microcontrollers.PropulsionMicrocontroller
-
-import java.net.DatagramSocket
-import java.net.InetAddress
-import java.util.concurrent.CountDownLatch
+import com.pi4j.io.i2c.I2CFactory
+import io.L3GD20
+import io.LSM303DLHC
 import java.util.concurrent.TimeUnit
-
-import rx.Observable
-import rx.broadcast.BasicOrder
-import rx.broadcast.InMemoryBroadcast
-import rx.broadcast.UdpBroadcast
-import rx.schedulers.Schedulers
-
-const val ENVIRONMENT_VARIABLE_PREFIX = "LM_"
-const val STARTUP_MESSAGE = "Lake Maps NL boat control software started"
-
-fun panic(message: String): Nothing = throw RuntimeException(message)
-fun panic(message: String, cause: Throwable): Nothing = throw RuntimeException(message, cause)
-
-fun envPrefixed(name: String) = ENVIRONMENT_VARIABLE_PREFIX + name
-fun env(name: String): String = (envPrefixed(name)).let { System.getenv(it) ?: panic("$it must be set") }
-fun env(name: String, default: String): String = System.getenv().getOrDefault(envPrefixed(name), default)
+import java.util.concurrent.atomic.AtomicBoolean
 
 fun main(args: Array<String>) {
-    println("Lake Maps NL boat control software starting...")
-
-    val propulsionMicrocontroller = SerialPort(env("PROPULSION_SERIAL_DEVICE"), baudRate = 57600).let {
-        serialPort -> PropulsionMicrocontroller(serialPort::recvByte, serialPort::send)
-    }
-
-    val broadcastAddress = InetAddress.getByName(env("BROADCAST_ADDRESS"))
-    val broadcastPort = env("BROADCAST_PORT").toInt()
-    val broadcastSocket = DatagramSocket(broadcastPort)
-    val broadcast = UdpBroadcast(broadcastSocket, broadcastAddress, broadcastPort, ValueSerializer(), BasicOrder())
-
-    val memoryBroadcast = InMemoryBroadcast()
-
-    val gpsEnabled = !env("DISABLE_GPS", "false").toBoolean()
-    if (gpsEnabled) {
-        with(SerialPort(env("GPS_SERIAL_DEVICE"), baudRate = 9600)) {
-            val gps = Gps({ recvChar() }, { send(it) }, { _ -> })
-            gps.setNmeaBaudRate(PMTK.BaudRate.BAUD_RATE_57600)
-            disconnect()
-        }
-
-        val gps = SerialPort(env("GPS_SERIAL_DEVICE"), baudRate = 57600).let {
-            serialPort -> Gps(serialPort::recvChar, serialPort::send, memoryBroadcast::sendAsync)
-        }
-        gps.setNmeaUpdateRate(PMTK.UpdateRate(100))
-        gps.poll()
-
-        Observable.interval(100, TimeUnit.MILLISECONDS)
-            .observeOn(Schedulers.io())
-            .subscribe({
-                // TODO: issue #67
-                gps.poll()
-            }, {
-                e -> panic("Poll GPS timer failed", e)
-            })
+    val i2CBus = I2CFactory.getInstance(1)
+    val accelerometerMagnetometer = LSM303DLHC(i2CBus.getDevice(0b0011001), i2CBus.getDevice(0b0011110))
+    val gyroscope = L3GD20(i2CBus.getDevice(0b1101011))
+
+    // If this runs and print out three values we good
+    val b = AtomicBoolean(true)
+    Runtime.getRuntime().addShutdownHook(Thread({
+        b.set(false)
+    }))
+    while (b.get()) {
+        println(gyroscope.rateOfRotation())
+//        println(accelerometerMagnetometer.acceleration())
+//        println(accelerometerMagnetometer.magneticFluxDensity())
+        TimeUnit.SECONDS.sleep(1)
     }
-
-    val boat = Boat(memoryBroadcast, PropulsionSystem(propulsionMicrocontroller))
-    Runtime.getRuntime().addShutdownHook(Thread(boat::shutdown))
-    boat.start(io = Schedulers.io(), clock = Schedulers.computation())
-
-    // GPS values should be sent across the wire
-    memoryBroadcast.valuesOfType<GpsValue>()
-        .observeOn(Schedulers.io())
-        .subscribe(broadcast::sendAsync, { e -> panic("Rebroadcast GPS value failed", e) })
-
-    // Anything sent across the wire should be re-broadcasted locally
-    broadcast.valuesOfType<Any>().subscribe(memoryBroadcast::sendAsync, { e -> panic("Local rebroadcast failed", e) })
-
-    println(STARTUP_MESSAGE)
-    CountDownLatch(1).await()
 }

projects/imu/build.gradle.kts

@@ -0,0 +1,31 @@
+import org.gradle.api.tasks.testing.logging.TestLogging
+import org.jetbrains.kotlin.gradle.tasks.KotlinCompile
+
+repositories {
+    jcenter()
+}
+
+plugins {
+    id("org.jetbrains.kotlin.jvm") version "1.2.31"
+}
+
+dependencies {
+    compile(project(":units"))
+    compile("com.pi4j:pi4j-core:1.1")
+    compile("org.jetbrains.kotlin:kotlin-stdlib-jre8")
+    testCompile("junit:junit:4.12")
+    testCompile("org.jetbrains.kotlin:kotlin-test-junit")
+}
+
+tasks.withType<Test> {
+    testLogging(closureOf<TestLogging> {
+        showStandardStreams = true
+        events("passed", "skipped", "failed")
+    })
+}
+
+tasks.withType<KotlinCompile> {
+    kotlinOptions {
+        jvmTarget = "1.8"
+    }
+}

projects/imu/src/main/kotlin/io/Accelerometer.kt

@@ -0,0 +1,5 @@
+package io
+
+interface Accelerometer {
+    fun acceleration(): XYZ
+}

projects/imu/src/main/kotlin/io/ByteArrayExtensions.kt

@@ -0,0 +1,3 @@
+package microcontrollers
+
+internal fun ByteArray.hex() = this.joinToString(separator = "") { it.hex() }

projects/imu/src/main/kotlin/io/ByteExtensions.kt

@@ -0,0 +1,3 @@
+package microcontrollers
+
+internal fun Byte.hex() = Integer.toHexString(this.toInt()).takeLast(2).padStart(2, '0')

projects/imu/src/main/kotlin/io/Gyroscope.kt

@@ -0,0 +1,5 @@
+package io
+
+interface Gyroscope {
+    fun rateOfRotation(): XYZ
+}

projects/imu/src/main/kotlin/io/I2CBus.kt

@@ -0,0 +1,5 @@
+package io
+
+interface I2CBus {
+    fun writeBytes(deviceAddress: Int, address: Int, bytes: ByteArray)
+}

projects/imu/src/main/kotlin/io/L3GD20.kt

@@ -0,0 +1,33 @@
+package io
+
+import com.pi4j.io.i2c.I2CDevice
+import java.nio.ByteBuffer
+
+private const val CTRL_REG1 = 0x20
+private const val OUT_X_L = 0x28
+
+class L3GD20(private val i2c: I2CDevice): Gyroscope {
+    init {
+        i2c.write(CTRL_REG1, 0x00)
+        i2c.write(CTRL_REG1, 0x0F)
+    }
+
+    override fun rateOfRotation(): XYZ {
+        val readSize = 6
+        val buffer = ByteArray(readSize)
+        i2c.read(OUT_X_L or 0x80, buffer, 0, readSize)
+
+        val bytes = buffer.foldRight(ByteBuffer.allocate(readSize)) { byte, acc ->
+            acc.put(byte)
+            acc
+        }
+
+        bytes.flip()
+        return with(bytes.asShortBuffer()) {
+            val z = get(0)
+            val y = get(1)
+            val x = get(2)
+            XYZ(x.toDouble(), y.toDouble(), z.toDouble())
+        }
+    }
+}

projects/imu/src/main/kotlin/io/LSM303DLHC.kt

@@ -0,0 +1,54 @@
+package io
+
+import com.pi4j.io.i2c.I2CDevice
+import java.nio.ByteBuffer
+
+private const val CTRL_REG1_A = 0x20
+private const val MR_REG_M = 0x02
+private const val OUT_X_L_A = 0x28
+private const val OUT_X_L_M = 0x03
+
+class LSM303DLHC(private val accelerometerDevice: I2CDevice, private val magDevice: I2CDevice): Accelerometer, Magnetometer {
+    init {
+        accelerometerDevice.write(CTRL_REG1_A, 0x27)
+        magDevice.write(MR_REG_M, 0x00)
+    }
+
+    override fun acceleration(): XYZ {
+        val readSize = 6
+        val buffer = ByteArray(readSize)
+        accelerometerDevice.read(OUT_X_L_A or 0x80, buffer, 0, readSize)
+
+        val bytes = buffer.foldRight(ByteBuffer.allocate(readSize)) { byte, acc ->
+            acc.put(byte)
+            acc
+        }
+
+        bytes.flip()
+        return with(bytes.asShortBuffer()) {
+            val z = get(0)
+            val y = get(1)
+            val x = get(2)
+            XYZ(x.toDouble(), y.toDouble(), z.toDouble())
+        }
+    }
+
+    override fun magneticFluxDensity(): XYZ {
+        val readSize = 6
+        val buffer = ByteArray(readSize)
+        magDevice.read(OUT_X_L_M, buffer, 0, readSize)
+
+        val bytes = buffer.fold(ByteBuffer.allocate(readSize)) { acc, byte ->
+            acc.put(byte)
+            acc
+        }
+
+        bytes.flip()
+        return with(bytes.asShortBuffer()) {
+            val x = get(0)
+            val z = get(1)
+            val y = get(2)
+            XYZ(x.toDouble(), y.toDouble(), z.toDouble())
+        }
+    }
+}

projects/imu/src/main/kotlin/io/Magnetometer.kt

@@ -0,0 +1,5 @@
+package io
+
+interface Magnetometer {
+    fun magneticFluxDensity(): XYZ
+}

projects/imu/src/main/kotlin/io/XYZ.kt

@@ -0,0 +1,3 @@
+package io
+
+data class XYZ(val x: Double, val y: Double, val z: Double)

settings.gradle

@@ -1,6 +1,7 @@
 include "bin"
 include "core"
 include "gps"
+include "imu"
 include "kv"
 include "microcontrollers"
 include "units"