Permalink
Find file
Fetching contributors…
Cannot retrieve contributors at this time
60 lines (53 sloc) 2.18 KB
module AdafruitLSM303
open(Prelude, Time, Vector)
include("\"Adafruit_LSM303.h\"")
type accMag = { ptr : pointer; offset : vector<float;3>; scale : vector<float;3> }
type accMagState = { timeState : timerState ref;
latchState : (vector<float;3>*vector<float;3>) ref }
fun make(minX : float, minY : float, minZ : float,
maxX : float, maxY : float, maxZ : float) : accMag = (
let offset = Vector:make<float;3>([(minX + maxX)/-2.0, (minY + maxY)/-2.0, (minZ + maxZ)/-2.0]);
let scale = Vector:make<float;3>([2.0/(maxX - minX), 2.0/(maxY - minY), 2.0/(maxZ - minZ)]);
let p = null;
#p.set((void*) new Adafruit_LSM303());#;
accMag { ptr = p; offset = offset; scale = scale }
)
fun state() : accMagState ref = (
let z = Vector:zero<float;3>();
ref accMagState { timeState = Time:state();
latchState = ref (z,z) }
)
fun begin(am : accMag) : unit = (
let p = am.ptr;
#((Adafruit_LSM303*) p.get())->begin();#
)
fun read(am : accMag) : (vector<float;3> * vector<float;3>) = (
let p = am.ptr;
let mutable accX : float = 0.0;
let mutable accY : float = 0.0;
let mutable accZ : float = 0.0;
let mutable magX : float = 0.0;
let mutable magY : float = 0.0;
let mutable magZ : float = 0.0;
#auto ptr = (Adafruit_LSM303*) p.get();
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;#;
(Vector:make<float;3>([accX, accY, accZ]),
Vector:multiply<float;3>(am.scale, Vector:add<float;3>(am.offset, Vector:make<float;3>([magX, magY, magZ]))))
)
fun getSignal(updateInterval : uint32,
accMag : accMag,
state : accMagState ref) : sig<(vector<float;3>*vector<float;3>)> = (
let tickSig = Time:every(updateInterval, (!state).timeState);
let accelComUpdateSig =
Signal:map<uint32, (vector<float;3>*vector<float;3>)>(
fn (t : uint32) : (vector<float;3>*vector<float;3>) ->
read(accMag),
tickSig);
Signal:latch<(vector<float;3>*vector<float;3>)>(accelComUpdateSig, (!state).latchState)
)