File content as of revision 16:799397f0d3a8:

/*
 
Copyright (c) 2012-2014 RedBearLab
 
Permission is hereby granted, free of charge, to any person obtaining a copy of this software
and associated documentation files (the "Software"), to deal in the Software without restriction,
including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
subject to the following conditions:
The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
 
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 
*/
 
#include "mbed.h"
#include "ble/BLE.h"
#include "wire.h"
#include "ButtonService.h"
 
#define BLE_Nano
//#define nRF_51822
 
#define LIS331HH
 
#ifdef nRF_51822
#define SCL         28
#define SDA         29
#endif
 
#ifdef BLE_Nano
#define SCL         P0_8
#define SDA         P0_10
#endif
 
 
#ifdef LIS3DH
#define ADDR_ONE    0x30
#define ADDR_TWO    0x32
#define AXIS_X      0x00
#define AXIS_Y      0x01
#define AXIS_Z      0x02
#define REG_OUT_X_L 0x28
#define REG_CTRL1   0x20
#define REG_CTRL4   0x23
#define REG_WHOAMI  0x0F
#define RANGE_2G    0x00
#define DEVICE_ID   0x33
 
 
#define DATARATE_400HZ          0b0111 // 400Hz
#define DATARATE_200HZ          0b0110 // 200Hz
#define DATARATE_100HZ          0b0101 // 100Hz
#define DATARATE_50HZ           0b0100 // 50Hz
#define DATARATE_25HZ           0b0011 // 25Hz
#define DATARATE_10HZ           0b0010 // 10Hz
#define DATARATE_1HZ            0b0001 // 1Hz
#define DATARATE_POWERDOWN      0      // Power down
#define DATARATE_LOWPOWER_1K6HZ 0b1000 // Low power mode (1.6KHz)
#define DATARATE_LOWPOWER_5KHZ  0b1001 // Low power mode (5KHz) / Normal power mode (1.25KHz)
#endif
 
 
#ifdef LIS331HH
#define ADDR_ONE    0x30
#define ADDR_TWO    0x32
#define AXIS_X      0x00
#define AXIS_Y      0x01
#define AXIS_Z      0x02
#define REG_OUT_X_L 0x28
#define REG_CTRL1   0x20
#define REG_CTRL4   0x23
#define REG_WHOAMI  0x0F
#define RANGE_2G    0x00
#define DEVICE_ID   0x33
 
 
#define DATARATE_1KHZ           0b11 // 1000Hz
#define DATARATE_400HZ          0b10 // 400Hz
#define DATARATE_100HZ          0b01 // 100Hz
#define DATARATE_50HZ           0b00 // 50Hz
#define DATARATE_POWERDOWN      0      // Power down
#define DATARATE_NORMAL_MODE    0b001
#define DATARATE_LOWPOWER_05HZ 0b010
#define DATARATE_LOWPOWER_1HZ   0b011
#define DATARATE_LOWPOWER_2HZ   0b100
#define DATARATE_LOWPOWER_5HZ   0b101
#define DATARATE_LOWPOWER_10HZ  0b110
#endif
 
#define PACKET_SIZE 20
#define QUEUE_SIZE 20

#define Z_THRESHOLD 5000
 
const static char     DEVICE_NAME[] = "LUMBERJACK_NANO";
static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
 
struct packetQueue
{
    uint16_t size;
    uint16_t nextPacketToSend;
    uint16_t nextSampleToSave;
    uint16_t liveSamples;
    uint8_t packets[QUEUE_SIZE][PACKET_SIZE];
};
 
packetQueue pq;
 
void addToQueue(uint8_t* packet) {
    for (int i = 0; i < PACKET_SIZE; i++) {
        pq.packets[pq.nextSampleToSave][i] = packet[i];
    }
    if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
    } else {
        pq.liveSamples += 1;
        pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
    }
    return;
}
 
