/* This task collects data from the sensors through I2C bus.
 * It is fairly speed critical, as we need to keep the I2C bus fully occupied
 * in order to get best performance.
 */


#include <ch.h>
#include <hal.h>
#include "sensor_task.h"
#include "lsm303.h"

volatile int mag_x, mag_y, mag_z;
volatile int acc_x, acc_y, acc_z;
volatile unsigned sensor_readings;
volatile unsigned mag_errors;
volatile unsigned acc_errors;
volatile unsigned sensor_start_time;
EventSource sensor_data_event;

static Thread *sensorThread = 0;
static WORKING_AREA(sensorThread_wa, 256);

static msg_t sensor_task(void *arg)
{
    chRegSetThreadName("sensor");
    
    // Read sensors at 200 Hz
    systime_t next_read = chTimeNow();
    while (!chThdShouldTerminate())
    {
        if (next_read <= chTimeNow()) // Workaround for bug # 3532935
            next_read = chTimeNow() + 1;
        
        chThdSleepUntil(next_read);
        next_read += MS2ST(5);
        
        int x,y,z;
        
        // Magnetometer
        if (lsm303_mag_read_results(&x, &y, &z))
        {
            chSysLock();
            mag_x = x; mag_y = y; mag_z = z;
            chSysUnlock();
        }
        else
        {
            mag_errors++;
        }
        
        // Accelerometer
        if (lsm303_acc_read_results(&x, &y, &z))
        {
            chSysLock();
            acc_x = x; acc_y = y; acc_z = z;
            sensor_readings++;
            chSysUnlock();
        }
        else
        {
            acc_errors++;
        }

        chEvtBroadcast(&sensor_data_event);
    }
    
    return 0;
}


static const I2CConfig i2c_config = {
    OPMODE_I2C,
    50000,
    STD_DUTY_CYCLE
};

void sensor_init()
{
    chEvtInit(&sensor_data_event);
}

void sensor_start()
{
    i2cStart(&I2CD2, &i2c_config);    
    
    lsm303_acc_write(LSM303_ACC_CTRL1, 0x67); // All channels @ 200Hz
    lsm303_acc_write(LSM303_ACC_CTRL2, 0x00); // No high-pass filtering
    lsm303_acc_write(LSM303_ACC_CTRL4, 0x88); // Block update, high resolution
    
    lsm303_mag_write(LSM303_MAG_CTRLA, 0x1C); // 220 Hz output rate
    lsm303_mag_write(LSM303_MAG_CTRLB, 0x20); // 1.3 Gauss range
    lsm303_mag_write(LSM303_MAG_MODE, 0x00); // Continuous conversion
    
    sensor_readings = acc_errors = mag_errors = 0;
    sensor_start_time = chTimeNow();
    
    sensorThread = chThdCreateStatic(sensorThread_wa, sizeof(sensorThread_wa), NORMALPRIO + 10, sensor_task, NULL);
}

void sensor_stop()
{
    chThdTerminate(sensorThread);
    chThdWait(sensorThread);
    i2cStop(&I2CD2);
}