Working code with BLE and accelerometer.(DFU pending)
Dependencies: BLE_API mbed nRF51822 strike_detect
Fork of TenCount_BLE by
Diff: main.cpp
- Revision:
- 4:2b0947ed0e0d
- Parent:
- 3:b3f6c612b603
- Child:
- 5:7c5630442028
--- a/main.cpp Thu Jan 07 02:46:54 2016 +0000 +++ b/main.cpp Sun Apr 24 15:02:56 2016 +0000 @@ -26,7 +26,9 @@ #include "mbed.h" #include "ble/BLE.h" - +#include "LIS3DH.h" +#include "strike.h" +#include "Sensor.h" #define BLE_UUID_TXRX_SERVICE 0x0000 /**< The UUID of the Nordic UART Service. */ #define BLE_UUID_TX_CHARACTERISTIC 0x0002 /**< The UUID of the TX Characteristic. */ @@ -34,6 +36,30 @@ #define TXRX_BUF_LEN 20 + +//you can receive less then 93 bytes at 921600 Bd +#define UART_SENSOR_COMMAND_BUFFER_SIZE (100) +#define SYSTEM_COM_BUFFER (1024) +#define RADIO_MESSAGE_LEN (64) +#define VERSION "alpha_20150409-01" +#define SPEW (0) +#define STRIKE_DETECT_1 (1) +#define STRIKE_DETECT_2 (2) + +#define DEFAULT_TIMESTEP (10) // in milliseconds +//#define DEFAULT_FAST_FILTER (650) +//#define DEFAULT_SLOW_FILTER (975) +#define DEFAULT_BIG_HIT_THRESHOLD (3000) +#define DEFAULT_SMALL_HIT_THRESHOLD (200) +#define DEFAULT_JERK_THRESHOLD (500) + +#define DEFAULT_C1_1 (0.35) +#define DEFAULT_C2_1 (0.65) +#define DEFAULT_C1_2 (0.025) +#define DEFAULT_C2_2 (0.975) + +#define DEFAULT_SHADOW_DECAY (0.8) + BLE ble; Serial pc(USBTX, USBRX); @@ -63,6 +89,36 @@ +void interpret(char parameter, int value); +void getRadioInput(char *data, uint16_t size); +void read_accel(); + +Ticker measure; +Ticker spew; +uint16_t strike_value = 0; +int last_result = 0; +bool new_strike = false; +bool active = false; +static volatile int config_parameter = 0; + +static volatile int mode = STRIKE_DETECT_2; +static volatile int timestep = DEFAULT_TIMESTEP; +//static volatile int fast_filter = DEFAULT_FAST_FILTER; +//static volatile int slow_filter = DEFAULT_SLOW_FILTER; +static volatile int big_hit_threshold = DEFAULT_BIG_HIT_THRESHOLD; +static volatile int small_hit_threshold = DEFAULT_SMALL_HIT_THRESHOLD; +static volatile int jerk_threshold = DEFAULT_JERK_THRESHOLD; + +static volatile float c1_1 = DEFAULT_C1_1; +static volatile float c2_1 = DEFAULT_C2_1; +static volatile float c1_2 = DEFAULT_C1_2; +static volatile float c2_2 = DEFAULT_C2_2; + +static volatile float shadow_decay = DEFAULT_SHADOW_DECAY; + +static int16_t xzy[3]; + + void disconnectionCallback(const Gap::DisconnectionCallbackParams_t *params) { pc.printf("Disconnected \r\n"); @@ -81,15 +137,15 @@ memset(txPayload, 0, TXRX_BUF_LEN); memcpy(txPayload, buf, TXRX_BUF_LEN); pc.printf("WriteHandler \r\n"); - pc.printf("Length: "); - pc.putc(bytesRead); - pc.printf("\r\n"); + pc.printf("Length: %d\r\n", bytesRead); pc.printf("Data: "); for(index=0; index<bytesRead; index++) { pc.putc(txPayload[index]); } pc.printf("\r\n"); + + getRadioInput((char*)txPayload, bytesRead); } } @@ -118,14 +174,16 @@ ble.onDataWritten(WrittenHandler); pc.baud(9600); - pc.printf("SimpleChat Init \r\n"); + pc.printf("TenCount \r\n"); + + LIS3DH_init(); pc.attach( uartCB , pc.RxIrq); // setup advertising ble.accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED); ble.setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); ble.accumulateAdvertisingPayload(GapAdvertisingData::SHORTENED_LOCAL_NAME, - (const uint8_t *)"Biscuit", sizeof("Biscuit") - 1); + (const uint8_t *)"TenCount", sizeof("TenCount") - 1); ble.accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_128BIT_SERVICE_IDS, (const uint8_t *)uart_base_uuid_rev, sizeof(uart_base_uuid)); // 100ms; in multiples of 0.625ms. @@ -136,9 +194,254 @@ ble.startAdvertising(); pc.printf("Advertising Start \r\n"); + char txbuff[40] = {0}; + while(1) { - ble.waitForEvent(); + //ble.waitForEvent(); + if(new_strike){ + if(mode == SPEW){ + pc.printf("s%u\r\n", strike_value); + } + else{ + snprintf(txbuff, sizeof(txbuff), "s%u\r\n", strike_value); + pc.printf("%s", txbuff); + ble.updateCharacteristicValue(rxCharacteristic.getValueAttribute().getHandle(), (uint8_t*)txbuff, strlen(txbuff)); + new_strike = false; + } + } + } +} + +void strike_detect1(){ + //static int16_t xzy[3]; + static Sensor* s = new Sensor(); + + LIS3DH_readAll(xzy); + //algorithm expects -/+8g, 0-4095, accelerometer reports ??? (+/-16g, 0-65535?) + int result = parseXY(xzy[0]/16 + 2048, xzy[2]/16 + 2048, s); + if(result){ + //strike_timer.reset(); + //strike_timer.start(); + strike_detect_reset(s); + result = int((float)result*(10000.0/65535.0)); + new_strike = true; + last_result = result; + strike_value = result; + } +} + +void strike_detect2(){ + static float low_pass1 = 0.0; + static float low_pass2 = 0.0; + float RMS = 0.0; + static float last_decay = 0.0; + float decay = 0.0; + static float accumulator = 0.0; + bool accumulating = false; + float jerk = 0.0; + static float shadow = 0.0; + + LIS3DH_readAll(xzy); + float t1 = xzy[0]; + float t2 = xzy[2]; + RMS = sqrt(t1*t1 + t2*t2); + + jerk = low_pass1; + low_pass1 = c1_1*RMS + c2_1*low_pass1; + low_pass2 = c1_2*RMS + c2_2*low_pass2; + + jerk = low_pass1 - jerk; + if(jerk > jerk_threshold)accumulating = true; + + decay = low_pass1 - low_pass2; + if(decay < 0) decay = 0; + + shadow *= shadow_decay; + + if(decay < 400){ + strike_value = ((long)accumulator/30); + if(strike_value > small_hit_threshold && strike_value > shadow){ + new_strike = true; + pc.printf("s%ld\r", (long)accumulator/30); + if(strike_value > big_hit_threshold) shadow = (float) strike_value; + } + accumulator = 0; + accumulating = false; + } + else if(accumulating)accumulator += decay; + last_decay = decay; +} + +void spew_accel_old(){ + static int counter = 0; + static float low_pass = 0.0; + float RMS = 0.0; + float delta = 0.2; + float decay = 0.0; + + //static int16_t xzy[3]; + LIS3DH_readAll(xzy); + RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]); + low_pass = delta*RMS + (1-delta)*low_pass; + + if(RMS > decay)decay = low_pass; + decay = decay * 0.95; + + //UART.printf("%d %d %d\r\n", last_result, xzy[0], xzy[2]); // should not be printing in interrupt, am going to hell + pc.printf("%f, %f, %f\r\n", RMS, low_pass, decay); // should not be printing in interrupt, am going to hell + if(counter > 15){ + last_result = 0; + counter = 0; + } + if(last_result)counter++; +} +/* +void spew_accel(){ + static float low_pass1 = 0.0; + static float low_pass2 = 0.0; + float RMS = 0.0; + static float last_decay = 0.0; + float decay = 0.0; + static float accumulator = 0.0; + bool accumulating = false; + float jerk = 0.0; + //static float biggest_jerk = 0.0; + + //static int16_t xzy[3]; + LIS3DH_readAll(xzy); + //RMS = (float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]; + float t1 = xzy[0]; + float t2 = xzy[2]; + RMS = sqrt(t1*t1 + t2*t2); + //RMS = sqrt((float)xzy[0]*(float)xzy[0] + (float)xzy[2]*(float)xzy[2]); + jerk = low_pass1; + low_pass1 = 0.35*RMS + 0.65*low_pass1; + low_pass2 = 0.025*RMS + 0.975*low_pass2; + + jerk = low_pass1 - jerk; + if(jerk > 500)accumulating = true; + //if(jerk > biggest_jerk)biggest_jerk = jerk; + + //if(RMS > decay)decay = RMS; + //decay = decay * 0.9999; + decay = low_pass1 - low_pass2; + if(decay < 0) decay = 0; + + if(decay < 400){ + if(accumulator > 10000)UART.printf("s%ld\r", (long)accumulator/30); + accumulator = 0; + accumulating = false; + //biggest_jerk = 0; + } + else if(accumulating)accumulator += decay; + last_decay = decay; + +}*/ + +void getRadioInput(char *data, uint16_t length) +{ + static int i = 0; + static char parameter = '_'; + static char buffer[RADIO_MESSAGE_LEN + 1]; + int value = 0; + + // listen for commands coming over bluetooth + for (int j = 0; j < length; ++j){ + char ch = data[j]; + + pc.printf("looping: %c. %c\r\n", ch, parameter); + + // if ch is a message terminator, if parameter is anything other than '_', process the message + if((ch == '\r' || ch == ';' || ch == '\n')) + { + if(i > 0 && parameter != '_') + { + buffer[i-1] = 0; + value = atoi(buffer); + + interpret(parameter, value); + } + + // done sending, reset parameters + parameter = '_'; + buffer[0] = 0; + i=0; + } + else + { // got anything but a message terminator + if(i==0) + parameter = ch; // nothing accumulated yet + else + buffer[i-1] = ch; + + i++; + } + //UART.printf("%c: %s, %d\r\n",parameter, buffer, i); + } +} + +void interpret(char parameter, int value){ + + switch(parameter){ + case 'g': + if(value == 1){ + new_strike = false; + //active = true; + if(mode == STRIKE_DETECT_1)measure.attach_us(&strike_detect1, timestep*1000); + if(mode == STRIKE_DETECT_2)measure.attach_us(&strike_detect2, timestep*1000); + } + if(value == 0)measure.detach(); //active = false; + break; + case 'm': + mode = value; + break; + case 'p': + config_parameter = value; + //UART.printf("parameter is: %d\r\n", value); + break; + case 'v': + if(config_parameter < 8)set_setting(config_parameter, value); + else{ + //UART.printf("value is: %d\r\n", value); + if(config_parameter == 101){ + //fast_filter = value; + c1_1 = (float)value/1000.0; + c2_1 = 1.0 - c1_1; + } + if(config_parameter == 102){ + //slow_filter = value; + c1_2 = (float)value/1000.0; + c2_2 = 1.0 - c1_2; + } + if(config_parameter == 103)big_hit_threshold = value; + if(config_parameter == 104)small_hit_threshold = value; + if(config_parameter == 105)jerk_threshold = value; + if(config_parameter == 106){ + float x = value; + shadow_decay = 0.9 + x*0.0001; + //UART.printf("shadow_decay is: %f\r\n", shadow_decay); + } + } + break; + case 's': + if(value > 0){ + //measure.attach_us(&spew_accel, 1000); + //spew.attach(&spew_accel, float(value)/1000); + //measure.attach_us(&read_accel, 1000); + } + if(value == 0)spew.detach(); //active = false; + break; + case 't': + //y(k) = a * y(k-1) + (1-a) * x(k) + //a = exp (-T/tau) + break; + case 'z': + pc.printf(VERSION); + break; + default: + // print stuff + break; } }