uint8_t* removeFromQueue() {
    if (pq.nextSampleToSave != pq.nextPacketToSend && pq.liveSamples > 0) {
        pq.liveSamples -= 1;
        uint8_t* old = pq.packets[pq.nextPacketToSend];
        pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
        return old;
    } else {
        return NULL;
    }
}
   
 
//Serial pc(USBTX, USBRX);
TwoWire Wire = TwoWire(NRF_TWI0);
 
static ButtonService *buttonServicePtr;
bool isThereAConnection = false;
bool weAreSending = false;
 
void sleep(unsigned int mseconds)
{
    clock_t goal = mseconds + clock();
    while (goal > clock());
}
 
void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
{
    BLE::Instance().gap().startAdvertising();
    isThereAConnection = false;
}
 
void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
{
    //pc.printf("Connection recieved!\r\n");
    isThereAConnection = true;
}

void startTransmission() {
    uint8_t* nextPacket = removeFromQueue();
    buttonServicePtr->updateButtonState(nextPacket);
}

void dataSentCallback(unsigned count) {
    //pc.printf("dataSent!!\r\n");
    uint8_t* nextPacket = removeFromQueue();
    buttonServicePtr->updateButtonState(nextPacket);
}
 
/**
 * This function is called when the ble initialization process has failled
 */
void onBleInitError(BLE &ble, ble_error_t error)
{
    /* Initialization error handling should go here */
}
 
/**
 * Callback triggered when the ble initialization process has finished
 */
void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
{
    BLE&        ble   = params->ble;
    ble_error_t error = params->error;
 
    if (error != BLE_ERROR_NONE) {
        /* In case of error, forward the error handling to onBleInitError */
        onBleInitError(ble, error);
        return;
    }
 
    /* Ensure that it is the default instance of BLE */
    if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
        return;
    }
 
    ble.gap().onDisconnection(disconnectionCallback);
    ble.gap().onConnection(connectionCallback);
    ble.onDataSent(dataSentCallback);
 
    /* Setup primary service */
    uint8_t initial_value[20] = {0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0,
                                 0, 0, 0, 0, 0};
    buttonServicePtr = new ButtonService(ble, initial_value);
 
    /* setup advertising */
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
    ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
    ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
    ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
    //pc.printf("start advertising now \r\n");
    ble.gap().startAdvertising();
}
 
void AT24C512_WriteBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
{
    Wire.beginTransmission(i2cAddr);
    int err = Wire.write( (uint8_t)addr );
    Wire.write(pbuf, length);
    if (err != 0) {
        //pc.printf("error on write write! %d\r\n", err);
    }
    uint8_t err8 = Wire.endTransmission();
    if (err8 != 0) {
        //pc.printf("error on write end transmission! %d\r\n", err8);
    }
}
 
void AT24C512_ReadBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
{
    Wire.beginTransmission(i2cAddr);
    int err= Wire.write( (uint8_t)addr );
    if (err != 0) {
        //pc.printf("error on read write! %d\r\n", err);
    }
    uint8_t err8 = Wire.endTransmission();
    if (err8 != 0) {
        //pc.printf("error on read end transmission! %d\r\n", err8);
    }
       
    err8 = Wire.requestFrom(i2cAddr+1, length);
    if (err != 0) {
        //pc.printf("error on read request from! %d\r\n", err8);
    }
    while( Wire.available() > 0 )
    {
        *pbuf = Wire.read();
        pbuf++;
    }
}
 
//Set the bit at index 'bit' to 'value' on 'input' and return
uint8_t setBit(uint8_t input, uint8_t bit, uint8_t value) {
    uint8_t mask = 1 << bit;
    input &= ~mask;
    if (value == 1) {
        input |= mask;
    }
    return input;
}
 
