AdafruitLSM303.jun

module AdafruitLSM303
open(Prelude, Time, Vector)
include("\"Adafruit_LSM303.h\"")

// p should be a Adafruit_LSM303 C++ object
alias accMag = { p : ptr, offset : vector<float, 3>, scale : vector<float, 3> }
alias accMagState = { timeState : timerState, 
                      latchState : (vector<float, 3>, vector<float, 3>) }

fun make(p: ptr,
         minX : float, minY : float, minZ : float,
         maxX : float, maxY : float, maxZ : float) : accMag = {
    let offset = [(minX + maxX)/-2.0, (minY + maxY)/-2.0, (minZ + maxZ)/-2.0]
    let scale = [2.0/(maxX - minX), 2.0/(maxY - minY), 2.0/(maxZ - minZ)]
    {
        p := p,
        offset := offset,
        scale := scale
    }
}

let state : accMagState = {
    let z = Vector:zero()
    { timeState := Time:state,
      latchState := (z, z) }
}

fun begin(am : accMag) : unit = {
    let p = am.p
    #((Adafruit_LSM303*) p)->begin()#
}

fun read(am : accMag) : (vector<float, 3>, vector<float, 3>) = {
    let p = am.p
    let mut accX = 0.0f
    let mut accY = 0.0f
    let mut accZ = 0.0f
    let mut magX = 0.0f
    let mut magY = 0.0f
    let mut magZ = 0.0f
    #
    auto ptr = (Adafruit_LSM303*) p;
    ptr->read();
    accX = ptr->accelData.x;
    accY = ptr->accelData.y;
    accZ = ptr->accelData.z;
    magX = ptr->magData.x;
    magY = ptr->magData.y;
    magZ = ptr->magData.z;
    #
    (
        [accX, accY, accZ],
        Vector:multiply(am.scale, Vector:add(am.offset, [magX, magY, magZ]))
    )
}

fun getSignal(updateInterval : uint32,
              accMag         : accMag,
              inout state    : accMagState) : sig<(vector<float, 3>, vector<float, 3>)> =
    Time:every(updateInterval, inout state.timeState) |>
    Signal:map((t) => read(accMag)) |>
    Signal:latch(inout state.latchState)