MZJ / Mbed 2 deprecated TenCount_BLE_08252015

Working code with BLE and accelerometer.(DFU pending)

Dependencies:   BLE_API mbed nRF51822 strike_detect

Fork of TenCount_BLE by MZJ

main.cpp@5:7c5630442028, 2016-08-25 (annotated)

Committer:
AamirNiaz
Date:
Thu Aug 25 09:02:27 2016 +0000
Revision:
5:7c5630442028
Parent:
4:2b0947ed0e0d 
nrf51822 code with working accelerometer.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
RedBearLab 1:1c058e553423 1 /*
RedBearLab 0:cffe8ac1bdf0 2
RedBearLab 1:1c058e553423 3 Copyright (c) 2012-2014 RedBearLab
RedBearLab 1:1c058e553423 4
RedBearLab 1:1c058e553423 5 Permission is hereby granted, free of charge, to any person obtaining a copy of this software
RedBearLab 1:1c058e553423 6 and associated documentation files (the "Software"), to deal in the Software without restriction,
RedBearLab 1:1c058e553423 7 including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense,
RedBearLab 1:1c058e553423 8 and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so,
RedBearLab 1:1c058e553423 9 subject to the following conditions:
RedBearLab 1:1c058e553423 10 The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software.
RedBearLab 1:1c058e553423 11
RedBearLab 1:1c058e553423 12 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED,
RedBearLab 1:1c058e553423 13 INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR
RedBearLab 1:1c058e553423 14 PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE
RedBearLab 1:1c058e553423 15 FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
RedBearLab 1:1c058e553423 16 ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
RedBearLab 1:1c058e553423 17
RedBearLab 1:1c058e553423 18 */
RedBearLab 1:1c058e553423 19
RedBearLab 1:1c058e553423 20 /*
RedBearLab 1:1c058e553423 21 * The application works with the BLEController iOS/Android App.
RedBearLab 1:1c058e553423 22 * Type something from the Terminal to send
RedBearLab 1:1c058e553423 23 * to the BLEController App or vice verse.
RedBearLab 1:1c058e553423 24 * Characteristics received from App will print on Terminal.
RedBearLab 1:1c058e553423 25 */
RedBearLab 1:1c058e553423 26
RedBearLab 0:cffe8ac1bdf0 27 #include "mbed.h"
RedBearLab 2:4b66b69c7ecb 28 #include "ble/BLE.h"
elmbed 4:2b0947ed0e0d 29 #include "LIS3DH.h"
elmbed 4:2b0947ed0e0d 30 #include "strike.h"
elmbed 4:2b0947ed0e0d 31 #include "Sensor.h"
RedBearLab 0:cffe8ac1bdf0 32
RedBearLab 0:cffe8ac1bdf0 33 #define BLE_UUID_TXRX_SERVICE 0x0000 /**< The UUID of the Nordic UART Service. */
RedBearLab 0:cffe8ac1bdf0 34 #define BLE_UUID_TX_CHARACTERISTIC 0x0002 /**< The UUID of the TX Characteristic. */
RedBearLab 0:cffe8ac1bdf0 35 #define BLE_UUIDS_RX_CHARACTERISTIC 0x0003 /**< The UUID of the RX Characteristic. */
RedBearLab 0:cffe8ac1bdf0 36
RedBearLab 0:cffe8ac1bdf0 37 #define TXRX_BUF_LEN 20
RedBearLab 0:cffe8ac1bdf0 38
elmbed 4:2b0947ed0e0d 39
elmbed 4:2b0947ed0e0d 40 //you can receive less then 93 bytes at 921600 Bd
elmbed 4:2b0947ed0e0d 41 #define UART_SENSOR_COMMAND_BUFFER_SIZE (100)
elmbed 4:2b0947ed0e0d 42 #define SYSTEM_COM_BUFFER (1024)
elmbed 4:2b0947ed0e0d 43 #define RADIO_MESSAGE_LEN (64)
elmbed 4:2b0947ed0e0d 44 #define VERSION "alpha_20150409-01"
elmbed 4:2b0947ed0e0d 45 #define SPEW (0)
elmbed 4:2b0947ed0e0d 46 #define STRIKE_DETECT_1 (1)
elmbed 4:2b0947ed0e0d 47 #define STRIKE_DETECT_2 (2)
elmbed 4:2b0947ed0e0d 48
elmbed 4:2b0947ed0e0d 49 #define DEFAULT_TIMESTEP (10) // in milliseconds
elmbed 4:2b0947ed0e0d 50 //#define DEFAULT_FAST_FILTER (650)
elmbed 4:2b0947ed0e0d 51 //#define DEFAULT_SLOW_FILTER (975)
elmbed 4:2b0947ed0e0d 52 #define DEFAULT_BIG_HIT_THRESHOLD (3000)
elmbed 4:2b0947ed0e0d 53 #define DEFAULT_SMALL_HIT_THRESHOLD (200)
elmbed 4:2b0947ed0e0d 54 #define DEFAULT_JERK_THRESHOLD (500)
elmbed 4:2b0947ed0e0d 55
elmbed 4:2b0947ed0e0d 56 #define DEFAULT_C1_1 (0.35)
elmbed 4:2b0947ed0e0d 57 #define DEFAULT_C2_1 (0.65)
elmbed 4:2b0947ed0e0d 58 #define DEFAULT_C1_2 (0.025)
elmbed 4:2b0947ed0e0d 59 #define DEFAULT_C2_2 (0.975)
elmbed 4:2b0947ed0e0d 60
elmbed 4:2b0947ed0e0d 61 #define DEFAULT_SHADOW_DECAY (0.8)
elmbed 4:2b0947ed0e0d 62
RedBearLab 2:4b66b69c7ecb 63 BLE ble;
RedBearLab 0:cffe8ac1bdf0 64
AamirNiaz 5:7c5630442028 65 Serial pc(P0_4, USBRX);
RedBearLab 0:cffe8ac1bdf0 66
RedBearLab 0:cffe8ac1bdf0 67
RedBearLab 0:cffe8ac1bdf0 68 // The Nordic UART Service
RedBearLab 0:cffe8ac1bdf0 69 static const uint8_t uart_base_uuid[] = {0x71, 0x3D, 0, 0, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
RedBearLab 0:cffe8ac1bdf0 70 static const uint8_t uart_tx_uuid[] = {0x71, 0x3D, 0, 3, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
RedBearLab 0:cffe8ac1bdf0 71 static const uint8_t uart_rx_uuid[] = {0x71, 0x3D, 0, 2, 0x50, 0x3E, 0x4C, 0x75, 0xBA, 0x94, 0x31, 0x48, 0xF1, 0x8D, 0x94, 0x1E};
RedBearLab 0:cffe8ac1bdf0 72 static const uint8_t uart_base_uuid_rev[] = {0x1E, 0x94, 0x8D, 0xF1, 0x48, 0x31, 0x94, 0xBA, 0x75, 0x4C, 0x3E, 0x50, 0, 0, 0x3D, 0x71};
RedBearLab 0:cffe8ac1bdf0 73
RedBearLab 0:cffe8ac1bdf0 74
RedBearLab 0:cffe8ac1bdf0 75 uint8_t txPayload[TXRX_BUF_LEN] = {0,};
RedBearLab 0:cffe8ac1bdf0 76 uint8_t rxPayload[TXRX_BUF_LEN] = {0,};
RedBearLab 0:cffe8ac1bdf0 77
RedBearLab 0:cffe8ac1bdf0 78 static uint8_t rx_buf[TXRX_BUF_LEN];
RedBearLab 0:cffe8ac1bdf0 79 static uint8_t rx_len=0;
RedBearLab 0:cffe8ac1bdf0 80
RedBearLab 0:cffe8ac1bdf0 81
RedBearLab 0:cffe8ac1bdf0 82 GattCharacteristic txCharacteristic (uart_tx_uuid, txPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE | GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_WRITE_WITHOUT_RESPONSE);
RedBearLab 0:cffe8ac1bdf0 83
RedBearLab 0:cffe8ac1bdf0 84 GattCharacteristic rxCharacteristic (uart_rx_uuid, rxPayload, 1, TXRX_BUF_LEN, GattCharacteristic::BLE_GATT_CHAR_PROPERTIES_NOTIFY);
RedBearLab 0:cffe8ac1bdf0 85
RedBearLab 0:cffe8ac1bdf0 86 GattCharacteristic *uartChars[] = {&txCharacteristic, &rxCharacteristic};
RedBearLab 0:cffe8ac1bdf0 87
RedBearLab 0:cffe8ac1bdf0 88 GattService uartService(uart_base_uuid, uartChars, sizeof(uartChars) / sizeof(GattCharacteristic *));
RedBearLab 0:cffe8ac1bdf0 89
RedBearLab 0:cffe8ac1bdf0 90
RedBearLab 0:cffe8ac1bdf0 91
elmbed 4:2b0947ed0e0d 92 void interpret(char parameter, int value);
elmbed 4:2b0947ed0e0d 93 void getRadioInput(char *data, uint16_t size);
elmbed 4:2b0947ed0e0d 94 void read_accel();
elmbed 4:2b0947ed0e0d 95
elmbed 4:2b0947ed0e0d 96 Ticker measure;
elmbed 4:2b0947ed0e0d 97 Ticker spew;
elmbed 4:2b0947ed0e0d 98 uint16_t strike_value = 0;
elmbed 4:2b0947ed0e0d 99 int last_result = 0;
elmbed 4:2b0947ed0e0d 100 bool new_strike = false;
elmbed 4:2b0947ed0e0d 101 bool active = false;
elmbed 4:2b0947ed0e0d 102 static volatile int config_parameter = 0;
elmbed 4:2b0947ed0e0d 103
elmbed 4:2b0947ed0e0d 104 static volatile int mode = STRIKE_DETECT_2;
elmbed 4:2b0947ed0e0d 105 static volatile int timestep = DEFAULT_TIMESTEP;
elmbed 4:2b0947ed0e0d 106 //static volatile int fast_filter = DEFAULT_FAST_FILTER;
elmbed 4:2b0947ed0e0d 107 //static volatile int slow_filter = DEFAULT_SLOW_FILTER;
elmbed 4:2b0947ed0e0d 108 static volatile int big_hit_threshold = DEFAULT_BIG_HIT_THRESHOLD;
elmbed 4:2b0947ed0e0d 109 static volatile int small_hit_threshold = DEFAULT_SMALL_HIT_THRESHOLD;
elmbed 4:2b0947ed0e0d 110 static volatile int jerk_threshold = DEFAULT_JERK_THRESHOLD;
elmbed 4:2b0947ed0e0d 111
elmbed 4:2b0947ed0e0d 112 static volatile float c1_1 = DEFAULT_C1_1;
elmbed 4:2b0947ed0e0d 113 static volatile float c2_1 = DEFAULT_C2_1;
elmbed 4:2b0947ed0e0d 114 static volatile float c1_2 = DEFAULT_C1_2;
elmbed 4:2b0947ed0e0d 115 static volatile float c2_2 = DEFAULT_C2_2;
elmbed 4:2b0947ed0e0d 116
elmbed 4:2b0947ed0e0d 117 static volatile float shadow_decay = DEFAULT_SHADOW_DECAY;
elmbed 4:2b0947ed0e0d 118
elmbed 4:2b0947ed0e0d 119 static int16_t xzy[3];
elmbed 4:2b0947ed0e0d 120
elmbed 4:2b0947ed0e0d 121
RedBearLab 3:b3f6c612b603 122 void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params)
RedBearLab 0:cffe8ac1bdf0 123 {
RedBearLab 0:cffe8ac1bdf0 124 pc.printf("Disconnected \r\n");
RedBearLab 0:cffe8ac1bdf0 125 pc.printf("Restart advertising \r\n");
RedBearLab 0:cffe8ac1bdf0 126 ble.startAdvertising();
RedBearLab 0:cffe8ac1bdf0 127 }
RedBearLab 0:cffe8ac1bdf0 128
RedBearLab 2:4b66b69c7ecb 129 void WrittenHandler(const GattWriteCallbackParams *Handler)
RedBearLab 0:cffe8ac1bdf0 130 {
RedBearLab 0:cffe8ac1bdf0 131 uint8_t buf[TXRX_BUF_LEN];
RedBearLab 0:cffe8ac1bdf0 132 uint16_t bytesRead, index;
RedBearLab 0:cffe8ac1bdf0 133
RedBearLab 2:4b66b69c7ecb 134 if (Handler->handle == txCharacteristic.getValueAttribute().getHandle())
RedBearLab 0:cffe8ac1bdf0 135 {
RedBearLab 0:cffe8ac1bdf0 136 ble.readCharacteristicValue(txCharacteristic.getValueAttribute().getHandle(), buf, &bytesRead);
RedBearLab 0:cffe8ac1bdf0 137 memset(txPayload, 0, TXRX_BUF_LEN);
RedBearLab 0:cffe8ac1bdf0 138 memcpy(txPayload, buf, TXRX_BUF_LEN);
RedBearLab 0:cffe8ac1bdf0 139 pc.printf("WriteHandler \r\n");
elmbed 4:2b0947ed0e0d 140 pc.printf("Length: %d\r\n", bytesRead);
RedBearLab 0:cffe8ac1bdf0 141 pc.printf("Data: ");
RedBearLab 0:cffe8ac1bdf0 142 for(index=0; index<bytesRead; index++)
RedBearLab 0:cffe8ac1bdf0 143 {
RedBearLab 0:cffe8ac1bdf0 144 pc.putc(txPayload[index]);
RedBearLab 0:cffe8ac1bdf0 145 }
RedBearLab 0:cffe8ac1bdf0 146 pc.printf("\r\n");
elmbed 4:2b0947ed0e0d 147
elmbed 4:2b0947ed0e0d 148 getRadioInput((char*)txPayload, bytesRead);
RedBearLab 0:cffe8ac1bdf0 149 }
RedBearLab 0:cffe8ac1bdf0 150 }
RedBearLab 0:cffe8ac1bdf0 151
RedBearLab 0:cffe8ac1bdf0 152 void uartCB(void)
RedBearLab 0:cffe8ac1bdf0 153 {
RedBearLab 0:cffe8ac1bdf0 154 while(pc.readable())
RedBearLab 0:cffe8ac1bdf0 155 {
RedBearLab 0:cffe8ac1bdf0 156 rx_buf[rx_len++] = pc.getc();
RedBearLab 0:cffe8ac1bdf0 157 if(rx_len>=20 || rx_buf[rx_len-1]=='\0' || rx_buf[rx_len-1]=='\n')
RedBearLab 0:cffe8ac1bdf0 158 {
RedBearLab 0:cffe8ac1bdf0 159 ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), rx_buf, rx_len);
RedBearLab 0:cffe8ac1bdf0 160 pc.printf("RecHandler \r\n");
RedBearLab 0:cffe8ac1bdf0 161 pc.printf("Length: ");
RedBearLab 0:cffe8ac1bdf0 162 pc.putc(rx_len);
RedBearLab 0:cffe8ac1bdf0 163 pc.printf("\r\n");
RedBearLab 0:cffe8ac1bdf0 164 rx_len = 0;
RedBearLab 0:cffe8ac1bdf0 165 break;
RedBearLab 0:cffe8ac1bdf0 166 }
RedBearLab 0:cffe8ac1bdf0 167 }
RedBearLab 0:cffe8ac1bdf0 168 }
RedBearLab 0:cffe8ac1bdf0 169
RedBearLab 0:cffe8ac1bdf0 170 int main(void)
RedBearLab 0:cffe8ac1bdf0 171 {
RedBearLab 0:cffe8ac1bdf0 172 ble.init();
RedBearLab 0:cffe8ac1bdf0 173 ble.onDisconnection(disconnectionCallback);
RedBearLab 0:cffe8ac1bdf0 174 ble.onDataWritten(WrittenHandler);
RedBearLab 0:cffe8ac1bdf0 175
RedBearLab 0:cffe8ac1bdf0 176 pc.baud(9600);
elmbed 4:2b0947ed0e0d 177 pc.printf("TenCount \r\n");
elmbed 4:2b0947ed0e0d 178
elmbed 4:2b0947ed0e0d 179 LIS3DH_init();
RedBearLab 0:cffe8ac1bdf0 180
AamirNiaz 5:7c5630442028 181 //pc.attach( uartCB , pc.RxIrq);
RedBearLab 0:cffe8ac1bdf0 182 // setup advertising
RedBearLab 0:cffe8ac1bdf0 183 ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED);
RedBearLab 0:cffe8ac1bdf0 184 ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
RedBearLab 0:cffe8ac1bdf0 185 ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME,
elmbed 4:2b0947ed0e0d 186 (const uint8_t *)"TenCount", sizeof("TenCount") - 1);
RedBearLab 0:cffe8ac1bdf0 187 ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS,
RedBearLab 0:cffe8ac1bdf0 188 (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid));
RedBearLab 0:cffe8ac1bdf0 189 // 100ms; in multiples of 0.625ms.
RedBearLab 0:cffe8ac1bdf0 190 ble.setAdvertisingInterval(160);
RedBearLab 0:cffe8ac1bdf0 191
RedBearLab 0:cffe8ac1bdf0 192 ble.addService(uartService);
RedBearLab 0:cffe8ac1bdf0 193
RedBearLab 0:cffe8ac1bdf0 194 ble.startAdvertising();
RedBearLab 0:cffe8ac1bdf0 195 pc.printf("Advertising Start \r\n");
RedBearLab 0:cffe8ac1bdf0 196
elmbed 4:2b0947ed0e0d 197 char txbuff[40] = {0};
elmbed 4:2b0947ed0e0d 198
RedBearLab 0:cffe8ac1bdf0 199 while(1)
RedBearLab 0:cffe8ac1bdf0 200 {
elmbed 4:2b0947ed0e0d 201 //ble.waitForEvent();
elmbed 4:2b0947ed0e0d 202 if(new_strike){
elmbed 4:2b0947ed0e0d 203 if(mode == SPEW){
elmbed 4:2b0947ed0e0d 204 pc.printf("s%u\r\n", strike_value);
elmbed 4:2b0947ed0e0d 205 }
elmbed 4:2b0947ed0e0d 206 else{
elmbed 4:2b0947ed0e0d 207 snprintf(txbuff, sizeof(txbuff), "s%u\r\n", strike_value);
elmbed 4:2b0947ed0e0d 208 pc.printf("%s", txbuff);
elmbed 4:2b0947ed0e0d 209 ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)txbuff, strlen(txbuff));
elmbed 4:2b0947ed0e0d 210 new_strike = false;
elmbed 4:2b0947ed0e0d 211 }
elmbed 4:2b0947ed0e0d 212 }
elmbed 4:2b0947ed0e0d 213 }
elmbed 4:2b0947ed0e0d 214 }
elmbed 4:2b0947ed0e0d 215
elmbed 4:2b0947ed0e0d 216 void strike_detect1(){
elmbed 4:2b0947ed0e0d 217 //static int16_t xzy[3];
elmbed 4:2b0947ed0e0d 218 static Sensor* s = new Sensor();
elmbed 4:2b0947ed0e0d 219
elmbed 4:2b0947ed0e0d 220 LIS3DH_readAll(xzy);
elmbed 4:2b0947ed0e0d 221 //algorithm expects -/+8g, 0-4095, accelerometer reports ??? (+/-16g, 0-65535?)
elmbed 4:2b0947ed0e0d 222 int result = parseXY(xzy[0]/16 + 2048, xzy[2]/16 + 2048, s);
elmbed 4:2b0947ed0e0d 223 if(result){
elmbed 4:2b0947ed0e0d 224 //strike_timer.reset();
elmbed 4:2b0947ed0e0d 225 //strike_timer.start();
elmbed 4:2b0947ed0e0d 226 strike_detect_reset(s);
elmbed 4:2b0947ed0e0d 227 result = int((float)result*(10000.0/65535.0));
elmbed 4:2b0947ed0e0d 228 new_strike = true;
elmbed 4:2b0947ed0e0d 229 last_result = result;
elmbed 4:2b0947ed0e0d 230 strike_value = result;
elmbed 4:2b0947ed0e0d 231 }
elmbed 4:2b0947ed0e0d 232 }
elmbed 4:2b0947ed0e0d 233
elmbed 4:2b0947ed0e0d 234 void strike_detect2(){
elmbed 4:2b0947ed0e0d 235 static float low_pass1 = 0.0;
elmbed 4:2b0947ed0e0d 236 static float low_pass2 = 0.0;
elmbed 4:2b0947ed0e0d 237 float RMS = 0.0;
elmbed 4:2b0947ed0e0d 238 static float last_decay = 0.0;
elmbed 4:2b0947ed0e0d 239 float decay = 0.0;
elmbed 4:2b0947ed0e0d 240 static float accumulator = 0.0;
elmbed 4:2b0947ed0e0d 241 bool accumulating = false;
elmbed 4:2b0947ed0e0d 242 float jerk = 0.0;
elmbed 4:2b0947ed0e0d 243 static float shadow = 0.0;
elmbed 4:2b0947ed0e0d 244
elmbed 4:2b0947ed0e0d 245 LIS3DH_readAll(xzy);
elmbed 4:2b0947ed0e0d 246 float t1 = xzy[0];
elmbed 4:2b0947ed0e0d 247 float t2 = xzy[2];
elmbed 4:2b0947ed0e0d 248 RMS = sqrt(t1*t1 + t2*t2);
elmbed 4:2b0947ed0e0d 249
elmbed 4:2b0947ed0e0d 250 jerk = low_pass1;
elmbed 4:2b0947ed0e0d 251 low_pass1 = c1_1*RMS + c2_1*low_pass1;
elmbed 4:2b0947ed0e0d 252 low_pass2 = c1_2*RMS + c2_2*low_pass2;
elmbed 4:2b0947ed0e0d 253
elmbed 4:2b0947ed0e0d 254 jerk = low_pass1 - jerk;
elmbed 4:2b0947ed0e0d 255 if(jerk > jerk_threshold)accumulating = true;
elmbed 4:2b0947ed0e0d 256
elmbed 4:2b0947ed0e0d 257 decay = low_pass1 - low_pass2;
elmbed 4:2b0947ed0e0d 258 if(decay < 0) decay = 0;
elmbed 4:2b0947ed0e0d 259
elmbed 4:2b0947ed0e0d 260 shadow *= shadow_decay;
elmbed 4:2b0947ed0e0d 261
elmbed 4:2b0947ed0e0d 262 if(decay < 400){
elmbed 4:2b0947ed0e0d 263 strike_value = ((long)accumulator/30);
elmbed 4:2b0947ed0e0d 264 if(strike_value > small_hit_threshold && strike_value > shadow){
elmbed 4:2b0947ed0e0d 265 new_strike = true;
elmbed 4:2b0947ed0e0d 266 pc.printf("s%ld\r", (long)accumulator/30);
elmbed 4:2b0947ed0e0d 267 if(strike_value > big_hit_threshold) shadow = (float) strike_value;
elmbed 4:2b0947ed0e0d 268 }
elmbed 4:2b0947ed0e0d 269 accumulator = 0;
elmbed 4:2b0947ed0e0d 270 accumulating = false;
elmbed 4:2b0947ed0e0d 271 }
elmbed 4:2b0947ed0e0d 272 else if(accumulating)accumulator += decay;
elmbed 4:2b0947ed0e0d 273 last_decay = decay;
elmbed 4:2b0947ed0e0d 274 }
elmbed 4:2b0947ed0e0d 275
elmbed 4:2b0947ed0e0d 276 void spew_accel_old(){
elmbed 4:2b0947ed0e0d 277 static int counter = 0;
elmbed 4:2b0947ed0e0d 278 static float low_pass = 0.0;
elmbed 4:2b0947ed0e0d 279 float RMS = 0.0;
elmbed 4:2b0947ed0e0d 280 float delta = 0.2;
elmbed 4:2b0947ed0e0d 281 float decay = 0.0;
elmbed 4:2b0947ed0e0d 282
elmbed 4:2b0947ed0e0d 283 //static int16_t xzy[3];
elmbed 4:2b0947ed0e0d 284 LIS3DH_readAll(xzy);
elmbed 4:2b0947ed0e0d 285 RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
elmbed 4:2b0947ed0e0d 286 low_pass = delta*RMS + (1-delta)*low_pass;
elmbed 4:2b0947ed0e0d 287
elmbed 4:2b0947ed0e0d 288 if(RMS > decay)decay = low_pass;
elmbed 4:2b0947ed0e0d 289 decay = decay * 0.95;
elmbed 4:2b0947ed0e0d 290
elmbed 4:2b0947ed0e0d 291 //UART.printf("%d %d %d\r\n", last_result, xzy[0], xzy[2]); // should not be printing in interrupt, am going to hell
elmbed 4:2b0947ed0e0d 292 pc.printf("%f, %f, %f\r\n", RMS, low_pass, decay); // should not be printing in interrupt, am going to hell
elmbed 4:2b0947ed0e0d 293 if(counter > 15){
elmbed 4:2b0947ed0e0d 294 last_result = 0;
elmbed 4:2b0947ed0e0d 295 counter = 0;
elmbed 4:2b0947ed0e0d 296 }
elmbed 4:2b0947ed0e0d 297 if(last_result)counter++;
elmbed 4:2b0947ed0e0d 298 }
elmbed 4:2b0947ed0e0d 299 /*
elmbed 4:2b0947ed0e0d 300 void spew_accel(){
elmbed 4:2b0947ed0e0d 301 static float low_pass1 = 0.0;
elmbed 4:2b0947ed0e0d 302 static float low_pass2 = 0.0;
elmbed 4:2b0947ed0e0d 303 float RMS = 0.0;
elmbed 4:2b0947ed0e0d 304 static float last_decay = 0.0;
elmbed 4:2b0947ed0e0d 305 float decay = 0.0;
elmbed 4:2b0947ed0e0d 306 static float accumulator = 0.0;
elmbed 4:2b0947ed0e0d 307 bool accumulating = false;
elmbed 4:2b0947ed0e0d 308 float jerk = 0.0;
elmbed 4:2b0947ed0e0d 309 //static float biggest_jerk = 0.0;
elmbed 4:2b0947ed0e0d 310
elmbed 4:2b0947ed0e0d 311 //static int16_t xzy[3];
elmbed 4:2b0947ed0e0d 312 LIS3DH_readAll(xzy);
elmbed 4:2b0947ed0e0d 313 //RMS = (float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2];
elmbed 4:2b0947ed0e0d 314 float t1 = xzy[0];
elmbed 4:2b0947ed0e0d 315 float t2 = xzy[2];
elmbed 4:2b0947ed0e0d 316 RMS = sqrt(t1*t1 + t2*t2);
elmbed 4:2b0947ed0e0d 317 //RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]);
elmbed 4:2b0947ed0e0d 318 jerk = low_pass1;
elmbed 4:2b0947ed0e0d 319 low_pass1 = 0.35*RMS + 0.65*low_pass1;
elmbed 4:2b0947ed0e0d 320 low_pass2 = 0.025*RMS + 0.975*low_pass2;
elmbed 4:2b0947ed0e0d 321
elmbed 4:2b0947ed0e0d 322 jerk = low_pass1 - jerk;
elmbed 4:2b0947ed0e0d 323 if(jerk > 500)accumulating = true;
elmbed 4:2b0947ed0e0d 324 //if(jerk > biggest_jerk)biggest_jerk = jerk;
elmbed 4:2b0947ed0e0d 325
elmbed 4:2b0947ed0e0d 326 //if(RMS > decay)decay = RMS;
elmbed 4:2b0947ed0e0d 327 //decay = decay * 0.9999;
elmbed 4:2b0947ed0e0d 328 decay = low_pass1 - low_pass2;
elmbed 4:2b0947ed0e0d 329 if(decay < 0) decay = 0;
elmbed 4:2b0947ed0e0d 330
elmbed 4:2b0947ed0e0d 331 if(decay < 400){
elmbed 4:2b0947ed0e0d 332 if(accumulator > 10000)UART.printf("s%ld\r", (long)accumulator/30);
elmbed 4:2b0947ed0e0d 333 accumulator = 0;
elmbed 4:2b0947ed0e0d 334 accumulating = false;
elmbed 4:2b0947ed0e0d 335 //biggest_jerk = 0;
elmbed 4:2b0947ed0e0d 336 }
elmbed 4:2b0947ed0e0d 337 else if(accumulating)accumulator += decay;
elmbed 4:2b0947ed0e0d 338 last_decay = decay;
elmbed 4:2b0947ed0e0d 339
elmbed 4:2b0947ed0e0d 340 }*/
elmbed 4:2b0947ed0e0d 341
elmbed 4:2b0947ed0e0d 342 void getRadioInput(char *data, uint16_t length)
elmbed 4:2b0947ed0e0d 343 {
elmbed 4:2b0947ed0e0d 344 static int i = 0;
elmbed 4:2b0947ed0e0d 345 static char parameter = '_';
elmbed 4:2b0947ed0e0d 346 static char buffer[RADIO_MESSAGE_LEN + 1];
elmbed 4:2b0947ed0e0d 347 int value = 0;
elmbed 4:2b0947ed0e0d 348
elmbed 4:2b0947ed0e0d 349 // listen for commands coming over bluetooth
elmbed 4:2b0947ed0e0d 350 for (int j = 0; j < length; ++j){
elmbed 4:2b0947ed0e0d 351 char ch = data[j];
elmbed 4:2b0947ed0e0d 352
elmbed 4:2b0947ed0e0d 353 pc.printf("looping: %c. %c\r\n", ch, parameter);
elmbed 4:2b0947ed0e0d 354
elmbed 4:2b0947ed0e0d 355 // if ch is a message terminator, if parameter is anything other than '_', process the message
elmbed 4:2b0947ed0e0d 356 if((ch == '\r' || ch == ';' || ch == '\n'))
elmbed 4:2b0947ed0e0d 357 {
elmbed 4:2b0947ed0e0d 358 if(i > 0 && parameter != '_')
elmbed 4:2b0947ed0e0d 359 {
elmbed 4:2b0947ed0e0d 360 buffer[i-1] = 0;
elmbed 4:2b0947ed0e0d 361 value = atoi(buffer);
elmbed 4:2b0947ed0e0d 362
elmbed 4:2b0947ed0e0d 363 interpret(parameter, value);
elmbed 4:2b0947ed0e0d 364 }
elmbed 4:2b0947ed0e0d 365
elmbed 4:2b0947ed0e0d 366 // done sending, reset parameters
elmbed 4:2b0947ed0e0d 367 parameter = '_';
elmbed 4:2b0947ed0e0d 368 buffer[0] = 0;
elmbed 4:2b0947ed0e0d 369 i=0;
elmbed 4:2b0947ed0e0d 370 }
elmbed 4:2b0947ed0e0d 371 else
elmbed 4:2b0947ed0e0d 372 { // got anything but a message terminator
elmbed 4:2b0947ed0e0d 373 if(i==0)
elmbed 4:2b0947ed0e0d 374 parameter = ch; // nothing accumulated yet
elmbed 4:2b0947ed0e0d 375 else
elmbed 4:2b0947ed0e0d 376 buffer[i-1] = ch;
elmbed 4:2b0947ed0e0d 377
elmbed 4:2b0947ed0e0d 378 i++;
elmbed 4:2b0947ed0e0d 379 }
elmbed 4:2b0947ed0e0d 380 //UART.printf("%c: %s, %d\r\n",parameter, buffer, i);
elmbed 4:2b0947ed0e0d 381 }
elmbed 4:2b0947ed0e0d 382 }
elmbed 4:2b0947ed0e0d 383
elmbed 4:2b0947ed0e0d 384 void interpret(char parameter, int value){
elmbed 4:2b0947ed0e0d 385
elmbed 4:2b0947ed0e0d 386 switch(parameter){
elmbed 4:2b0947ed0e0d 387 case 'g':
elmbed 4:2b0947ed0e0d 388 if(value == 1){
elmbed 4:2b0947ed0e0d 389 new_strike = false;
elmbed 4:2b0947ed0e0d 390 //active = true;
elmbed 4:2b0947ed0e0d 391 if(mode == STRIKE_DETECT_1)measure.attach_us(&strike_detect1, timestep*1000);
elmbed 4:2b0947ed0e0d 392 if(mode == STRIKE_DETECT_2)measure.attach_us(&strike_detect2, timestep*1000);
elmbed 4:2b0947ed0e0d 393 }
elmbed 4:2b0947ed0e0d 394 if(value == 0)measure.detach(); //active = false;
elmbed 4:2b0947ed0e0d 395 break;
elmbed 4:2b0947ed0e0d 396 case 'm':
elmbed 4:2b0947ed0e0d 397 mode = value;
elmbed 4:2b0947ed0e0d 398 break;
elmbed 4:2b0947ed0e0d 399 case 'p':
elmbed 4:2b0947ed0e0d 400 config_parameter = value;
elmbed 4:2b0947ed0e0d 401 //UART.printf("parameter is: %d\r\n", value);
elmbed 4:2b0947ed0e0d 402 break;
elmbed 4:2b0947ed0e0d 403 case 'v':
elmbed 4:2b0947ed0e0d 404 if(config_parameter < 8)set_setting(config_parameter, value);
elmbed 4:2b0947ed0e0d 405 else{
elmbed 4:2b0947ed0e0d 406 //UART.printf("value is: %d\r\n", value);
elmbed 4:2b0947ed0e0d 407 if(config_parameter == 101){
elmbed 4:2b0947ed0e0d 408 //fast_filter = value;
elmbed 4:2b0947ed0e0d 409 c1_1 = (float)value/1000.0;
elmbed 4:2b0947ed0e0d 410 c2_1 = 1.0 - c1_1;
elmbed 4:2b0947ed0e0d 411 }
elmbed 4:2b0947ed0e0d 412 if(config_parameter == 102){
elmbed 4:2b0947ed0e0d 413 //slow_filter = value;
elmbed 4:2b0947ed0e0d 414 c1_2 = (float)value/1000.0;
elmbed 4:2b0947ed0e0d 415 c2_2 = 1.0 - c1_2;
elmbed 4:2b0947ed0e0d 416 }
elmbed 4:2b0947ed0e0d 417 if(config_parameter == 103)big_hit_threshold = value;
elmbed 4:2b0947ed0e0d 418 if(config_parameter == 104)small_hit_threshold = value;
elmbed 4:2b0947ed0e0d 419 if(config_parameter == 105)jerk_threshold = value;
elmbed 4:2b0947ed0e0d 420 if(config_parameter == 106){
elmbed 4:2b0947ed0e0d 421 float x = value;
elmbed 4:2b0947ed0e0d 422 shadow_decay = 0.9 + x*0.0001;
elmbed 4:2b0947ed0e0d 423 //UART.printf("shadow_decay is: %f\r\n", shadow_decay);
elmbed 4:2b0947ed0e0d 424 }
elmbed 4:2b0947ed0e0d 425 }
elmbed 4:2b0947ed0e0d 426 break;
elmbed 4:2b0947ed0e0d 427 case 's':
elmbed 4:2b0947ed0e0d 428 if(value > 0){
elmbed 4:2b0947ed0e0d 429 //measure.attach_us(&spew_accel, 1000);
elmbed 4:2b0947ed0e0d 430 //spew.attach(&spew_accel, float(value)/1000);
elmbed 4:2b0947ed0e0d 431 //measure.attach_us(&read_accel, 1000);
elmbed 4:2b0947ed0e0d 432 }
elmbed 4:2b0947ed0e0d 433 if(value == 0)spew.detach(); //active = false;
elmbed 4:2b0947ed0e0d 434 break;
elmbed 4:2b0947ed0e0d 435 case 't':
elmbed 4:2b0947ed0e0d 436 //y(k) = a * y(k-1) + (1-a) * x(k)
elmbed 4:2b0947ed0e0d 437 //a = exp (-T/tau)
elmbed 4:2b0947ed0e0d 438 break;
elmbed 4:2b0947ed0e0d 439 case 'z':
elmbed 4:2b0947ed0e0d 440 pc.printf(VERSION);
elmbed 4:2b0947ed0e0d 441 break;
elmbed 4:2b0947ed0e0d 442 default:
elmbed 4:2b0947ed0e0d 443 // print stuff
elmbed 4:2b0947ed0e0d 444 break;
RedBearLab 0:cffe8ac1bdf0 445 }
RedBearLab 0:cffe8ac1bdf0 446 }
RedBearLab 0:cffe8ac1bdf0 447
RedBearLab 0:cffe8ac1bdf0 448
RedBearLab 0:cffe8ac1bdf0 449
RedBearLab 0:cffe8ac1bdf0 450
RedBearLab 0:cffe8ac1bdf0 451
RedBearLab 0:cffe8ac1bdf0 452
RedBearLab 0:cffe8ac1bdf0 453
RedBearLab 0:cffe8ac1bdf0 454
RedBearLab 0:cffe8ac1bdf0 455
RedBearLab 0:cffe8ac1bdf0 456
RedBearLab 0:cffe8ac1bdf0 457
RedBearLab 0:cffe8ac1bdf0 458
RedBearLab 0:cffe8ac1bdf0 459
RedBearLab 0:cffe8ac1bdf0 460
RedBearLab 0:cffe8ac1bdf0 461
RedBearLab 0:cffe8ac1bdf0 462