int16_t getAxis(uint16_t axis, uint16_t i2cAddr)
{
    uint8_t base = REG_OUT_X_L + (2 * axis);
    uint8_t* low = new uint8_t[1];
    uint8_t* high = new uint8_t[1];
    AT24C512_ReadBytes(base, low, 1, i2cAddr);
    AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
    int16_t res = low[0] | (high[0] << 8);
    delete[] low;
    delete[] high;
    return res;
}
 
void setRange(uint8_t range, uint16_t i2cAddr) {
    uint8_t* val = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
    val[0] &= ~(0b110000); //zero out lowest 2 bits of top 4 bits
    val[0] |= (range << 4); // write in our new range
    //pc.printf("REG_CTRL4 after setRange: 0x%x\r\n", *val);
    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
    delete[] val;
}
 
//Set whether we want to use high resolution or not
void setHighResolution(bool highRes, uint16_t i2cAddr) {
    uint8_t* val = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
    uint8_t final;
    if (highRes) {
        final = setBit(val[0], 3, 1);
    } else {
        final = setBit(val[0], 3, 0);
    }
    val[0] = final;
    //pc.printf("REG_CTRL4 after setHiRes: 0x%x\r\n", *val);
    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
    delete[] val;
}
 
void setAxisStatus(uint8_t axis, bool enable, uint16_t i2cAddr) {
    uint8_t* current = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);//get value from the register
    uint8_t final;
    if (enable == 1) {
        final = setBit(current[0], axis, 1);
    } else {
        final = setBit(current[0], axis, 0);
    }
    current[0] = final;
    AT24C512_WriteBytes(REG_CTRL1, current, 1, i2cAddr);
   
    AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);
    //pc.printf("REG_CTRL1 after setAxisStatus: 0x%x\r\n", *current);
    delete[] current;
}
 
void setDataRate(uint8_t dataRate, uint16_t i2cAddr) {
    uint8_t* val = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
    //pc.printf("REG_CTRL1 before data rate set: 0x%x\r\n", *val);
    val[0] &= 0b11100111; //d
    val[0] |= (dataRate << 3);
    AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
   
    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
    //pc.printf("REG_CTRL1 after data rate set: 0x%x\r\n", *val);
    delete[] val;
}
 
void setPowerMode(uint8_t powerMode, uint16_t i2cAddr) {
    uint8_t* val = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
    val[0] &= 0b11111;
    val[0] |= (powerMode << 5);
    //pc.printf("writing this to REG_CTRL1: 0x%x\r\n", *val);
    AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
   
    AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
    //pc.printf("REG_CTRL1 after power mode set: 0x%x\r\n", *val);
    delete[] val;
}
   
void setBDU(bool bdu, uint16_t i2cAddr)
{
    uint8_t* val = new uint8_t[1];
    AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
    //pc.printf("REG_CTRL4: 0x%x\r\n", *val);
    uint8_t final;
    if (bdu) {
        final = setBit(val[0], 7, 1);
    } else {
        final = setBit(val[0], 7, 0);
    }
    val[0] = final;
    //pc.printf("REG_CTRL4 after setBDU: 0x%x\r\n", *val);
    AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
    delete[] val;
}
 
int16_t getX(uint16_t i2cAddr)
{
    return getAxis(AXIS_X, i2cAddr);
}
 
int16_t getY(uint16_t i2cAddr)
{
    return getAxis(AXIS_Y, i2cAddr);
}
 
int16_t getZ(uint16_t i2cAddr)
{
    return getAxis(AXIS_Z, i2cAddr);
}
 
