File content as of revision 44:0d72a8a1288a:

#include "Updater.h"
#include "pinouts.h"
#include "L3G4200D.h"
#include "LSM303DLM.h"
#include "IncrementalEncoder.h"

Updater::Updater() {
    thisTime = 0;
    lastTime = 0;
    t = new Timer;
    t->start();
    _callback = 0;
}

Updater *Updater::instance() 
{
    static Updater instance;

    return &instance;
}


void Updater::attach(Callback<void()> cb) {
    _callback = cb;

    return;
}


void Updater::update()
{
    static L3G4200D gyro(I2CSDA, I2CSCL);
    static LSM303DLM accel(I2CSDA, I2CSCL);
    static IncrementalEncoder encoder(ALEFT);

    // Compute dt
    thisTime = t->read_us();
    _dt = ((float) thisTime - (float) lastTime) / 1000.0;
    if (lastTime > thisTime) _dt = -_dt; // attempt to fix rollover
    lastTime = thisTime;
    
    _ecount += encoder.read();
    gyro.read(_gyro);
    accel.read(_accel, _mag);
 
    //gyro[_z_] = (g_sign[_z_] * (g[_z_] - g_offset[_z_])) / g_scale[_z_];

    // Save current data into history fifo to use 1 second from now
    //history[now].dist = (sensors.lrEncDistance + sensors.rrEncDistance) / 2.0; // current distance traveled
    //history[now].gyro = sensors.gyro[_z_];  // current raw gyro data
    //history[now].dt = dt; // current dt, to address jitter
    //history[now].hdg = clamp360( history[prev].hdg + dt*(history[now].gyro) ); // compute current heading from current gyro
    //float r = PI/180 * history[now].hdg;
    //history[now].x = history[prev].x + history[now].dist * sin(r);    // update current position
    //history[now].y = history[prev].y + history[now].dist * cos(r);

    // Convert this into some kind of status message/event thingy

    if (_callback)
        _callback();

    return;
}

void Updater::imu(int g[3], int a[3], int m[3], float& dt) 
{
    for (int i=0; i < 3; i++) {
        g[i] = _gyro[i];
        a[i] = _accel[i];
        m[i] = _mag[i];
    }
    dt = _dt;

    return;
}


void Updater::gyro(int g[3], float& dt) 
{
    for (int i=0; i < 3; i++) {
        g[i] = _gyro[i];
    }
    dt = _dt;
    
    return;
}


void Updater::accel(int a[3], float& dt) 
{
    for (int i=0; i < 3; i++) {
        a[i] = _accel[i];
    }
    dt = _dt;
    
    return;
}


void Updater::mag(int m[3], float& dt) 
{
    for (int i=0; i < 3; i++) {
        m[i] = _mag[i];
    }
    dt = _dt;
    
    return;
}


int Updater::encoder()
{
    int result=_ecount;
    _ecount = 0;
    
    return result;    
}