File content as of revision 13:5566df1250f1:

#include "Updater.h"


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


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

    return &instance;
}


void Updater::start(int interval_ms)
{
    Timer timer;
    
    thisTime = 0;
    lastTime = 0; 
    t = &timer;
    t->start();
   
    EventQueue *queue = mbed_highprio_event_queue();
    Event<void()> event(queue, callback(this, &Updater::update));
    event.period(interval_ms);
    event.post();
    queue->dispatch_forever();
}


void Updater::update()
{
    static DigitalOut led2(LED2);
    static L3G4200D gyro(p9, p10); // TODO parameterize

    // Compute dt
    thisTime = t->read_us();
    _dt = (lastTime < 0) ? 0 : ((float) thisTime - (float) lastTime) / 1000.0; // first pass let dt=0
    lastTime = thisTime;

    // Read encoders
    // Read gyro
    gyro.read(_gyro);
    
    //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
    led2 = !led2;

    return;
}