int main(void)
{
    //pc.baud(9600);
    wait(5);
    //Wire.begin();
    Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
 
    //pc.printf("\r\n\r\n\r\nStarting...\r\n");
 
    wait(5);
   
    setAxisStatus(AXIS_X, true, ADDR_ONE);
    setAxisStatus(AXIS_Y, true, ADDR_ONE);
    setAxisStatus(AXIS_Z, true, ADDR_ONE);
    setDataRate(DATARATE_400HZ, ADDR_ONE);
    setPowerMode(DATARATE_NORMAL_MODE, ADDR_ONE);
    //setHighResolution(true, ADDR_ONE);
    setBDU(true, ADDR_ONE);
    //setRange(RANGE_2G, ADDR_ONE);
   
   
    setAxisStatus(AXIS_X, true, ADDR_TWO);
    setAxisStatus(AXIS_Y, true, ADDR_TWO);
    setAxisStatus(AXIS_Z, true, ADDR_TWO);
    setDataRate(DATARATE_400HZ, ADDR_TWO);
    setPowerMode(DATARATE_NORMAL_MODE, ADDR_TWO);
    //setHighResolution(true, ADDR_TWO);
    setBDU(true, ADDR_TWO);
    //setRange(RANGE_2G, ADDR_TWO);
 
    uint8_t* val = new uint8_t[1];
    *val = 0x80;
    AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_ONE);
    AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_TWO);
    AT24C512_ReadBytes(REG_CTRL4, val, 1, ADDR_ONE);
    //pc.printf("REG_CTRL4, should be 0x80: 0x%x\r\n", *val);
 
    uint8_t* whoami = new uint8_t[1];
    AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_ONE);
    //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
    AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_TWO);
    //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
    AT24C512_ReadBytes(0x1F, whoami, 1, ADDR_ONE);
 
   
    BLE &ble = BLE::Instance();
    ble.init(bleInitComplete);
   
    //pc.printf("entering spin loop\r\n");
    /* SpinWait for initialization to complete. This is necessary because the
     * BLE object is used in the main loop below. */
    while (ble.hasInitialized()  == false) { /* spin loop */ }
    //pc.printf("leaving spin loop\r\n");
   
    pq.size = QUEUE_SIZE;
    pq.nextPacketToSend = 0;
    pq.nextSampleToSave = 0;
    pq.liveSamples = 0;
    
    uint8_t lastThreePackets[3][20];
    uint8_t lastThreeIndex = 0;
    uint8_t underThresholdCount = 0;
    bool inAKeyStroke = false;
   
    while(1)
    {
        //pc.printf("Read data from AT24C512\r\n");
        int16_t x1 = getX(ADDR_ONE);
        int16_t y1 = getY(ADDR_ONE);
        int16_t z1 = getZ(ADDR_ONE);
       
        int16_t x2 = getX(ADDR_TWO);
        int16_t y2 = getY(ADDR_TWO);
        int16_t z2 = getZ(ADDR_TWO);
       
        //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
        //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
        uint8_t values[20] = {(uint8_t)(x1 >> 8), (uint8_t)x1, (uint8_t)(y1 >> 8), (uint8_t)y1, (uint8_t)(z1 >> 8), (uint8_t)z1,
                              (uint8_t)(x2 >> 8), (uint8_t)x2, (uint8_t)(y2 >> 8), (uint8_t)y2, (uint8_t)(z2 >> 8), (uint8_t)z2,
                              0, 0, 0, 0, 0, 0, 0, 0};
        //TODO: handle negative accels
        if (z1 > Z_THRESHOLD || z2 > Z_THRESHOLD) {
            underThresholdCount = 0;
            if (!inAKeyStroke) {
                //start transmitting
                inAKeyStroke = true;
                for (int i = 0; i < 3; i++) {
                    addToQueue(lastThreePackets[(lastThreeIndex - 1 - i) % 3]);
                }
                addToQueue(values);
                startTransmission();
            } else {
                addToQueue(values);
            }
        } else if (underThresholdCount < 3 && inAKeyStroke) {
            underThresholdCount++;
            addToQueue(values);
        } else {
            for (int i = 0; i < 20; i++) {
                lastThreePackets[lastThreeIndex][i] = values[i];
            }
            lastThreeIndex = (lastThreeIndex + 1) % 3;
            inAKeyStroke = false;
        }
 
        wait_ms(50);
    }
 
}