main.cpp

00001 /*
00002 
00003 Copyright (c) 2012-2014 RedBearLab
00004 
00005 Permission is hereby granted, free of charge, to any person obtaining a copy of this software 
00006 and associated documentation files (the "Software"), to deal in the Software without restriction, 
00007 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, 
00008 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, 
00009 subject to the following conditions:
00010 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
00011 
00012 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, 
00013 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR 
00014 PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE 
00015 FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, 
00016 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
00017 
00018 */
00019 
00020 #include "mbed.h"
00021 #include "ble/BLE.h"
00022 #include "wire.h"
00023 #include "ButtonService.h"
00024 
00025 #define BLE_Nano
00026 //#define nRF_51822
00027 
00028 #define LIS331HH
00029 
00030 #ifdef nRF_51822
00031 #define SCL         28
00032 #define SDA         29
00033 #endif
00034 
00035 #ifdef BLE_Nano
00036 #define SCL         P0_8 
00037 #define SDA         P0_10
00038 #endif
00039 
00040 
00041 #ifdef LIS3DH
00042 #define ADDR_ONE    0x30
00043 #define ADDR_TWO    0x32
00044 #define AXIS_X      0x00
00045 #define AXIS_Y      0x01
00046 #define AXIS_Z      0x02
00047 #define REG_OUT_X_L 0x28
00048 #define REG_CTRL1   0x20
00049 #define REG_CTRL4   0x23
00050 #define REG_WHOAMI  0x0F
00051 #define RANGE_2G    0x00
00052 #define DEVICE_ID   0x33
00053 
00054 
00055 #define DATARATE_400HZ          0b0111 // 400Hz 
00056 #define DATARATE_200HZ          0b0110 // 200Hz
00057 #define DATARATE_100HZ          0b0101 // 100Hz
00058 #define DATARATE_50HZ           0b0100 // 50Hz
00059 #define DATARATE_25HZ           0b0011 // 25Hz
00060 #define DATARATE_10HZ           0b0010 // 10Hz
00061 #define DATARATE_1HZ            0b0001 // 1Hz
00062 #define DATARATE_POWERDOWN      0      // Power down
00063 #define DATARATE_LOWPOWER_1K6HZ 0b1000 // Low power mode (1.6KHz)
00064 #define DATARATE_LOWPOWER_5KHZ  0b1001 // Low power mode (5KHz) / Normal power mode (1.25KHz)
00065 #endif
00066 
00067 
00068 #ifdef LIS331HH
00069 #define ADDR_ONE    0x30
00070 #define ADDR_TWO    0x32
00071 #define AXIS_X      0x00
00072 #define AXIS_Y      0x01
00073 #define AXIS_Z      0x02
00074 #define REG_OUT_X_L 0x28
00075 #define REG_CTRL1   0x20
00076 #define REG_CTRL4   0x23
00077 #define REG_WHOAMI  0x0F
00078 #define RANGE_2G    0x00
00079 #define DEVICE_ID   0x33
00080 
00081 
00082 #define DATARATE_1KHZ           0b11 // 1000Hz
00083 #define DATARATE_400HZ          0b10 // 400Hz
00084 #define DATARATE_100HZ          0b01 // 100Hz
00085 #define DATARATE_50HZ           0b00 // 50Hz
00086 #define DATARATE_POWERDOWN      0      // Power down
00087 #define DATARATE_NORMAL_MODE    0b001
00088 #define DATARATE_LOWPOWER_0.5HZ 0b010
00089 #define DATARATE_LOWPOWER_1HZ   0b011
00090 #define DATARATE_LOWPOWER_2HZ   0b100
00091 #define DATARATE_LOWPOWER_5HZ   0b101
00092 #define DATARATE_LOWPOWER_10HZ  0b110
00093 #endif
00094 
00095 #define PACKET_SIZE 20
00096 #define QUEUE_SIZE 20
00097 
00098 
00099 const static char     DEVICE_NAME[] = "LUMBERJACK_NANO";
00100 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
00101 
00102 struct packetQueue
00103 {
00104     uint16_t size;
00105     uint16_t nextPacketToSend;
00106     uint16_t nextSampleToSave;
00107     uint16_t liveSamples;
00108     uint8_t packets[QUEUE_SIZE][PACKET_SIZE];
00109 };
00110 
00111 packetQueue pq;
00112 
00113 void addToQueue(uint8_t* packet) {
00114     for (int i = 0; i < PACKET_SIZE; i++) {
00115         pq.packets[pq.nextSampleToSave][i] = packet[i];
00116     }
00117     if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
00118         pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
00119         pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
00120     } else {
00121         pq.liveSamples += 1;
00122         pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
00123     }
00124     return;
00125 }
00126 
00127 uint8_t* removeFromQueue() {
00128     if (pq.nextSampleToSave != pq.nextPacketToSend && pq.liveSamples > 0) {
00129         pq.liveSamples -= 1;
00130         uint8_t* old = pq.packets[pq.nextPacketToSend];
00131         pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
00132         return old;
00133     } else {
00134         return NULL;
00135     }
00136 }
00137     
00138 
00139 Serial pc(USBTX, USBRX);
00140 TwoWire Wire = TwoWire(NRF_TWI0);
00141 
00142 static ButtonService *buttonServicePtr;
00143 bool isThereAConnection = false;
00144 
00145 void sleep(unsigned int mseconds)
00146 {
00147     clock_t goal = mseconds + clock();
00148     while (goal > clock());
00149 }
00150 
00151 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
00152 {
00153     BLE::Instance().gap().startAdvertising();
00154     isThereAConnection = false;
00155 }
00156 
00157 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
00158 {
00159     pc.printf("Connection recieved!\r\n");
00160     isThereAConnection = true;
00161 }
00162 
00163 /**
00164  * This function is called when the ble initialization process has failled
00165  */
00166 void onBleInitError(BLE &ble, ble_error_t error)
00167 {
00168     /* Initialization error handling should go here */
00169 }
00170 
00171 /**
00172  * Callback triggered when the ble initialization process has finished
00173  */
00174 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
00175 {
00176     BLE&        ble   = params->ble;
00177     ble_error_t error = params->error;
00178 
00179     if (error != BLE_ERROR_NONE) {
00180         /* In case of error, forward the error handling to onBleInitError */
00181         onBleInitError(ble, error);
00182         return;
00183     }
00184 
00185     /* Ensure that it is the default instance of BLE */
00186     if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
00187         return;
00188     }
00189 
00190     ble.gap().onDisconnection(disconnectionCallback);
00191     ble.gap().onConnection(connectionCallback);
00192 
00193     /* Setup primary service */
00194     uint8_t initial_value[20] = {0, 0, 0, 0, 0,
00195                                  0, 0, 0, 0, 0,
00196                                  0, 0, 0, 0, 0,
00197                                  0, 0, 0, 0, 0};
00198     buttonServicePtr = new ButtonService(ble, initial_value);
00199 
00200     /* setup advertising */
00201     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
00202     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
00203     ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
00204     ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
00205     ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
00206     pc.printf("start advertising now \r\n");
00207     ble.gap().startAdvertising();
00208 }
00209 
00210 void AT24C512_WriteBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
00211 {
00212     Wire.beginTransmission(i2cAddr);
00213     int err = Wire.write( (uint8_t)addr );
00214     Wire.write(pbuf, length);
00215     if (err != 0) {
00216         pc.printf("error on write write! %d\r\n", err);
00217     }
00218     uint8_t err8 = Wire.endTransmission();
00219     if (err8 != 0) {
00220         pc.printf("error on write end transmission! %d\r\n", err8);
00221     }
00222 }
00223 
00224 void AT24C512_ReadBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
00225 {
00226     Wire.beginTransmission(i2cAddr);
00227     int err= Wire.write( (uint8_t)addr );
00228     if (err != 0) {
00229         pc.printf("error on read write! %d\r\n", err);
00230     }
00231     uint8_t err8 = Wire.endTransmission();
00232     if (err8 != 0) {
00233         pc.printf("error on read end transmission! %d\r\n", err8);
00234     }
00235        
00236     err8 = Wire.requestFrom(i2cAddr+1, length);
00237     if (err != 0) {
00238         pc.printf("error on read request from! %d\r\n", err8);
00239     }
00240     while( Wire.available() > 0 )
00241     {
00242         *pbuf = Wire.read();
00243         pbuf++;
00244     }
00245 }
00246 
00247 //Set the bit at index 'bit' to 'value' on 'input' and return
00248 uint8_t setBit(uint8_t input, uint8_t bit, uint8_t value) {
00249     uint8_t mask = 1 << bit;
00250     input &= ~mask;
00251     if (value == 1) {
00252         input |= mask;
00253     }
00254     return input;
00255 }
00256 
00257 uint16_t getAxis(uint16_t axis, uint16_t i2cAddr)
00258 {
00259     uint8_t base = REG_OUT_X_L + (2 * axis);
00260     uint8_t* low = new uint8_t[1];
00261     uint8_t* high = new uint8_t[1];
00262     AT24C512_ReadBytes(base, low, 1, i2cAddr);
00263     AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
00264     uint16_t res = low[0] | (high[0] << 8);
00265     delete[] low;
00266     delete[] high;
00267     return res;
00268 }
00269 
00270 void setRange(uint8_t range, uint16_t i2cAddr) {
00271     uint8_t* val = new uint8_t[1];
00272     AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
00273     val[0] &= ~(0b110000); //zero out lowest 2 bits of top 4 bits
00274     val[0] |= (range << 4); // write in our new range
00275     pc.printf("REG_CTRL4 after setRange: 0x%x\r\n", *val);
00276     AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
00277     delete[] val;
00278 }
00279 
00280 //Set whether we want to use high resolution or not
00281 void setHighResolution(bool highRes, uint16_t i2cAddr) {
00282     uint8_t* val = new uint8_t[1];
00283     AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
00284     uint8_t final;
00285     if (highRes) {
00286         final = setBit(val[0], 3, 1);
00287     } else {
00288         final = setBit(val[0], 3, 0);
00289     }
00290     val[0] = final;
00291     pc.printf("REG_CTRL4 after setHiRes: 0x%x\r\n", *val);
00292     AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
00293     delete[] val;
00294 }
00295 
00296 void setAxisStatus(uint8_t axis, bool enable, uint16_t i2cAddr) {
00297     uint8_t* current = new uint8_t[1];
00298     AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);//get value from the register
00299     uint8_t final;
00300     if (enable == 1) {
00301         final = setBit(current[0], axis, 1);
00302     } else {
00303         final = setBit(current[0], axis, 0);
00304     }
00305     current[0] = final;
00306     AT24C512_WriteBytes(REG_CTRL1, current, 1, i2cAddr);
00307     
00308     AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);
00309     pc.printf("REG_CTRL1 after setAxisStatus: 0x%x\r\n", *current);
00310     delete[] current;
00311 }
00312 
00313 void setDataRate(uint8_t dataRate, uint16_t i2cAddr) {
00314     uint8_t* val = new uint8_t[1];
00315     AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
00316     pc.printf("REG_CTRL1 before data rate set: 0x%x\r\n", *val);
00317     val[0] &= 0b11100111; //d
00318     val[0] |= (dataRate << 3);
00319     AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
00320     
00321     AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
00322     pc.printf("REG_CTRL1 after data rate set: 0x%x\r\n", *val);
00323     delete[] val;
00324 }
00325 
00326 void setPowerMode(uint8_t powerMode, uint16_t i2cAddr) {
00327     uint8_t* val = new uint8_t[1];
00328     AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
00329     val[0] &= 0b11111;
00330     val[0] |= (powerMode << 5);
00331     //pc.printf("writing this to REG_CTRL1: 0x%x\r\n", *val);
00332     AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
00333     
00334     AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
00335     pc.printf("REG_CTRL1 after power mode set: 0x%x\r\n", *val);
00336     delete[] val;
00337 }
00338     
00339 void setBDU(bool bdu, uint16_t i2cAddr)
00340 {
00341     uint8_t* val = new uint8_t[1];
00342     AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
00343     pc.printf("REG_CTRL4: 0x%x\r\n", *val);
00344     uint8_t final;
00345     if (bdu) {
00346         final = setBit(val[0], 7, 1);
00347     } else {
00348         final = setBit(val[0], 7, 0);
00349     }
00350     val[0] = final;
00351     pc.printf("REG_CTRL4 after setBDU: 0x%x\r\n", *val);
00352     AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
00353     delete[] val;
00354 }
00355 
00356 uint16_t getX(uint16_t i2cAddr)
00357 {
00358     return getAxis(AXIS_X, i2cAddr);
00359 }
00360 
00361 uint16_t getY(uint16_t i2cAddr)
00362 {
00363     return getAxis(AXIS_Y, i2cAddr); 
00364 }
00365 
00366 uint16_t getZ(uint16_t i2cAddr)
00367 {
00368     return getAxis(AXIS_Z, i2cAddr);
00369 }
00370 
00371 int main(void)
00372 {
00373     pc.baud(9600);
00374     wait(5);
00375     //Wire.begin();
00376     Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
00377 
00378     pc.printf("\r\n\r\n\r\nStarting...\r\n");
00379 
00380     wait(5);
00381     
00382     setAxisStatus(AXIS_X, true, ADDR_ONE);
00383     setAxisStatus(AXIS_Y, true, ADDR_ONE);
00384     setAxisStatus(AXIS_Z, true, ADDR_ONE);
00385     setDataRate(DATARATE_400HZ, ADDR_ONE);
00386     setPowerMode(DATARATE_NORMAL_MODE, ADDR_ONE);
00387     //setHighResolution(true, ADDR_ONE);
00388     setBDU(true, ADDR_ONE);
00389     //setRange(RANGE_2G, ADDR_ONE);
00390     
00391     
00392     setAxisStatus(AXIS_X, true, ADDR_TWO);
00393     setAxisStatus(AXIS_Y, true, ADDR_TWO);
00394     setAxisStatus(AXIS_Z, true, ADDR_TWO);
00395     setDataRate(DATARATE_400HZ, ADDR_TWO);
00396     setPowerMode(DATARATE_NORMAL_MODE, ADDR_TWO); 
00397     //setHighResolution(true, ADDR_TWO);
00398     setBDU(true, ADDR_TWO);
00399     //setRange(RANGE_2G, ADDR_TWO);
00400 
00401     uint8_t* val = new uint8_t[1];
00402     *val = 0x80;
00403     AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_ONE);
00404     AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_TWO);
00405     AT24C512_ReadBytes(REG_CTRL4, val, 1, ADDR_ONE);
00406     pc.printf("REG_CTRL4, should be 0x80: 0x%x\r\n", *val);
00407 
00408     uint8_t* whoami = new uint8_t[1];
00409     AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_ONE);
00410     pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
00411     AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_TWO);
00412     pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
00413     AT24C512_ReadBytes(0x1F, whoami, 1, ADDR_ONE);
00414 
00415     
00416     BLE &ble = BLE::Instance();
00417     ble.init(bleInitComplete);
00418     
00419     pc.printf("entering spin loop\r\n");
00420     /* SpinWait for initialization to complete. This is necessary because the
00421      * BLE object is used in the main loop below. */
00422     while (ble.hasInitialized()  == false) { /* spin loop */ }
00423     pc.printf("leaving spin loop\r\n");
00424     
00425     pq.size = QUEUE_SIZE;
00426     pq.nextPacketToSend = 0;
00427     pq.nextSampleToSave = 0;
00428     pq.liveSamples = 0;
00429     
00430     while(1)
00431     {
00432         //pc.printf("Read data from AT24C512\r\n");
00433         uint16_t x1 = getX(ADDR_ONE);
00434         uint16_t y1 = getY(ADDR_ONE);
00435         uint16_t z1 = getZ(ADDR_ONE);
00436         
00437         uint16_t x2 = getX(ADDR_TWO);
00438         uint16_t y2 = getY(ADDR_TWO);
00439         uint16_t z2 = getZ(ADDR_TWO);
00440         
00441         
00442         pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
00443         pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
00444         pc.printf("\r\n");
00445         
00446         if(isThereAConnection) {
00447             pc.printf("sending Notification\r\n");
00448             uint8_t values[20] = {(uint8_t)x1, (uint8_t)(x1 >> 8), (uint8_t)y1, (uint8_t)(y1 >> 8), (uint8_t)z1, (uint8_t)(z1 >> 8),
00449                                   (uint8_t)x2, (uint8_t)(x2 >> 8), (uint8_t)y2, (uint8_t)(y2 >> 8), (uint8_t)z2, (uint8_t)(z2 >> 8),
00450                                   0, 0, 0, 0, 0, 0, 0, 0};
00451             buttonServicePtr->updateButtonState(values);
00452         }
00453 
00454         wait(1);
00455     }
00456 
00457 }