Zachary Newman / Mbed 2 deprecated zach_thresholding

zach_thesholding

Dependencies:   BLE_API mbed nRF51822

Fork of BLE_notifications_with_orig_mbed by Nicholas Kosarek

main.cpp@16:799397f0d3a8, 2017-05-02 (annotated)

Committer:
znew711
Date:
Tue May 02 23:42:04 2017 +0000
Revision:
16:799397f0d3a8
Parent:
15:d117591084ff
Child:
17:09ceae7cb00e
first draft of thresholding

Who changed what in which revision?

UserRevisionLine numberNew contents of line
nkosarek 15:d117591084ff 1 /*
nkosarek 15:d117591084ff 2
nkosarek 15:d117591084ff 3 Copyright (c) 2012-2014 RedBearLab
nkosarek 15:d117591084ff 4
nkosarek 15:d117591084ff 5 Permission is hereby granted, free of charge, to any person obtaining a copy of this software
nkosarek 15:d117591084ff 6 and associated documentation files (the "Software"), to deal in the Software without restriction,
nkosarek 15:d117591084ff 7 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
nkosarek 15:d117591084ff 8 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
nkosarek 15:d117591084ff 9 subject to the following conditions:
nkosarek 15:d117591084ff 10 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
nkosarek 15:d117591084ff 11
nkosarek 15:d117591084ff 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
nkosarek 15:d117591084ff 13 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
nkosarek 15:d117591084ff 14 PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
nkosarek 15:d117591084ff 15 FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
nkosarek 15:d117591084ff 16 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
nkosarek 15:d117591084ff 17
nkosarek 15:d117591084ff 18 */
nkosarek 15:d117591084ff 19
rgrover1 0:28f095301cb2 20 #include "mbed.h"
andresag 10:7943b5c1117a 21 #include "ble/BLE.h"
nkosarek 15:d117591084ff 22 #include "wire.h"
rgrover1 0:28f095301cb2 23 #include "ButtonService.h"
nkosarek 15:d117591084ff 24
nkosarek 15:d117591084ff 25 #define BLE_Nano
nkosarek 15:d117591084ff 26 //#define nRF_51822
nkosarek 15:d117591084ff 27
nkosarek 15:d117591084ff 28 #define LIS331HH
nkosarek 15:d117591084ff 29
nkosarek 15:d117591084ff 30 #ifdef nRF_51822
nkosarek 15:d117591084ff 31 #define SCL 28
nkosarek 15:d117591084ff 32 #define SDA 29
nkosarek 15:d117591084ff 33 #endif
nkosarek 15:d117591084ff 34
nkosarek 15:d117591084ff 35 #ifdef BLE_Nano
nkosarek 15:d117591084ff 36 #define SCL P0_8
nkosarek 15:d117591084ff 37 #define SDA P0_10
nkosarek 15:d117591084ff 38 #endif
nkosarek 15:d117591084ff 39
nkosarek 15:d117591084ff 40
nkosarek 15:d117591084ff 41 #ifdef LIS3DH
nkosarek 15:d117591084ff 42 #define ADDR_ONE 0x30
nkosarek 15:d117591084ff 43 #define ADDR_TWO 0x32
nkosarek 15:d117591084ff 44 #define AXIS_X 0x00
nkosarek 15:d117591084ff 45 #define AXIS_Y 0x01
nkosarek 15:d117591084ff 46 #define AXIS_Z 0x02
nkosarek 15:d117591084ff 47 #define REG_OUT_X_L 0x28
nkosarek 15:d117591084ff 48 #define REG_CTRL1 0x20
nkosarek 15:d117591084ff 49 #define REG_CTRL4 0x23
nkosarek 15:d117591084ff 50 #define REG_WHOAMI 0x0F
nkosarek 15:d117591084ff 51 #define RANGE_2G 0x00
nkosarek 15:d117591084ff 52 #define DEVICE_ID 0x33
nkosarek 15:d117591084ff 53
nkosarek 15:d117591084ff 54
nkosarek 15:d117591084ff 55 #define DATARATE_400HZ 0b0111 // 400Hz
nkosarek 15:d117591084ff 56 #define DATARATE_200HZ 0b0110 // 200Hz
nkosarek 15:d117591084ff 57 #define DATARATE_100HZ 0b0101 // 100Hz
nkosarek 15:d117591084ff 58 #define DATARATE_50HZ 0b0100 // 50Hz
nkosarek 15:d117591084ff 59 #define DATARATE_25HZ 0b0011 // 25Hz
nkosarek 15:d117591084ff 60 #define DATARATE_10HZ 0b0010 // 10Hz
nkosarek 15:d117591084ff 61 #define DATARATE_1HZ 0b0001 // 1Hz
nkosarek 15:d117591084ff 62 #define DATARATE_POWERDOWN 0 // Power down
nkosarek 15:d117591084ff 63 #define DATARATE_LOWPOWER_1K6HZ 0b1000 // Low power mode (1.6KHz)
nkosarek 15:d117591084ff 64 #define DATARATE_LOWPOWER_5KHZ 0b1001 // Low power mode (5KHz) / Normal power mode (1.25KHz)
nkosarek 15:d117591084ff 65 #endif
nkosarek 15:d117591084ff 66
nkosarek 15:d117591084ff 67
nkosarek 15:d117591084ff 68 #ifdef LIS331HH
nkosarek 15:d117591084ff 69 #define ADDR_ONE 0x30
nkosarek 15:d117591084ff 70 #define ADDR_TWO 0x32
nkosarek 15:d117591084ff 71 #define AXIS_X 0x00
nkosarek 15:d117591084ff 72 #define AXIS_Y 0x01
nkosarek 15:d117591084ff 73 #define AXIS_Z 0x02
nkosarek 15:d117591084ff 74 #define REG_OUT_X_L 0x28
nkosarek 15:d117591084ff 75 #define REG_CTRL1 0x20
nkosarek 15:d117591084ff 76 #define REG_CTRL4 0x23
nkosarek 15:d117591084ff 77 #define REG_WHOAMI 0x0F
nkosarek 15:d117591084ff 78 #define RANGE_2G 0x00
nkosarek 15:d117591084ff 79 #define DEVICE_ID 0x33
nkosarek 15:d117591084ff 80
nkosarek 15:d117591084ff 81
nkosarek 15:d117591084ff 82 #define DATARATE_1KHZ 0b11 // 1000Hz
nkosarek 15:d117591084ff 83 #define DATARATE_400HZ 0b10 // 400Hz
nkosarek 15:d117591084ff 84 #define DATARATE_100HZ 0b01 // 100Hz
nkosarek 15:d117591084ff 85 #define DATARATE_50HZ 0b00 // 50Hz
nkosarek 15:d117591084ff 86 #define DATARATE_POWERDOWN 0 // Power down
nkosarek 15:d117591084ff 87 #define DATARATE_NORMAL_MODE 0b001
znew711 16:799397f0d3a8 88 #define DATARATE_LOWPOWER_05HZ 0b010
nkosarek 15:d117591084ff 89 #define DATARATE_LOWPOWER_1HZ 0b011
nkosarek 15:d117591084ff 90 #define DATARATE_LOWPOWER_2HZ 0b100
nkosarek 15:d117591084ff 91 #define DATARATE_LOWPOWER_5HZ 0b101
nkosarek 15:d117591084ff 92 #define DATARATE_LOWPOWER_10HZ 0b110
nkosarek 15:d117591084ff 93 #endif
nkosarek 15:d117591084ff 94
nkosarek 15:d117591084ff 95 #define PACKET_SIZE 20
nkosarek 15:d117591084ff 96 #define QUEUE_SIZE 20
znew711 16:799397f0d3a8 97
znew711 16:799397f0d3a8 98 #define Z_THRESHOLD 5000
nkosarek 15:d117591084ff 99
znew711 11:fd1cf9dbf3a4 100 const static char DEVICE_NAME[] = "LUMBERJACK_NANO";
rgrover1 0:28f095301cb2 101 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
nkosarek 15:d117591084ff 102
nkosarek 15:d117591084ff 103 struct packetQueue
nkosarek 15:d117591084ff 104 {
nkosarek 15:d117591084ff 105 uint16_t size;
nkosarek 15:d117591084ff 106 uint16_t nextPacketToSend;
nkosarek 15:d117591084ff 107 uint16_t nextSampleToSave;
nkosarek 15:d117591084ff 108 uint16_t liveSamples;
nkosarek 15:d117591084ff 109 uint8_t packets[QUEUE_SIZE][PACKET_SIZE];
nkosarek 15:d117591084ff 110 };
nkosarek 15:d117591084ff 111
nkosarek 15:d117591084ff 112 packetQueue pq;
nkosarek 15:d117591084ff 113
nkosarek 15:d117591084ff 114 void addToQueue(uint8_t* packet) {
nkosarek 15:d117591084ff 115 for (int i = 0; i < PACKET_SIZE; i++) {
nkosarek 15:d117591084ff 116 pq.packets[pq.nextSampleToSave][i] = packet[i];
nkosarek 15:d117591084ff 117 }
nkosarek 15:d117591084ff 118 if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
nkosarek 15:d117591084ff 119 pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 120 pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 121 } else {
nkosarek 15:d117591084ff 122 pq.liveSamples += 1;
nkosarek 15:d117591084ff 123 pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 124 }
nkosarek 15:d117591084ff 125 return;
nkosarek 15:d117591084ff 126 }
nkosarek 15:d117591084ff 127
nkosarek 15:d117591084ff 128 uint8_t* removeFromQueue() {
nkosarek 15:d117591084ff 129 if (pq.nextSampleToSave != pq.nextPacketToSend && pq.liveSamples > 0) {
nkosarek 15:d117591084ff 130 pq.liveSamples -= 1;
nkosarek 15:d117591084ff 131 uint8_t* old = pq.packets[pq.nextPacketToSend];
nkosarek 15:d117591084ff 132 pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 133 return old;
nkosarek 15:d117591084ff 134 } else {
nkosarek 15:d117591084ff 135 return NULL;
nkosarek 15:d117591084ff 136 }
nkosarek 15:d117591084ff 137 }
nkosarek 15:d117591084ff 138
nkosarek 15:d117591084ff 139
nkosarek 15:d117591084ff 140 //Serial pc(USBTX, USBRX);
nkosarek 15:d117591084ff 141 TwoWire Wire = TwoWire(NRF_TWI0);
nkosarek 15:d117591084ff 142
andresag 10:7943b5c1117a 143 static ButtonService *buttonServicePtr;
znew711 11:fd1cf9dbf3a4 144 bool isThereAConnection = false;
znew711 16:799397f0d3a8 145 bool weAreSending = false;
nkosarek 15:d117591084ff 146
znew711 11:fd1cf9dbf3a4 147 void sleep(unsigned int mseconds)
rgrover1 0:28f095301cb2 148 {
znew711 11:fd1cf9dbf3a4 149 clock_t goal = mseconds + clock();
znew711 11:fd1cf9dbf3a4 150 while (goal > clock());
rgrover1 0:28f095301cb2 151 }
nkosarek 15:d117591084ff 152
rgrover1 8:a7ba7aaba460 153 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
rgrover1 0:28f095301cb2 154 {
andresag 10:7943b5c1117a 155 BLE::Instance().gap().startAdvertising();
znew711 11:fd1cf9dbf3a4 156 isThereAConnection = false;
rgrover1 0:28f095301cb2 157 }
nkosarek 15:d117591084ff 158
znew711 11:fd1cf9dbf3a4 159 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
rgrover1 0:28f095301cb2 160 {
nkosarek 15:d117591084ff 161 //pc.printf("Connection recieved!\r\n");
znew711 11:fd1cf9dbf3a4 162 isThereAConnection = true;
rgrover1 0:28f095301cb2 163 }
znew711 16:799397f0d3a8 164
znew711 16:799397f0d3a8 165 void startTransmission() {
znew711 16:799397f0d3a8 166 uint8_t* nextPacket = removeFromQueue();
znew711 16:799397f0d3a8 167 buttonServicePtr->updateButtonState(nextPacket);
znew711 16:799397f0d3a8 168 }
znew711 16:799397f0d3a8 169
znew711 16:799397f0d3a8 170 void dataSentCallback(unsigned count) {
znew711 16:799397f0d3a8 171 //pc.printf("dataSent!!\r\n");
znew711 16:799397f0d3a8 172 uint8_t* nextPacket = removeFromQueue();
znew711 16:799397f0d3a8 173 buttonServicePtr->updateButtonState(nextPacket);
znew711 16:799397f0d3a8 174 }
nkosarek 15:d117591084ff 175
andresag 10:7943b5c1117a 176 /**
andresag 10:7943b5c1117a 177 * This function is called when the ble initialization process has failled
andresag 10:7943b5c1117a 178 */
andresag 10:7943b5c1117a 179 void onBleInitError(BLE &ble, ble_error_t error)
andresag 10:7943b5c1117a 180 {
andresag 10:7943b5c1117a 181 /* Initialization error handling should go here */
andresag 10:7943b5c1117a 182 }
nkosarek 15:d117591084ff 183
andresag 10:7943b5c1117a 184 /**
andresag 10:7943b5c1117a 185 * Callback triggered when the ble initialization process has finished
andresag 10:7943b5c1117a 186 */
andresag 10:7943b5c1117a 187 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
andresag 10:7943b5c1117a 188 {
andresag 10:7943b5c1117a 189 BLE& ble = params->ble;
andresag 10:7943b5c1117a 190 ble_error_t error = params->error;
nkosarek 15:d117591084ff 191
andresag 10:7943b5c1117a 192 if (error != BLE_ERROR_NONE) {
andresag 10:7943b5c1117a 193 /* In case of error, forward the error handling to onBleInitError */
andresag 10:7943b5c1117a 194 onBleInitError(ble, error);
andresag 10:7943b5c1117a 195 return;
andresag 10:7943b5c1117a 196 }
nkosarek 15:d117591084ff 197
andresag 10:7943b5c1117a 198 /* Ensure that it is the default instance of BLE */
andresag 10:7943b5c1117a 199 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
andresag 10:7943b5c1117a 200 return;
andresag 10:7943b5c1117a 201 }
nkosarek 15:d117591084ff 202
andresag 10:7943b5c1117a 203 ble.gap().onDisconnection(disconnectionCallback);
znew711 11:fd1cf9dbf3a4 204 ble.gap().onConnection(connectionCallback);
znew711 16:799397f0d3a8 205 ble.onDataSent(dataSentCallback);
nkosarek 15:d117591084ff 206
andresag 10:7943b5c1117a 207 /* Setup primary service */
nkosarek 15:d117591084ff 208 uint8_t initial_value[20] = {0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 209 0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 210 0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 211 0, 0, 0, 0, 0};
nkosarek 15:d117591084ff 212 buttonServicePtr = new ButtonService(ble, initial_value);
nkosarek 15:d117591084ff 213
andresag 10:7943b5c1117a 214 /* setup advertising */
andresag 10:7943b5c1117a 215 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
andresag 10:7943b5c1117a 216 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
andresag 10:7943b5c1117a 217 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
andresag 10:7943b5c1117a 218 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
andresag 10:7943b5c1117a 219 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
nkosarek 15:d117591084ff 220 //pc.printf("start advertising now \r\n");
andresag 10:7943b5c1117a 221 ble.gap().startAdvertising();
andresag 10:7943b5c1117a 222 }
nkosarek 15:d117591084ff 223
nkosarek 15:d117591084ff 224 void AT24C512_WriteBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
nkosarek 15:d117591084ff 225 {
nkosarek 15:d117591084ff 226 Wire.beginTransmission(i2cAddr);
nkosarek 15:d117591084ff 227 int err = Wire.write( (uint8_t)addr );
nkosarek 15:d117591084ff 228 Wire.write(pbuf, length);
nkosarek 15:d117591084ff 229 if (err != 0) {
nkosarek 15:d117591084ff 230 //pc.printf("error on write write! %d\r\n", err);
nkosarek 15:d117591084ff 231 }
nkosarek 15:d117591084ff 232 uint8_t err8 = Wire.endTransmission();
nkosarek 15:d117591084ff 233 if (err8 != 0) {
nkosarek 15:d117591084ff 234 //pc.printf("error on write end transmission! %d\r\n", err8);
nkosarek 15:d117591084ff 235 }
nkosarek 15:d117591084ff 236 }
nkosarek 15:d117591084ff 237
nkosarek 15:d117591084ff 238 void AT24C512_ReadBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
nkosarek 15:d117591084ff 239 {
nkosarek 15:d117591084ff 240 Wire.beginTransmission(i2cAddr);
nkosarek 15:d117591084ff 241 int err= Wire.write( (uint8_t)addr );
nkosarek 15:d117591084ff 242 if (err != 0) {
nkosarek 15:d117591084ff 243 //pc.printf("error on read write! %d\r\n", err);
nkosarek 15:d117591084ff 244 }
nkosarek 15:d117591084ff 245 uint8_t err8 = Wire.endTransmission();
nkosarek 15:d117591084ff 246 if (err8 != 0) {
nkosarek 15:d117591084ff 247 //pc.printf("error on read end transmission! %d\r\n", err8);
nkosarek 15:d117591084ff 248 }
nkosarek 15:d117591084ff 249
nkosarek 15:d117591084ff 250 err8 = Wire.requestFrom(i2cAddr+1, length);
nkosarek 15:d117591084ff 251 if (err != 0) {
nkosarek 15:d117591084ff 252 //pc.printf("error on read request from! %d\r\n", err8);
nkosarek 15:d117591084ff 253 }
nkosarek 15:d117591084ff 254 while( Wire.available() > 0 )
nkosarek 15:d117591084ff 255 {
nkosarek 15:d117591084ff 256 *pbuf = Wire.read();
nkosarek 15:d117591084ff 257 pbuf++;
nkosarek 15:d117591084ff 258 }
nkosarek 15:d117591084ff 259 }
nkosarek 15:d117591084ff 260
nkosarek 15:d117591084ff 261 //Set the bit at index 'bit' to 'value' on 'input' and return
nkosarek 15:d117591084ff 262 uint8_t setBit(uint8_t input, uint8_t bit, uint8_t value) {
nkosarek 15:d117591084ff 263 uint8_t mask = 1 << bit;
nkosarek 15:d117591084ff 264 input &= ~mask;
nkosarek 15:d117591084ff 265 if (value == 1) {
nkosarek 15:d117591084ff 266 input |= mask;
nkosarek 15:d117591084ff 267 }
nkosarek 15:d117591084ff 268 return input;
nkosarek 15:d117591084ff 269 }
nkosarek 15:d117591084ff 270
znew711 16:799397f0d3a8 271 int16_t getAxis(uint16_t axis, uint16_t i2cAddr)
nkosarek 15:d117591084ff 272 {
nkosarek 15:d117591084ff 273 uint8_t base = REG_OUT_X_L + (2 * axis);
nkosarek 15:d117591084ff 274 uint8_t* low = new uint8_t[1];
nkosarek 15:d117591084ff 275 uint8_t* high = new uint8_t[1];
nkosarek 15:d117591084ff 276 AT24C512_ReadBytes(base, low, 1, i2cAddr);
nkosarek 15:d117591084ff 277 AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
znew711 16:799397f0d3a8 278 int16_t res = low[0] | (high[0] << 8);
nkosarek 15:d117591084ff 279 delete[] low;
nkosarek 15:d117591084ff 280 delete[] high;
nkosarek 15:d117591084ff 281 return res;
nkosarek 15:d117591084ff 282 }
nkosarek 15:d117591084ff 283
nkosarek 15:d117591084ff 284 void setRange(uint8_t range, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 285 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 286 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 287 val[0] &= ~(0b110000); //zero out lowest 2 bits of top 4 bits
nkosarek 15:d117591084ff 288 val[0] |= (range << 4); // write in our new range
nkosarek 15:d117591084ff 289 //pc.printf("REG_CTRL4 after setRange: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 290 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 291 delete[] val;
nkosarek 15:d117591084ff 292 }
nkosarek 15:d117591084ff 293
nkosarek 15:d117591084ff 294 //Set whether we want to use high resolution or not
nkosarek 15:d117591084ff 295 void setHighResolution(bool highRes, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 296 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 297 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 298 uint8_t final;
nkosarek 15:d117591084ff 299 if (highRes) {
nkosarek 15:d117591084ff 300 final = setBit(val[0], 3, 1);
nkosarek 15:d117591084ff 301 } else {
nkosarek 15:d117591084ff 302 final = setBit(val[0], 3, 0);
nkosarek 15:d117591084ff 303 }
nkosarek 15:d117591084ff 304 val[0] = final;
nkosarek 15:d117591084ff 305 //pc.printf("REG_CTRL4 after setHiRes: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 306 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 307 delete[] val;
nkosarek 15:d117591084ff 308 }
nkosarek 15:d117591084ff 309
nkosarek 15:d117591084ff 310 void setAxisStatus(uint8_t axis, bool enable, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 311 uint8_t* current = new uint8_t[1];
nkosarek 15:d117591084ff 312 AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 313 uint8_t final;
nkosarek 15:d117591084ff 314 if (enable == 1) {
nkosarek 15:d117591084ff 315 final = setBit(current[0], axis, 1);
nkosarek 15:d117591084ff 316 } else {
nkosarek 15:d117591084ff 317 final = setBit(current[0], axis, 0);
nkosarek 15:d117591084ff 318 }
nkosarek 15:d117591084ff 319 current[0] = final;
nkosarek 15:d117591084ff 320 AT24C512_WriteBytes(REG_CTRL1, current, 1, i2cAddr);
nkosarek 15:d117591084ff 321
nkosarek 15:d117591084ff 322 AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);
nkosarek 15:d117591084ff 323 //pc.printf("REG_CTRL1 after setAxisStatus: 0x%x\r\n", *current);
nkosarek 15:d117591084ff 324 delete[] current;
nkosarek 15:d117591084ff 325 }
nkosarek 15:d117591084ff 326
nkosarek 15:d117591084ff 327 void setDataRate(uint8_t dataRate, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 328 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 329 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 330 //pc.printf("REG_CTRL1 before data rate set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 331 val[0] &= 0b11100111; //d
nkosarek 15:d117591084ff 332 val[0] |= (dataRate << 3);
nkosarek 15:d117591084ff 333 AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 334
nkosarek 15:d117591084ff 335 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 336 //pc.printf("REG_CTRL1 after data rate set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 337 delete[] val;
nkosarek 15:d117591084ff 338 }
nkosarek 15:d117591084ff 339
nkosarek 15:d117591084ff 340 void setPowerMode(uint8_t powerMode, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 341 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 342 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 343 val[0] &= 0b11111;
nkosarek 15:d117591084ff 344 val[0] |= (powerMode << 5);
nkosarek 15:d117591084ff 345 //pc.printf("writing this to REG_CTRL1: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 346 AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 347
nkosarek 15:d117591084ff 348 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 349 //pc.printf("REG_CTRL1 after power mode set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 350 delete[] val;
nkosarek 15:d117591084ff 351 }
nkosarek 15:d117591084ff 352
nkosarek 15:d117591084ff 353 void setBDU(bool bdu, uint16_t i2cAddr)
nkosarek 15:d117591084ff 354 {
nkosarek 15:d117591084ff 355 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 356 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 357 //pc.printf("REG_CTRL4: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 358 uint8_t final;
nkosarek 15:d117591084ff 359 if (bdu) {
nkosarek 15:d117591084ff 360 final = setBit(val[0], 7, 1);
nkosarek 15:d117591084ff 361 } else {
nkosarek 15:d117591084ff 362 final = setBit(val[0], 7, 0);
nkosarek 15:d117591084ff 363 }
nkosarek 15:d117591084ff 364 val[0] = final;
nkosarek 15:d117591084ff 365 //pc.printf("REG_CTRL4 after setBDU: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 366 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 367 delete[] val;
nkosarek 15:d117591084ff 368 }
nkosarek 15:d117591084ff 369
znew711 16:799397f0d3a8 370 int16_t getX(uint16_t i2cAddr)
nkosarek 15:d117591084ff 371 {
nkosarek 15:d117591084ff 372 return getAxis(AXIS_X, i2cAddr);
nkosarek 15:d117591084ff 373 }
nkosarek 15:d117591084ff 374
znew711 16:799397f0d3a8 375 int16_t getY(uint16_t i2cAddr)
nkosarek 15:d117591084ff 376 {
nkosarek 15:d117591084ff 377 return getAxis(AXIS_Y, i2cAddr);
nkosarek 15:d117591084ff 378 }
nkosarek 15:d117591084ff 379
znew711 16:799397f0d3a8 380 int16_t getZ(uint16_t i2cAddr)
nkosarek 15:d117591084ff 381 {
nkosarek 15:d117591084ff 382 return getAxis(AXIS_Z, i2cAddr);
nkosarek 15:d117591084ff 383 }
nkosarek 15:d117591084ff 384
rgrover1 0:28f095301cb2 385 int main(void)
rgrover1 0:28f095301cb2 386 {
nkosarek 15:d117591084ff 387 //pc.baud(9600);
nkosarek 15:d117591084ff 388 wait(5);
nkosarek 15:d117591084ff 389 //Wire.begin();
nkosarek 15:d117591084ff 390 Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
nkosarek 15:d117591084ff 391
nkosarek 15:d117591084ff 392 //pc.printf("\r\n\r\n\r\nStarting...\r\n");
nkosarek 15:d117591084ff 393
nkosarek 15:d117591084ff 394 wait(5);
nkosarek 15:d117591084ff 395
nkosarek 15:d117591084ff 396 setAxisStatus(AXIS_X, true, ADDR_ONE);
nkosarek 15:d117591084ff 397 setAxisStatus(AXIS_Y, true, ADDR_ONE);
nkosarek 15:d117591084ff 398 setAxisStatus(AXIS_Z, true, ADDR_ONE);
nkosarek 15:d117591084ff 399 setDataRate(DATARATE_400HZ, ADDR_ONE);
nkosarek 15:d117591084ff 400 setPowerMode(DATARATE_NORMAL_MODE, ADDR_ONE);
nkosarek 15:d117591084ff 401 //setHighResolution(true, ADDR_ONE);
nkosarek 15:d117591084ff 402 setBDU(true, ADDR_ONE);
nkosarek 15:d117591084ff 403 //setRange(RANGE_2G, ADDR_ONE);
nkosarek 15:d117591084ff 404
nkosarek 15:d117591084ff 405
nkosarek 15:d117591084ff 406 setAxisStatus(AXIS_X, true, ADDR_TWO);
nkosarek 15:d117591084ff 407 setAxisStatus(AXIS_Y, true, ADDR_TWO);
nkosarek 15:d117591084ff 408 setAxisStatus(AXIS_Z, true, ADDR_TWO);
nkosarek 15:d117591084ff 409 setDataRate(DATARATE_400HZ, ADDR_TWO);
nkosarek 15:d117591084ff 410 setPowerMode(DATARATE_NORMAL_MODE, ADDR_TWO);
nkosarek 15:d117591084ff 411 //setHighResolution(true, ADDR_TWO);
nkosarek 15:d117591084ff 412 setBDU(true, ADDR_TWO);
nkosarek 15:d117591084ff 413 //setRange(RANGE_2G, ADDR_TWO);
nkosarek 15:d117591084ff 414
nkosarek 15:d117591084ff 415 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 416 *val = 0x80;
nkosarek 15:d117591084ff 417 AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_ONE);
nkosarek 15:d117591084ff 418 AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_TWO);
nkosarek 15:d117591084ff 419 AT24C512_ReadBytes(REG_CTRL4, val, 1, ADDR_ONE);
nkosarek 15:d117591084ff 420 //pc.printf("REG_CTRL4, should be 0x80: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 421
nkosarek 15:d117591084ff 422 uint8_t* whoami = new uint8_t[1];
nkosarek 15:d117591084ff 423 AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_ONE);
nkosarek 15:d117591084ff 424 //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
nkosarek 15:d117591084ff 425 AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_TWO);
nkosarek 15:d117591084ff 426 //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
nkosarek 15:d117591084ff 427 AT24C512_ReadBytes(0x1F, whoami, 1, ADDR_ONE);
nkosarek 15:d117591084ff 428
nkosarek 15:d117591084ff 429
andresag 10:7943b5c1117a 430 BLE &ble = BLE::Instance();
andresag 10:7943b5c1117a 431 ble.init(bleInitComplete);
nkosarek 15:d117591084ff 432
nkosarek 15:d117591084ff 433 //pc.printf("entering spin loop\r\n");
andresag 10:7943b5c1117a 434 /* SpinWait for initialization to complete. This is necessary because the
andresag 10:7943b5c1117a 435 * BLE object is used in the main loop below. */
andresag 10:7943b5c1117a 436 while (ble.hasInitialized() == false) { /* spin loop */ }
nkosarek 15:d117591084ff 437 //pc.printf("leaving spin loop\r\n");
nkosarek 15:d117591084ff 438
nkosarek 15:d117591084ff 439 pq.size = QUEUE_SIZE;
nkosarek 15:d117591084ff 440 pq.nextPacketToSend = 0;
nkosarek 15:d117591084ff 441 pq.nextSampleToSave = 0;
nkosarek 15:d117591084ff 442 pq.liveSamples = 0;
znew711 16:799397f0d3a8 443
znew711 16:799397f0d3a8 444 uint8_t lastThreePackets[3][20];
znew711 16:799397f0d3a8 445 uint8_t lastThreeIndex = 0;
znew711 16:799397f0d3a8 446 uint8_t underThresholdCount = 0;
znew711 16:799397f0d3a8 447 bool inAKeyStroke = false;
nkosarek 15:d117591084ff 448
nkosarek 15:d117591084ff 449 while(1)
nkosarek 15:d117591084ff 450 {
nkosarek 15:d117591084ff 451 //pc.printf("Read data from AT24C512\r\n");
znew711 16:799397f0d3a8 452 int16_t x1 = getX(ADDR_ONE);
znew711 16:799397f0d3a8 453 int16_t y1 = getY(ADDR_ONE);
znew711 16:799397f0d3a8 454 int16_t z1 = getZ(ADDR_ONE);
nkosarek 15:d117591084ff 455
znew711 16:799397f0d3a8 456 int16_t x2 = getX(ADDR_TWO);
znew711 16:799397f0d3a8 457 int16_t y2 = getY(ADDR_TWO);
znew711 16:799397f0d3a8 458 int16_t z2 = getZ(ADDR_TWO);
nkosarek 15:d117591084ff 459
nkosarek 15:d117591084ff 460 //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
nkosarek 15:d117591084ff 461 //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
znew711 16:799397f0d3a8 462 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,
znew711 16:799397f0d3a8 463 (uint8_t)(x2 >> 8), (uint8_t)x2, (uint8_t)(y2 >> 8), (uint8_t)y2, (uint8_t)(z2 >> 8), (uint8_t)z2,
znew711 16:799397f0d3a8 464 0, 0, 0, 0, 0, 0, 0, 0};
znew711 16:799397f0d3a8 465 //TODO: handle negative accels
znew711 16:799397f0d3a8 466 if (z1 > Z_THRESHOLD || z2 > Z_THRESHOLD) {
znew711 16:799397f0d3a8 467 underThresholdCount = 0;
znew711 16:799397f0d3a8 468 if (!inAKeyStroke) {
znew711 16:799397f0d3a8 469 //start transmitting
znew711 16:799397f0d3a8 470 inAKeyStroke = true;
znew711 16:799397f0d3a8 471 for (int i = 0; i < 3; i++) {
znew711 16:799397f0d3a8 472 addToQueue(lastThreePackets[(lastThreeIndex - 1 - i) % 3]);
znew711 16:799397f0d3a8 473 }
znew711 16:799397f0d3a8 474 addToQueue(values);
znew711 16:799397f0d3a8 475 startTransmission();
znew711 16:799397f0d3a8 476 } else {
znew711 16:799397f0d3a8 477 addToQueue(values);
znew711 16:799397f0d3a8 478 }
znew711 16:799397f0d3a8 479 } else if (underThresholdCount < 3 && inAKeyStroke) {
znew711 16:799397f0d3a8 480 underThresholdCount++;
znew711 16:799397f0d3a8 481 addToQueue(values);
znew711 16:799397f0d3a8 482 } else {
znew711 16:799397f0d3a8 483 for (int i = 0; i < 20; i++) {
znew711 16:799397f0d3a8 484 lastThreePackets[lastThreeIndex][i] = values[i];
znew711 16:799397f0d3a8 485 }
znew711 16:799397f0d3a8 486 lastThreeIndex = (lastThreeIndex + 1) % 3;
znew711 16:799397f0d3a8 487 inAKeyStroke = false;
rgrover1 9:0f6951db24f1 488 }
nkosarek 15:d117591084ff 489
znew711 16:799397f0d3a8 490 wait_ms(50);
rgrover1 0:28f095301cb2 491 }
nkosarek 15:d117591084ff 492
nkosarek 15:d117591084ff 493 }