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@18:6645cb0f517f, 2017-05-04 (annotated)

Committer:
znew711
Date:
Thu May 04 03:27:26 2017 +0000
Revision:
18:6645cb0f517f
Parent:
17:09ceae7cb00e 
better noise tolerance

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
znew711 17:09ceae7cb00e 96 #define QUEUE_SIZE 100
znew711 16:799397f0d3a8 97
znew711 16:799397f0d3a8 98 #define Z_THRESHOLD 5000
znew711 18:6645cb0f517f 99 #define ACCEL_ONE_RESTING 5100
znew711 18:6645cb0f517f 100 #define ACCEL_TWO_RESTING 5500
znew711 18:6645cb0f517f 101 #define ACCEL_ONE_EPSILON 600
znew711 18:6645cb0f517f 102 #define ACCEL_TWO_EPSILON 600
nkosarek 15:d117591084ff 103
znew711 11:fd1cf9dbf3a4 104 const static char DEVICE_NAME[] = "LUMBERJACK_NANO";
rgrover1 0:28f095301cb2 105 static const uint16_t uuid16_list[] = {ButtonService::BUTTON_SERVICE_UUID};
nkosarek 15:d117591084ff 106
nkosarek 15:d117591084ff 107 struct packetQueue
nkosarek 15:d117591084ff 108 {
nkosarek 15:d117591084ff 109 uint16_t size;
nkosarek 15:d117591084ff 110 uint16_t nextPacketToSend;
nkosarek 15:d117591084ff 111 uint16_t nextSampleToSave;
nkosarek 15:d117591084ff 112 uint16_t liveSamples;
nkosarek 15:d117591084ff 113 uint8_t packets[QUEUE_SIZE][PACKET_SIZE];
nkosarek 15:d117591084ff 114 };
nkosarek 15:d117591084ff 115
nkosarek 15:d117591084ff 116 packetQueue pq;
nkosarek 15:d117591084ff 117
nkosarek 15:d117591084ff 118 void addToQueue(uint8_t* packet) {
znew711 17:09ceae7cb00e 119 if (pq.nextPacketToSend == pq.nextSampleToSave && pq.liveSamples > 0) {
znew711 17:09ceae7cb00e 120 pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
znew711 17:09ceae7cb00e 121 for (int i = 0; i < PACKET_SIZE; i++) {
znew711 17:09ceae7cb00e 122 pq.packets[pq.nextSampleToSave][i] = packet[i];
znew711 17:09ceae7cb00e 123 }
znew711 17:09ceae7cb00e 124 } else {
znew711 17:09ceae7cb00e 125 for (int i = 0; i < PACKET_SIZE; i++) {
znew711 17:09ceae7cb00e 126 pq.packets[pq.nextSampleToSave][i] = packet[i];
znew711 17:09ceae7cb00e 127 }
znew711 17:09ceae7cb00e 128 pq.liveSamples += 1;
nkosarek 15:d117591084ff 129 }
znew711 17:09ceae7cb00e 130 pq.nextSampleToSave = (pq.nextSampleToSave + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 131 return;
nkosarek 15:d117591084ff 132 }
nkosarek 15:d117591084ff 133
nkosarek 15:d117591084ff 134 uint8_t* removeFromQueue() {
znew711 17:09ceae7cb00e 135 if (pq.liveSamples > 0) {
nkosarek 15:d117591084ff 136 pq.liveSamples -= 1;
nkosarek 15:d117591084ff 137 uint8_t* old = pq.packets[pq.nextPacketToSend];
nkosarek 15:d117591084ff 138 pq.nextPacketToSend = (pq.nextPacketToSend + 1) % QUEUE_SIZE;
nkosarek 15:d117591084ff 139 return old;
nkosarek 15:d117591084ff 140 } else {
nkosarek 15:d117591084ff 141 return NULL;
nkosarek 15:d117591084ff 142 }
nkosarek 15:d117591084ff 143 }
nkosarek 15:d117591084ff 144
nkosarek 15:d117591084ff 145
nkosarek 15:d117591084ff 146 //Serial pc(USBTX, USBRX);
nkosarek 15:d117591084ff 147 TwoWire Wire = TwoWire(NRF_TWI0);
nkosarek 15:d117591084ff 148
andresag 10:7943b5c1117a 149 static ButtonService *buttonServicePtr;
znew711 11:fd1cf9dbf3a4 150 bool isThereAConnection = false;
znew711 16:799397f0d3a8 151 bool weAreSending = false;
nkosarek 15:d117591084ff 152
znew711 11:fd1cf9dbf3a4 153 void sleep(unsigned int mseconds)
rgrover1 0:28f095301cb2 154 {
znew711 11:fd1cf9dbf3a4 155 clock_t goal = mseconds + clock();
znew711 11:fd1cf9dbf3a4 156 while (goal > clock());
rgrover1 0:28f095301cb2 157 }
nkosarek 15:d117591084ff 158
rgrover1 8:a7ba7aaba460 159 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
rgrover1 0:28f095301cb2 160 {
andresag 10:7943b5c1117a 161 BLE::Instance().gap().startAdvertising();
znew711 11:fd1cf9dbf3a4 162 isThereAConnection = false;
rgrover1 0:28f095301cb2 163 }
nkosarek 15:d117591084ff 164
znew711 11:fd1cf9dbf3a4 165 void connectionCallback(const Gap::ConnectionCallbackParams_t *params)
rgrover1 0:28f095301cb2 166 {
nkosarek 15:d117591084ff 167 //pc.printf("Connection recieved!\r\n");
znew711 11:fd1cf9dbf3a4 168 isThereAConnection = true;
rgrover1 0:28f095301cb2 169 }
znew711 16:799397f0d3a8 170
znew711 16:799397f0d3a8 171 void startTransmission() {
znew711 16:799397f0d3a8 172 uint8_t* nextPacket = removeFromQueue();
znew711 17:09ceae7cb00e 173 if (nextPacket != NULL) {
znew711 17:09ceae7cb00e 174 buttonServicePtr->updateButtonState(nextPacket);
znew711 17:09ceae7cb00e 175 }
znew711 16:799397f0d3a8 176 }
znew711 16:799397f0d3a8 177
znew711 16:799397f0d3a8 178 void dataSentCallback(unsigned count) {
znew711 16:799397f0d3a8 179 //pc.printf("dataSent!!\r\n");
znew711 16:799397f0d3a8 180 uint8_t* nextPacket = removeFromQueue();
znew711 17:09ceae7cb00e 181 if (nextPacket != NULL) {
znew711 17:09ceae7cb00e 182 buttonServicePtr->updateButtonState(nextPacket);
znew711 17:09ceae7cb00e 183 }
znew711 16:799397f0d3a8 184 }
nkosarek 15:d117591084ff 185
andresag 10:7943b5c1117a 186 /**
andresag 10:7943b5c1117a 187 * This function is called when the ble initialization process has failled
andresag 10:7943b5c1117a 188 */
andresag 10:7943b5c1117a 189 void onBleInitError(BLE &ble, ble_error_t error)
andresag 10:7943b5c1117a 190 {
andresag 10:7943b5c1117a 191 /* Initialization error handling should go here */
andresag 10:7943b5c1117a 192 }
nkosarek 15:d117591084ff 193
andresag 10:7943b5c1117a 194 /**
andresag 10:7943b5c1117a 195 * Callback triggered when the ble initialization process has finished
andresag 10:7943b5c1117a 196 */
andresag 10:7943b5c1117a 197 void bleInitComplete(BLE::InitializationCompleteCallbackContext *params)
andresag 10:7943b5c1117a 198 {
andresag 10:7943b5c1117a 199 BLE& ble = params->ble;
andresag 10:7943b5c1117a 200 ble_error_t error = params->error;
nkosarek 15:d117591084ff 201
andresag 10:7943b5c1117a 202 if (error != BLE_ERROR_NONE) {
andresag 10:7943b5c1117a 203 /* In case of error, forward the error handling to onBleInitError */
andresag 10:7943b5c1117a 204 onBleInitError(ble, error);
andresag 10:7943b5c1117a 205 return;
andresag 10:7943b5c1117a 206 }
nkosarek 15:d117591084ff 207
andresag 10:7943b5c1117a 208 /* Ensure that it is the default instance of BLE */
andresag 10:7943b5c1117a 209 if(ble.getInstanceID() != BLE::DEFAULT_INSTANCE) {
andresag 10:7943b5c1117a 210 return;
andresag 10:7943b5c1117a 211 }
nkosarek 15:d117591084ff 212
andresag 10:7943b5c1117a 213 ble.gap().onDisconnection(disconnectionCallback);
znew711 11:fd1cf9dbf3a4 214 ble.gap().onConnection(connectionCallback);
znew711 16:799397f0d3a8 215 ble.onDataSent(dataSentCallback);
nkosarek 15:d117591084ff 216
andresag 10:7943b5c1117a 217 /* Setup primary service */
nkosarek 15:d117591084ff 218 uint8_t initial_value[20] = {0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 219 0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 220 0, 0, 0, 0, 0,
nkosarek 15:d117591084ff 221 0, 0, 0, 0, 0};
nkosarek 15:d117591084ff 222 buttonServicePtr = new ButtonService(ble, initial_value);
nkosarek 15:d117591084ff 223
andresag 10:7943b5c1117a 224 /* setup advertising */
andresag 10:7943b5c1117a 225 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
andresag 10:7943b5c1117a 226 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
andresag 10:7943b5c1117a 227 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
andresag 10:7943b5c1117a 228 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
andresag 10:7943b5c1117a 229 ble.gap().setAdvertisingInterval(1000); /* 1000ms. */
nkosarek 15:d117591084ff 230 //pc.printf("start advertising now \r\n");
andresag 10:7943b5c1117a 231 ble.gap().startAdvertising();
andresag 10:7943b5c1117a 232 }
nkosarek 15:d117591084ff 233
nkosarek 15:d117591084ff 234 void AT24C512_WriteBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
nkosarek 15:d117591084ff 235 {
nkosarek 15:d117591084ff 236 Wire.beginTransmission(i2cAddr);
nkosarek 15:d117591084ff 237 int err = Wire.write( (uint8_t)addr );
nkosarek 15:d117591084ff 238 Wire.write(pbuf, length);
nkosarek 15:d117591084ff 239 if (err != 0) {
nkosarek 15:d117591084ff 240 //pc.printf("error on write write! %d\r\n", err);
nkosarek 15:d117591084ff 241 }
nkosarek 15:d117591084ff 242 uint8_t err8 = Wire.endTransmission();
nkosarek 15:d117591084ff 243 if (err8 != 0) {
nkosarek 15:d117591084ff 244 //pc.printf("error on write end transmission! %d\r\n", err8);
nkosarek 15:d117591084ff 245 }
nkosarek 15:d117591084ff 246 }
nkosarek 15:d117591084ff 247
nkosarek 15:d117591084ff 248 void AT24C512_ReadBytes(uint16_t addr, uint8_t *pbuf, uint16_t length, uint16_t i2cAddr)
nkosarek 15:d117591084ff 249 {
nkosarek 15:d117591084ff 250 Wire.beginTransmission(i2cAddr);
nkosarek 15:d117591084ff 251 int err= Wire.write( (uint8_t)addr );
nkosarek 15:d117591084ff 252 if (err != 0) {
nkosarek 15:d117591084ff 253 //pc.printf("error on read write! %d\r\n", err);
nkosarek 15:d117591084ff 254 }
nkosarek 15:d117591084ff 255 uint8_t err8 = Wire.endTransmission();
nkosarek 15:d117591084ff 256 if (err8 != 0) {
nkosarek 15:d117591084ff 257 //pc.printf("error on read end transmission! %d\r\n", err8);
nkosarek 15:d117591084ff 258 }
nkosarek 15:d117591084ff 259
nkosarek 15:d117591084ff 260 err8 = Wire.requestFrom(i2cAddr+1, length);
nkosarek 15:d117591084ff 261 if (err != 0) {
nkosarek 15:d117591084ff 262 //pc.printf("error on read request from! %d\r\n", err8);
nkosarek 15:d117591084ff 263 }
nkosarek 15:d117591084ff 264 while( Wire.available() > 0 )
nkosarek 15:d117591084ff 265 {
nkosarek 15:d117591084ff 266 *pbuf = Wire.read();
nkosarek 15:d117591084ff 267 pbuf++;
nkosarek 15:d117591084ff 268 }
nkosarek 15:d117591084ff 269 }
nkosarek 15:d117591084ff 270
nkosarek 15:d117591084ff 271 //Set the bit at index 'bit' to 'value' on 'input' and return
nkosarek 15:d117591084ff 272 uint8_t setBit(uint8_t input, uint8_t bit, uint8_t value) {
nkosarek 15:d117591084ff 273 uint8_t mask = 1 << bit;
nkosarek 15:d117591084ff 274 input &= ~mask;
nkosarek 15:d117591084ff 275 if (value == 1) {
nkosarek 15:d117591084ff 276 input |= mask;
nkosarek 15:d117591084ff 277 }
nkosarek 15:d117591084ff 278 return input;
nkosarek 15:d117591084ff 279 }
nkosarek 15:d117591084ff 280
znew711 16:799397f0d3a8 281 int16_t getAxis(uint16_t axis, uint16_t i2cAddr)
nkosarek 15:d117591084ff 282 {
nkosarek 15:d117591084ff 283 uint8_t base = REG_OUT_X_L + (2 * axis);
nkosarek 15:d117591084ff 284 uint8_t* low = new uint8_t[1];
nkosarek 15:d117591084ff 285 uint8_t* high = new uint8_t[1];
nkosarek 15:d117591084ff 286 AT24C512_ReadBytes(base, low, 1, i2cAddr);
nkosarek 15:d117591084ff 287 AT24C512_ReadBytes(base + 1, high, 1, i2cAddr);
znew711 16:799397f0d3a8 288 int16_t res = low[0] | (high[0] << 8);
nkosarek 15:d117591084ff 289 delete[] low;
nkosarek 15:d117591084ff 290 delete[] high;
nkosarek 15:d117591084ff 291 return res;
nkosarek 15:d117591084ff 292 }
nkosarek 15:d117591084ff 293
nkosarek 15:d117591084ff 294 void setRange(uint8_t range, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 295 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 296 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 297 val[0] &= ~(0b110000); //zero out lowest 2 bits of top 4 bits
nkosarek 15:d117591084ff 298 val[0] |= (range << 4); // write in our new range
nkosarek 15:d117591084ff 299 //pc.printf("REG_CTRL4 after setRange: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 300 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 301 delete[] val;
nkosarek 15:d117591084ff 302 }
nkosarek 15:d117591084ff 303
nkosarek 15:d117591084ff 304 //Set whether we want to use high resolution or not
nkosarek 15:d117591084ff 305 void setHighResolution(bool highRes, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 306 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 307 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 308 uint8_t final;
nkosarek 15:d117591084ff 309 if (highRes) {
nkosarek 15:d117591084ff 310 final = setBit(val[0], 3, 1);
nkosarek 15:d117591084ff 311 } else {
nkosarek 15:d117591084ff 312 final = setBit(val[0], 3, 0);
nkosarek 15:d117591084ff 313 }
nkosarek 15:d117591084ff 314 val[0] = final;
nkosarek 15:d117591084ff 315 //pc.printf("REG_CTRL4 after setHiRes: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 316 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 317 delete[] val;
nkosarek 15:d117591084ff 318 }
nkosarek 15:d117591084ff 319
nkosarek 15:d117591084ff 320 void setAxisStatus(uint8_t axis, bool enable, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 321 uint8_t* current = new uint8_t[1];
nkosarek 15:d117591084ff 322 AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 323 uint8_t final;
nkosarek 15:d117591084ff 324 if (enable == 1) {
nkosarek 15:d117591084ff 325 final = setBit(current[0], axis, 1);
nkosarek 15:d117591084ff 326 } else {
nkosarek 15:d117591084ff 327 final = setBit(current[0], axis, 0);
nkosarek 15:d117591084ff 328 }
nkosarek 15:d117591084ff 329 current[0] = final;
nkosarek 15:d117591084ff 330 AT24C512_WriteBytes(REG_CTRL1, current, 1, i2cAddr);
nkosarek 15:d117591084ff 331
nkosarek 15:d117591084ff 332 AT24C512_ReadBytes(REG_CTRL1, current, 1, i2cAddr);
nkosarek 15:d117591084ff 333 //pc.printf("REG_CTRL1 after setAxisStatus: 0x%x\r\n", *current);
nkosarek 15:d117591084ff 334 delete[] current;
nkosarek 15:d117591084ff 335 }
nkosarek 15:d117591084ff 336
nkosarek 15:d117591084ff 337 void setDataRate(uint8_t dataRate, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 338 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 339 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 340 //pc.printf("REG_CTRL1 before data rate set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 341 val[0] &= 0b11100111; //d
nkosarek 15:d117591084ff 342 val[0] |= (dataRate << 3);
nkosarek 15:d117591084ff 343 AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 344
nkosarek 15:d117591084ff 345 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 346 //pc.printf("REG_CTRL1 after data rate set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 347 delete[] val;
nkosarek 15:d117591084ff 348 }
nkosarek 15:d117591084ff 349
nkosarek 15:d117591084ff 350 void setPowerMode(uint8_t powerMode, uint16_t i2cAddr) {
nkosarek 15:d117591084ff 351 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 352 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 353 val[0] &= 0b11111;
nkosarek 15:d117591084ff 354 val[0] |= (powerMode << 5);
nkosarek 15:d117591084ff 355 //pc.printf("writing this to REG_CTRL1: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 356 AT24C512_WriteBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 357
nkosarek 15:d117591084ff 358 AT24C512_ReadBytes(REG_CTRL1, val, 1, i2cAddr);
nkosarek 15:d117591084ff 359 //pc.printf("REG_CTRL1 after power mode set: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 360 delete[] val;
nkosarek 15:d117591084ff 361 }
nkosarek 15:d117591084ff 362
nkosarek 15:d117591084ff 363 void setBDU(bool bdu, uint16_t i2cAddr)
nkosarek 15:d117591084ff 364 {
nkosarek 15:d117591084ff 365 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 366 AT24C512_ReadBytes(REG_CTRL4, val, 1, i2cAddr);//get value from the register
nkosarek 15:d117591084ff 367 //pc.printf("REG_CTRL4: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 368 uint8_t final;
nkosarek 15:d117591084ff 369 if (bdu) {
nkosarek 15:d117591084ff 370 final = setBit(val[0], 7, 1);
nkosarek 15:d117591084ff 371 } else {
nkosarek 15:d117591084ff 372 final = setBit(val[0], 7, 0);
nkosarek 15:d117591084ff 373 }
nkosarek 15:d117591084ff 374 val[0] = final;
nkosarek 15:d117591084ff 375 //pc.printf("REG_CTRL4 after setBDU: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 376 AT24C512_WriteBytes(REG_CTRL4, val, 1, i2cAddr);
nkosarek 15:d117591084ff 377 delete[] val;
nkosarek 15:d117591084ff 378 }
nkosarek 15:d117591084ff 379
znew711 16:799397f0d3a8 380 int16_t getX(uint16_t i2cAddr)
nkosarek 15:d117591084ff 381 {
nkosarek 15:d117591084ff 382 return getAxis(AXIS_X, i2cAddr);
nkosarek 15:d117591084ff 383 }
nkosarek 15:d117591084ff 384
znew711 16:799397f0d3a8 385 int16_t getY(uint16_t i2cAddr)
nkosarek 15:d117591084ff 386 {
nkosarek 15:d117591084ff 387 return getAxis(AXIS_Y, i2cAddr);
nkosarek 15:d117591084ff 388 }
nkosarek 15:d117591084ff 389
znew711 16:799397f0d3a8 390 int16_t getZ(uint16_t i2cAddr)
nkosarek 15:d117591084ff 391 {
nkosarek 15:d117591084ff 392 return getAxis(AXIS_Z, i2cAddr);
nkosarek 15:d117591084ff 393 }
nkosarek 15:d117591084ff 394
rgrover1 0:28f095301cb2 395 int main(void)
rgrover1 0:28f095301cb2 396 {
nkosarek 15:d117591084ff 397 //pc.baud(9600);
nkosarek 15:d117591084ff 398 wait(5);
nkosarek 15:d117591084ff 399 //Wire.begin();
nkosarek 15:d117591084ff 400 Wire.begin(SCL, SDA, TWI_FREQUENCY_100K);
nkosarek 15:d117591084ff 401
nkosarek 15:d117591084ff 402 //pc.printf("\r\n\r\n\r\nStarting...\r\n");
nkosarek 15:d117591084ff 403
nkosarek 15:d117591084ff 404 wait(5);
nkosarek 15:d117591084ff 405
nkosarek 15:d117591084ff 406 setAxisStatus(AXIS_X, true, ADDR_ONE);
nkosarek 15:d117591084ff 407 setAxisStatus(AXIS_Y, true, ADDR_ONE);
nkosarek 15:d117591084ff 408 setAxisStatus(AXIS_Z, true, ADDR_ONE);
nkosarek 15:d117591084ff 409 setDataRate(DATARATE_400HZ, ADDR_ONE);
nkosarek 15:d117591084ff 410 setPowerMode(DATARATE_NORMAL_MODE, ADDR_ONE);
nkosarek 15:d117591084ff 411 //setHighResolution(true, ADDR_ONE);
nkosarek 15:d117591084ff 412 setBDU(true, ADDR_ONE);
nkosarek 15:d117591084ff 413 //setRange(RANGE_2G, ADDR_ONE);
nkosarek 15:d117591084ff 414
nkosarek 15:d117591084ff 415
nkosarek 15:d117591084ff 416 setAxisStatus(AXIS_X, true, ADDR_TWO);
nkosarek 15:d117591084ff 417 setAxisStatus(AXIS_Y, true, ADDR_TWO);
nkosarek 15:d117591084ff 418 setAxisStatus(AXIS_Z, true, ADDR_TWO);
nkosarek 15:d117591084ff 419 setDataRate(DATARATE_400HZ, ADDR_TWO);
nkosarek 15:d117591084ff 420 setPowerMode(DATARATE_NORMAL_MODE, ADDR_TWO);
nkosarek 15:d117591084ff 421 //setHighResolution(true, ADDR_TWO);
nkosarek 15:d117591084ff 422 setBDU(true, ADDR_TWO);
nkosarek 15:d117591084ff 423 //setRange(RANGE_2G, ADDR_TWO);
nkosarek 15:d117591084ff 424
nkosarek 15:d117591084ff 425 uint8_t* val = new uint8_t[1];
nkosarek 15:d117591084ff 426 *val = 0x80;
nkosarek 15:d117591084ff 427 AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_ONE);
nkosarek 15:d117591084ff 428 AT24C512_WriteBytes(REG_CTRL4, val, 1, ADDR_TWO);
nkosarek 15:d117591084ff 429 AT24C512_ReadBytes(REG_CTRL4, val, 1, ADDR_ONE);
nkosarek 15:d117591084ff 430 //pc.printf("REG_CTRL4, should be 0x80: 0x%x\r\n", *val);
nkosarek 15:d117591084ff 431
nkosarek 15:d117591084ff 432 uint8_t* whoami = new uint8_t[1];
nkosarek 15:d117591084ff 433 AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_ONE);
nkosarek 15:d117591084ff 434 //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
nkosarek 15:d117591084ff 435 AT24C512_ReadBytes(REG_WHOAMI, whoami, 1, ADDR_TWO);
nkosarek 15:d117591084ff 436 //pc.printf("REG_WHOAMI should be 0x32: 0x%x\r\n", *whoami);
nkosarek 15:d117591084ff 437 AT24C512_ReadBytes(0x1F, whoami, 1, ADDR_ONE);
nkosarek 15:d117591084ff 438
nkosarek 15:d117591084ff 439
andresag 10:7943b5c1117a 440 BLE &ble = BLE::Instance();
andresag 10:7943b5c1117a 441 ble.init(bleInitComplete);
nkosarek 15:d117591084ff 442
nkosarek 15:d117591084ff 443 //pc.printf("entering spin loop\r\n");
andresag 10:7943b5c1117a 444 /* SpinWait for initialization to complete. This is necessary because the
andresag 10:7943b5c1117a 445 * BLE object is used in the main loop below. */
andresag 10:7943b5c1117a 446 while (ble.hasInitialized() == false) { /* spin loop */ }
nkosarek 15:d117591084ff 447 //pc.printf("leaving spin loop\r\n");
nkosarek 15:d117591084ff 448
nkosarek 15:d117591084ff 449 pq.size = QUEUE_SIZE;
nkosarek 15:d117591084ff 450 pq.nextPacketToSend = 0;
nkosarek 15:d117591084ff 451 pq.nextSampleToSave = 0;
nkosarek 15:d117591084ff 452 pq.liveSamples = 0;
znew711 16:799397f0d3a8 453
znew711 16:799397f0d3a8 454 uint8_t lastThreePackets[3][20];
znew711 18:6645cb0f517f 455 uint8_t firstThreePackets[3][20];
znew711 18:6645cb0f517f 456 uint8_t streakIndex = 0;
znew711 16:799397f0d3a8 457 uint8_t lastThreeIndex = 0;
znew711 16:799397f0d3a8 458 uint8_t underThresholdCount = 0;
znew711 18:6645cb0f517f 459 uint8_t aboveThresholdCount = 0;
znew711 18:6645cb0f517f 460 bool inAStreak = false;
znew711 17:09ceae7cb00e 461 uint16_t counter = 0;
nkosarek 15:d117591084ff 462
nkosarek 15:d117591084ff 463 while(1)
nkosarek 15:d117591084ff 464 {
nkosarek 15:d117591084ff 465 //pc.printf("Read data from AT24C512\r\n");
znew711 16:799397f0d3a8 466 int16_t x1 = getX(ADDR_ONE);
znew711 16:799397f0d3a8 467 int16_t y1 = getY(ADDR_ONE);
znew711 16:799397f0d3a8 468 int16_t z1 = getZ(ADDR_ONE);
nkosarek 15:d117591084ff 469
znew711 16:799397f0d3a8 470 int16_t x2 = getX(ADDR_TWO);
znew711 16:799397f0d3a8 471 int16_t y2 = getY(ADDR_TWO);
znew711 16:799397f0d3a8 472 int16_t z2 = getZ(ADDR_TWO);
nkosarek 15:d117591084ff 473
nkosarek 15:d117591084ff 474 //pc.printf("Accel one: x %d y %d z %d\r\n", (int16_t)x1, (int16_t)y1, (int16_t)z1);
nkosarek 15:d117591084ff 475 //pc.printf("Accel two: x %d y %d z %d\r\n", (int16_t)x2, (int16_t)y2, (int16_t)z2);
znew711 17:09ceae7cb00e 476 bool triggerOne = z1 > ACCEL_ONE_RESTING + ACCEL_ONE_EPSILON || z1 < ACCEL_ONE_RESTING - ACCEL_ONE_EPSILON;
znew711 17:09ceae7cb00e 477 bool triggerTwo = z2 > ACCEL_TWO_RESTING + ACCEL_TWO_EPSILON || z2 < ACCEL_TWO_RESTING - ACCEL_TWO_EPSILON;
znew711 16:799397f0d3a8 478 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 479 (uint8_t)(x2 >> 8), (uint8_t)x2, (uint8_t)(y2 >> 8), (uint8_t)y2, (uint8_t)(z2 >> 8), (uint8_t)z2,
znew711 17:09ceae7cb00e 480 (uint8_t)(counter >> 8), (uint8_t) counter, 0, 0, 0, 0, (uint8_t) triggerOne, (uint8_t) triggerTwo};
znew711 16:799397f0d3a8 481 //TODO: handle negative accels
znew711 18:6645cb0f517f 482 if (triggerOne || triggerTwo) {
znew711 18:6645cb0f517f 483 inAStreak = true;
znew711 16:799397f0d3a8 484 underThresholdCount = 0;
znew711 18:6645cb0f517f 485 if (streakIndex < 3) {
znew711 18:6645cb0f517f 486 for (int i = 0; i < 20; i++) {
znew711 18:6645cb0f517f 487 firstThreePackets[streakIndex][i] = values[i];
znew711 18:6645cb0f517f 488 }
znew711 18:6645cb0f517f 489 streakIndex++;
znew711 18:6645cb0f517f 490 aboveThresholdCount++;
znew711 18:6645cb0f517f 491 } else if (streakIndex == 3) {
znew711 18:6645cb0f517f 492 streakIndex++;
znew711 18:6645cb0f517f 493 uint8_t temp[20];
znew711 16:799397f0d3a8 494 for (int i = 0; i < 3; i++) {
znew711 17:09ceae7cb00e 495 for (int j = 0; j < 20; j++) {
znew711 17:09ceae7cb00e 496 temp[j] = lastThreePackets[(lastThreeIndex + i) % 3][j];
znew711 17:09ceae7cb00e 497 }
znew711 17:09ceae7cb00e 498 addToQueue(temp);
znew711 16:799397f0d3a8 499 }
znew711 18:6645cb0f517f 500 for (int i = 0; i < 3; i++) {
znew711 18:6645cb0f517f 501 for (int j = 0; j < 20; j++) {
znew711 18:6645cb0f517f 502 temp[j] = firstThreePackets[i][j];
znew711 18:6645cb0f517f 503 }
znew711 18:6645cb0f517f 504 addToQueue(temp);
znew711 18:6645cb0f517f 505 }
znew711 16:799397f0d3a8 506 addToQueue(values);
znew711 16:799397f0d3a8 507 startTransmission();
znew711 16:799397f0d3a8 508 } else {
znew711 18:6645cb0f517f 509 streakIndex++;
znew711 16:799397f0d3a8 510 addToQueue(values);
znew711 16:799397f0d3a8 511 }
znew711 16:799397f0d3a8 512 } else {
znew711 18:6645cb0f517f 513 if (streakIndex < 3 && inAStreak) {
znew711 18:6645cb0f517f 514 underThresholdCount += 1;
znew711 18:6645cb0f517f 515 for (int i = 0; i < 20; i++) {
znew711 18:6645cb0f517f 516 firstThreePackets[streakIndex][i] = values[i];
znew711 18:6645cb0f517f 517 }
znew711 18:6645cb0f517f 518 streakIndex++;
znew711 18:6645cb0f517f 519 } else if (streakIndex == 3) {
znew711 18:6645cb0f517f 520 underThresholdCount++;
znew711 18:6645cb0f517f 521 if (aboveThresholdCount > 1) {
znew711 18:6645cb0f517f 522 uint8_t temp[20];
znew711 18:6645cb0f517f 523 for (int i = 0; i < 3; i++) {
znew711 18:6645cb0f517f 524 for (int j = 0; j < 20; j++) {
znew711 18:6645cb0f517f 525 temp[j] = lastThreePackets[(lastThreeIndex + i) % 3][j];
znew711 18:6645cb0f517f 526 }
znew711 18:6645cb0f517f 527 addToQueue(temp);
znew711 18:6645cb0f517f 528 }
znew711 18:6645cb0f517f 529 for (int i = 0; i < 3; i++) {
znew711 18:6645cb0f517f 530 for (int j = 0; j < 20; j++) {
znew711 18:6645cb0f517f 531 temp[j] = firstThreePackets[i][j];
znew711 18:6645cb0f517f 532 }
znew711 18:6645cb0f517f 533 addToQueue(temp);
znew711 18:6645cb0f517f 534 }
znew711 18:6645cb0f517f 535 addToQueue(values);
znew711 18:6645cb0f517f 536 streakIndex++;
znew711 18:6645cb0f517f 537 startTransmission();
znew711 18:6645cb0f517f 538 } else {
znew711 18:6645cb0f517f 539 streakIndex = 0;
znew711 18:6645cb0f517f 540 inAStreak = false;
znew711 18:6645cb0f517f 541 aboveThresholdCount = 0;
znew711 18:6645cb0f517f 542 }
znew711 18:6645cb0f517f 543 } else if (underThresholdCount < 3 && inAStreak) {
znew711 18:6645cb0f517f 544 underThresholdCount++;
znew711 18:6645cb0f517f 545 streakIndex++;
znew711 18:6645cb0f517f 546 addToQueue(values);
znew711 18:6645cb0f517f 547 } else {
znew711 18:6645cb0f517f 548 inAStreak = false;
znew711 18:6645cb0f517f 549 streakIndex = 0;
znew711 18:6645cb0f517f 550 aboveThresholdCount = 0;
znew711 18:6645cb0f517f 551 for (int i = 0; i < 20; i++) {
znew711 18:6645cb0f517f 552 lastThreePackets[lastThreeIndex][i] = values[i];
znew711 18:6645cb0f517f 553 }
znew711 18:6645cb0f517f 554 lastThreeIndex = (lastThreeIndex + 1) % 3;
znew711 16:799397f0d3a8 555 }
rgrover1 9:0f6951db24f1 556 }
znew711 18:6645cb0f517f 557 counter++;
znew711 18:6645cb0f517f 558 wait_ms(5);
rgrover1 0:28f095301cb2 559 }
nkosarek 15:d117591084ff 560 }