Masaya Shimoyamada
reduced message bytes for BLE
Dependencies: BLE_API mbed nRF51822
Fork of
Inp_Fiber_Logo_FOTA
by 
inupathy
main.cpp
- Committer:
- ayaando
- Date:
- 2017-04-26
- Revision:
- 2:221df6d014f5
- Parent:
- 1:f28bdfaf3005
- Child:
- 3:ab649240543c
File content as of revision 2:221df6d014f5:
#include "mbed.h"
#include "neopixel.h"
#include "nrf_soc.h"
#include "common.h"
#include "ble/BLE.h"
#include "ble/services/HeartRateService.h"
#include "ble/services/BatteryService.h"
#include "ble/services/DeviceInformationService.h"
#include "UARTService.h"
#include "ble_gap.h"
#include "ble/services/DFUService.h" // ★OTA用
#define RAINBOW_RATIO 1
#define RCFILTER 0.9
#define RCFILTER2 0.9
#define MAIN_NEOPIXEL_COUNT 6
#define MIN_LIGHT 0
#define PNNX_FACTOR 0.1 // 0.06
#define MAXARRAY 5 //15 // 心拍平均値計算用の配列サイズ //20
#define MAXHRVARRAY 1 //30 // 脈間ゆらぎ平均値計算用の配列サイズ
#define MIN_INTERVAL 200
#define INTERVAL 10 // Loopの間隔 通常時推奨1 通信時推奨２０
#define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7)
#define MAXCONARRAY 10
#define MAXSTRESSARRAY 3
#define DIM_LED 1 //0.2
//#define DIM_LED 1.0
// 心拍数の最低値、最高値
// 人間の場合は60～150、標準80
// 犬の場合は40～200、標準60
#define HR_MIN 40
#define HR_MAX 150
#define RELAX_RATIO_MIN 0.6
// ラジアンを度に変換する．
#define RadianToDegree(radian) ((180 / 3.14159) * (radian))
// 度をラジアンに変換する．
#define DegreeToRadian(degree) ((3.14159 / 180) * (degree))
// 色の識別値設定（ピンではない）
#define RED 0
#define BLUE 1
#define GREEN 2
#define SHOW_EXCITE 1
#define SHOW_HAPPY 2
#define SHOW_CONCENTRATION 4
int displayFlag = 0;
BLE ble;
DFUService *dfuService; // ★OTA用
AnalogIn HRSigIn(P0_4);
//AnalogIn HRSigIn(P0_4);
//PwmOut RedPin1(D0);
//PwmOut GreenPin1(D1);
//PwmOut BluePin1(D2);
//PwmOut RedPin2(D3);
//PwmOut GreenPin2(D5);
//PwmOut BluePin2(D4);
//DigitalOut PwrPin(D3);
//const static char DEVICE_NAME[] = "InupLogo";
static char DEVICE_NAME[16] = "IND";
static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE,
GattService::UUID_DEVICE_INFORMATION_SERVICE};
static volatile bool triggerSensorPolling = true;
short MainNeopixelColorArrayR[MAIN_NEOPIXEL_COUNT];
short MainNeopixelColorArrayG[MAIN_NEOPIXEL_COUNT];
short MainNeopixelColorArrayB[MAIN_NEOPIXEL_COUNT];
short ColorArrayR[MAIN_NEOPIXEL_COUNT];
short ColorArrayG[MAIN_NEOPIXEL_COUNT];
short ColorArrayB[MAIN_NEOPIXEL_COUNT];
short prevColorArrayR[MAIN_NEOPIXEL_COUNT];
short prevColorArrayG[MAIN_NEOPIXEL_COUNT];
short prevColorArrayB[MAIN_NEOPIXEL_COUNT];
/*
unsigned char MainNeopixelColorArrayR[MAIN_NEOPIXEL_COUNT];
unsigned char MainNeopixelColorArrayG[MAIN_NEOPIXEL_COUNT];
unsigned char MainNeopixelColorArrayB[MAIN_NEOPIXEL_COUNT];
unsigned char ColorArrayR[MAIN_NEOPIXEL_COUNT];
unsigned char ColorArrayG[MAIN_NEOPIXEL_COUNT];
unsigned char ColorArrayB[MAIN_NEOPIXEL_COUNT];
unsigned char prevColorArrayR[MAIN_NEOPIXEL_COUNT];
unsigned char prevColorArrayG[MAIN_NEOPIXEL_COUNT];
unsigned char prevColorArrayB[MAIN_NEOPIXEL_COUNT];
*/
//short TempColorArrayR[MAIN_NEOPIXEL_COUNT];
//short TempColorArrayG[MAIN_NEOPIXEL_COUNT];
//short TempColorArrayB[MAIN_NEOPIXEL_COUNT];
//float MainNeopixelColorArrayRatio[MAIN_NEOPIXEL_COUNT];
#define NEOPIXEL_COMMANDLEN ((MAIN_NEOPIXEL_COUNT * 3) + 1 + 1)
unsigned long pixelStartCounter;
int mainCurrentPixel = 0;
int mainPrevPixel = 0;
int mainPixelLightCount;
int mainPixelStep = 0;
int mainStepsRemain = 0;
unsigned long currentmillis = 0;
unsigned long prevmillis = 0;
unsigned long intervalmillis = 0;
unsigned long prevIntervalmillis = 0;
unsigned long intervalmillisArray[MAXARRAY] = {1000};
unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1};
float conArray[MAXCONARRAY] = {0.5};
//float stressArray[MAXSTRESSARRAY] = {0.5};
float prevFilteredAngle = 0.5;
float happyRatio = 0.0;
float prevRelaxRatio = 1.0;
float prevFilteredDistRatio = 0.0;
/*
float avrgClrR[MAXCOLORARRAY];
float avrgClrG[MAXCOLORARRAY];
float avrgClrB[MAXCOLORARRAY];
//int avrgClrP[MAXCOLORARRAY];
*/
/*
#define LED_COLOR_VARIATION 25
int LEDColorArrayB[] = {255, 225, 205, 185, 165, 145, 125, 105, 85, 65, 45, 25, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
int LEDColorArrayG[] = { 0, 20, 40, 60, 80, 100, 120, 140, 160, 180, 200, 220, 255, 225, 205, 185, 165, 145, 125, 105, 85, 65, 45, 25, 0 };
int LEDColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 20, 40, 60, 80, 100, 120, 140, 160, 180, 200, 220, 255 };
*/
#define LED_COLOR_VARIATION 21
float LEDColorArrayB[] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float LEDColorArrayG[] = { 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0 };
float LEDColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
/*
#define LED_COLOR_VARIATION 41
float LEDColorArrayB[] = {1.0, 0.95, 0.9, 0.85, 0.8, 0.75, 0.7, 0.65, 0.6, 0.55, 0.5, 0.45, 0.4, 0.35, 0.3, 0.25, 0.2, 0.15, 0.1, 0.05, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float LEDColorArrayG[] = { 0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0, 0.95, 0.9, 0.85, 0.8, 0.75, 0.7, 0.65, 0.6, 0.55, 0.5, 0.45, 0.4, 0.35, 0.3, 0.25, 0.2, 0.15, 0.1, 0.05, 0 };
float LEDColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0 };
*/
/*
#define CONCENTRATION_COLOR_VARIATION 35
float ConColorArrayB[] = {1.0, 0.9, 0.8, 0.75, 0.7, 0.65, 0.6, 0.55, 0.5, 0.45, 0.4, 0.35, 0.3, 0.25, 0.2, 0.15, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float ConColorArrayG[] = { 0, 0.1, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.85, 0.9, 0.95, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
float ConColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.05, 0.1, 0.15, 0.2, 0.25, 0.3, 0.35, 0.4, 0.45, 0.5, 0.55, 0.6, 0.65, 0.7, 0.75, 0.8, 0.9, 1.0 };
*/
#define CONCENTRATION_COLOR_VARIATION 21
float ConColorArrayB[] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float ConColorArrayG[] = { 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
float ConColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
/*
#define HAPPY_COLOR_VARIATION 21
float HappyColorArrayB[] = {1.0, 0.9, 0.8, 0.7, 0.6, 0.5, 0.4, 0.3, 0.2, 0.1, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 };
float HappyColorArrayG[] = { 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0 };
float HappyColorArrayR[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 0.9, 1.0 };
#define LED_BRIGHTNESS_VARIATION 6
float LEDBrightnessArray[] = {0, 0, 0, 0, 1,1};
*/
#if 1
#define RAINBOW_COLOR_VARIATION 6
//unsigned char RainbowR[] = { 255, 255, 255, 0, 0, 75, 238};
//unsigned char RainbowG[] = { 0, 165, 255, 128, 0, 0, 130};
//unsigned char RainbowB[] = { 0, 0, 0, 0, 255, 130, 238};
unsigned char RainbowR[] = { 255, 0, 0, 0, 0, 0};
unsigned char RainbowG[] = { 0, 0, 255, 0, 0, 0};
unsigned char RainbowB[] = { 0, 0, 0, 0, 255, 0};
//#define RAINBOW_COLOR_VARIATION 14
//unsigned char RainbowR[] = {255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 75, 75, 238, 238};
//unsigned char RainbowG[] = { 0, 0, 165, 165, 255, 255, 128, 128, 0, 0, 0, 0, 130, 130};
//unsigned char RainbowB[] = { 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 130, 130, 238, 238};
#else
//#define RAINBOW_COLOR_VARIATION 21
//unsigned char RainbowR[] = {255, 255, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 75, 75, 75, 238, 238, 238};
//unsigned char RainbowG[] = { 0, 0, 0, 165, 165, 165, 255, 255, 255, 128, 128, 128, 0, 0, 0, 0, 0, 0, 130, 130, 130} ;
//unsigned char RainbowB[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 130, 130, 130, 238, 238, 238};
//#define RAINBOW_COLOR_VARIATION 21
//unsigned char RainbowR[] = {255, 191, 63, 0, 0, 0, 0, 63, 191};
//unsigned char RainbowG[] = { 0, 63, 191, 255, 191, 63, 0, 0, 0} ;
//unsigned char RainbowB[] = { 0, 0, 0, 0, 63, 191, 255, 191, 63};
//unsigned char RainbowR[] = {255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
//unsigned char RainbowG[] = { 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0} ;
//unsigned char RainbowB[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255};
//#define RAINBOW_COLOR_VARIATION 15
//unsigned char RainbowR[] = {255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
//unsigned char RainbowG[] = { 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0} ;
//unsigned char RainbowB[] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255};
#define RAINBOW_COLOR_VARIATION 18
unsigned char RainbowR[] = {255, 255, 255, 0, 0, 0, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0};
unsigned char RainbowG[] = { 0, 0, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 255, 255, 255, 0, 0, 0};
unsigned char RainbowB[] = { 0, 0, 255, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255};
//#define RAINBOW_COLOR_VARIATION 6
//unsigned char RainbowR[] = {255, 0, 0, 255, 255, 255};
//unsigned char RainbowG[] = { 0, 255, 255, 255, 0, 0};
//unsigned char RainbowB[] = { 0, 255, 0, 0, 0, 255};
//#define RAINBOW_COLOR_VARIATION 13
//unsigned short RainbowR[] = {1023, 255, 255, 255, 255, 0, 0, 0, 0, 0, 0, 255, 255};
//unsigned short RainbowG[] = {1023, 0, 0, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0} ;
//unsigned short RainbowB[] = {1023, 0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255};
#endif
float rainbowOffset = 0.0;
bool bConcentration;
unsigned long loopCounter = 0;
int lightPos = 1;
int prevLightPos = 1;
double relaxRatio = 0.0;
float conRatio = 0.5;
float conRatioNow = 0.5;
double HR_ratio = 0.0;
unsigned long IBI;
int x3, y3;
int mainPixelCounter;
int prevInterval;
int prevprevInterval;
int read_battery_level(void)
{
uint16_t value;
float fValue;
uint32_t config = NRF_ADC->CONFIG;
NRF_ADC->ENABLE = ADC_ENABLE_ENABLE_Enabled;
NRF_ADC->CONFIG = (ADC_CONFIG_RES_10bit << ADC_CONFIG_RES_Pos) |
(ADC_CONFIG_INPSEL_SupplyOneThirdPrescaling << ADC_CONFIG_INPSEL_Pos) |
(ADC_CONFIG_REFSEL_VBG << ADC_CONFIG_REFSEL_Pos) |
(ADC_CONFIG_PSEL_Disabled << ADC_CONFIG_PSEL_Pos) |
(ADC_CONFIG_EXTREFSEL_None << ADC_CONFIG_EXTREFSEL_Pos);
NRF_ADC->CONFIG &= ~ADC_CONFIG_PSEL_Msk;
NRF_ADC->CONFIG |= ADC_CONFIG_PSEL_Disabled << ADC_CONFIG_PSEL_Pos;
NRF_ADC->TASKS_START = 1;
while (((NRF_ADC->BUSY & ADC_BUSY_BUSY_Msk) >> ADC_BUSY_BUSY_Pos) == ADC_BUSY_BUSY_Busy) {};
value = (uint16_t)NRF_ADC->RESULT; // 10 bit
// fValue = (value * 3.3) / 1024.0;
float ref = (4.0 - 3.0) / 4.0;
ref = 1023.0 * ref;
fValue = (1023.0 - value);
fValue = ref - fValue ;
if(fValue < 0.0) {
fValue = 0.0;
}
fValue = fValue / ref;
// fValue = fValue - ref;
if(fValue > 1.0) {
fValue = 1.0;
}
// 3.1v is the threshold for battery off
NRF_ADC->CONFIG = config;
return (int)floor(fValue * 100.0);
}
void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason)
{
ble.gap().startAdvertising(); // restart advertising
}
/*
void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback)
{
ble.gap().startAdvertising(); // restart advertising
}
*/
uint8_t BPM = 0;
float sensorValue = 0;
Timer t;
Timer tRand;
float redVal = 0;
float blueVal = 255;
float greenVal = 0;
Ticker neopixelTick;
UARTService *uartServicePtr;
void neopixelTick_Handler() {
return;
}
bool RadioOn = false;
void radioNotificationHandler(bool radio_active) {
RadioOn = radio_active;
}
//Serial mySerial(USBTX, USBRX);
//Serial mySerial(P0_9, P0_11);
void getExciteColor(int bpm, int &rVal, int &gVal, int &bVal)
{
int i, j;
float *pColorArrayR;
float *pColorArrayG;
float *pColorArrayB;
double dbThresholdWidth;
// BPMがどの色領域に入るか判定する。
// MAX - MIN
dbThresholdWidth = (double)((HR_MAX - HR_MIN) / (LED_COLOR_VARIATION - 2));
for(j = 0; j < (LED_COLOR_VARIATION); j++)
{
if(bpm <= HR_MIN + dbThresholdWidth * j)
{
break;
}
}
if(j >= (LED_COLOR_VARIATION))
{
j = (LED_COLOR_VARIATION) - 1;
}
if(j < 0){
j = 0;
}
if(j > ((LED_COLOR_VARIATION) - 1)){
j = ((LED_COLOR_VARIATION) - 1);
}
pColorArrayR = LEDColorArrayR;
pColorArrayG = LEDColorArrayG;
pColorArrayB = LEDColorArrayB;
rVal = 255 * pColorArrayR[j];
gVal = 255 * pColorArrayG[j];
bVal = 255 * pColorArrayB[j];
if(rVal > 255)
{
rVal = 255;
}
else if(rVal < 0){
rVal = 0;
}
if(gVal > 255)
{
gVal = 255;
}
else if(gVal < 0){
gVal = 0;
}
if(bVal > 255)
{
bVal = 255;
}
else if(bVal < 0){
bVal = 0;
}
}
// MACアドレスを取得してBLEのデバイス名に組み込む
void setBleDeviceName(char *deviceName)
{
int i;
//BLEProtocol::AddressType_t type; //< The type of the BLE address.
//BLEProtocol::AddressBytes_t mac_address; //< The BLE address. // typedef uint8_t AddressBytes_t[ADDR_LEN]
ble_gap_addr_t mac_address;
if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){
return;
}
// ADDR_LENは6
for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){
sprintf(deviceName, "%s%02x", deviceName, (mac_address.addr)[i]);
}
sprintf(deviceName, "%s%s", deviceName, "\0");
/*
if(ble.getAddress(&type, mac_address) == BLE_ERROR_NONE){
// ADDR_LENは6
for(i=BLEProtocol::ADDR_LEN-1; i>=0; i--){
sprintf(deviceName, "%s%02x", deviceName, mac_address[i]);
}
sprintf(deviceName, "%s%s", deviceName, "\0");
}
*/
}
int main(void)
{
char msgIBI[100];
int nMsgIBILen = 0;
char msgHappy[10];
int nMsgHappyLen = 0;
char msgConcentration[10];
int nMsgConcentrationLen = 0;
char msgRelax[10];
int nMsgRelaxLen = 0;
char msg[256];
int nMsgLen = 0;
float Brightness = 1.0;
bool brighten = true;
float brightenSpeedMs = 1000.0;
float colorSpeedMs = 1000.0;
float conRatioShow = 0.5;
int i;
unsigned long lightPrevMillis = 0;
int currentBPM;
float fBeatIndex = 0.0;
// displayFlag = SHOW_EXCITE;
displayFlag = SHOW_EXCITE | SHOW_HAPPY | SHOW_CONCENTRATION;
//displayFlag = SHOW_HAPPY ;
// mySerial.baud(9600);
// mySerial.format(8, Serial::None, 1);
// PwrPin = 1;
static uint8_t rrIntervalH = 0;
static uint8_t rrIntervalL = 1;
// static uint8_t bpm[8] = {0x10, hrmCounter, 0, 0, 0, 0, rrIntervalH, rrIntervalL};
neopixel_strip_t neopixel1;
neopixel_init(&neopixel1, P0_7, 1);
neopixel_clear(&neopixel1);
neopixel_strip_t neopixel2;
neopixel_init(&neopixel2, P0_8, 1);
neopixel_clear(&neopixel2);
neopixel_strip_t neopixel3;
neopixel_init(&neopixel3, P0_10, 1);
neopixel_clear(&neopixel3);
neopixel_strip_t neopixel4;
neopixel_init(&neopixel4, P0_9, 1);
neopixel_clear(&neopixel4);
neopixel_strip_t neopixel5;
neopixel_init(&neopixel5, P0_15, 1);
neopixel_clear(&neopixel5);
neopixel_strip_t neopixel6;
neopixel_init(&neopixel6, P0_11, 1);
neopixel_clear(&neopixel6);
neopixel_strip_t *pixels[6];
pixels[0] = &neopixel1;
pixels[1] = &neopixel2;
pixels[2] = &neopixel3;
pixels[3] = &neopixel4;
pixels[4] = &neopixel5;
pixels[5] = &neopixel6;
// my_analogin_init();
ble.init();
ble.gap().onDisconnection(disconnectionCallback);
ble.gap().onRadioNotification(radioNotificationHandler);
// ble.gap().onTimeout(ConnectTimeoutCallback);
UARTService uartService(ble);
uartServicePtr = &uartService;
// ★OTA用
dfuService = new DFUService(ble, NULL);
/* Setup primary service. */
HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST);
// HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER);
// BatteryService batteryService(ble, 100);
/* Setup auxiliary service. */
//DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1");
// ★
DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", "SN1", "0.0.1", "0.0.1", "0.0.1");
// MACアドレスをDEVICE_NAMEに組み込む
setBleDeviceName(DEVICE_NAME);
/* Setup advertising. */
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE);
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list));
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR);
ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME));
ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED);
ble.gap().setAdvertisingInterval(800); /* 1000ms */
ble.gap().startAdvertising();
int j;
for(j = 0; j < MAIN_NEOPIXEL_COUNT; j++){
// MainNeopixelColorArrayRatio[j] = 0;
ColorArrayR[j] = 0;
ColorArrayG[j] = 0;
ColorArrayB[j] = 255;
}
for(j = 0; j < MAIN_NEOPIXEL_COUNT; j++){
// MainNeopixelColorArrayRatio[j] = 0;
prevColorArrayR[j] = 0;
prevColorArrayG[j] = 0;
prevColorArrayB[j] = 255;
}
int firstR = 0;
int firstG = 0;
int firstB = 0;
float dim = 0.5;
for( i = 0; i < MAIN_NEOPIXEL_COUNT; i++)
{
switch(i) {
case 0:
firstR = short(0 * dim);
firstG = short(0 * dim);
firstB = short(255 * dim);
break;
case 1:
firstR = short(0 * dim);
firstG = short(127 * dim);
firstB = short(127 * dim);
break;
case 2:
firstR = short(0 * dim);
firstG = short(255 * dim);
firstB = short(0 * dim);
break;
case 3:
firstR = short(127 * dim);
firstG = short(127 * dim);
firstB = short(0 * dim);
break;
case 4:
firstR = short(200 * dim);
firstG = short(55 * dim);
firstB = short(0 * dim);
break;
case 5:
firstR = short(255 * dim);
firstG = short(0 * dim);
firstB = short(0 * dim);
break;
}
neopixel_set_color(
pixels[i],
0,
firstR,
firstG,
firstB);
neopixel_show(pixels[i]);
}
t.start();
prevmillis = t.read_ms();
bool bPulseEvent = false;
float degPoint = 0;
float degRatio = 0;
bool bSwingR = false;
float swingIndex = 0.0;
float midPoint = 0.5;
// neopixelTick.attach(neopixelTick_Handler, 0.005);
while (1) {
// batteryService.updateBatteryLevel( (uint8_t)read_battery_level());
float currentValue = HRSigIn;
bPulseEvent = false;
if ((currentValue <= 0.5) && (sensorValue > 0.5))
{
bPulseEvent = true;
}
int sens = 1023 * currentValue;
// printf("S%d", sens);
// printf("S%d\n", sens);
sensorValue = currentValue;
if(bPulseEvent) {
fBeatIndex = 0.0;
// batteryService.updateBatteryLevel( (uint8_t)read_battery_level());
// 現在の時刻を取得
// t.stop(); // stop, startは時間計測の結果を有意に短くしてしまうので使わない
currentmillis = t.read_ms();
// t.start();
// 現在時刻から過去時刻を引き、間隔を求める
intervalmillis = currentmillis - prevmillis;
// 心拍の間の間隔
IBI = intervalmillis;
// 現在の時刻を過去時刻として記録
prevmillis = currentmillis;
// 平均値算出用配列に値を詰める
double averageIntervalMillis;
for(i = 0; i < MAXARRAY ; i++)
{
// 最後尾以外の場合
if(i != MAXARRAY - 1)
{
// 次の要素の値をコピーする
intervalmillisArray[i] = intervalmillisArray[i + 1];
}
// 最後尾の場合
else {
// 最新値を詰める
intervalmillisArray[i] = intervalmillis;
}
}
// 先頭が0の間は何もしない
unsigned long sumIntervalMillis = 0;
unsigned long sumIntervalMillisV = 0;
if (intervalmillisArray[0] != 0)
{
// 平均値算出
averageIntervalMillis = 0;
for(i = 0; i < MAXARRAY ; i++)
{
sumIntervalMillis += intervalmillisArray[i];
sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i];
}
double squ;
double var;
double stdDev;
averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY;
squ = sumIntervalMillisV - (double)(sumIntervalMillis * sumIntervalMillis) / (double)MAXARRAY;
var = squ / (double)(MAXARRAY - 1);
stdDev = sqrt(var);
BPM = min32_of(60000 / averageIntervalMillis, 255);
// if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) {
currentBPM = min32_of(60000 / intervalmillis, 255);
/* }
else {
currentBPM = BPM;
} */
}
/*
int IBI1024 = IBI * (1024/1000);
bpm[1] = BPM;
bpm[6] = (IBI1024 & 0xFF00) >> 8;
bpm[7] = (IBI1024 & 0xFF);
*/
// update bpm
if(ble.gap().getState().connected == 1)
{
hrServiceBPM.updateHeartRate(BPM);
// hrServiceIBI.updateHeartRate(IBI);
}
// printf("B%d\n", BPM);
// printf("Q%d\n", IBI);
// nMsgLen = sprintf(msg,"B%d\0", BPM);
// ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI);
// ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
// 平均値算出用配列に値を詰める
for(i = 0; i < MAXHRVARRAY ; i++)
{
// 最後尾以外の場合
if(i != MAXHRVARRAY - 1)
{
// 次の要素の値をコピーする
intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1];
}
// 最後尾の場合
else {
// 最新値を詰める
intervalmillisHRVArray[i] = intervalmillis;
}
}
// 平均値算出
int nNNInterval;
unsigned long ulIntervalSum = 0;
int nSampleCount = 0;
for(i = 0; i < MAXHRVARRAY ; i++)
{
if(intervalmillisHRVArray[i] > 0)
{
ulIntervalSum += intervalmillisHRVArray[i];
nSampleCount++;
}
}
float IntervalAverage = 0.0;
if(nSampleCount > 0){
IntervalAverage = (float)ulIntervalSum / (float)nSampleCount;
}
nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR);
bool bContinue = false;
int nNNxCount = 0;
for(i = 0; i < MAXHRVARRAY ; i++)
{
// if not end
if(i < MAXHRVARRAY - 1) {
if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval)
{
if(bContinue == false)
{
nNNxCount++;
}
nNNxCount++;
bContinue = true;
}
else {
bContinue = false;
}
}
}
float diffRatio = (float)nNNxCount / (float)nSampleCount;
if(diffRatio > 1)
{
diffRatio = 1;
}
// リラックスの係数を算出する
relaxRatio = diffRatio;
nMsgRelaxLen = sprintf(msgRelax,"R%0.2f\0", relaxRatio);
// nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio);
// ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
// 現在値から最低値を引く
// relaxRatio = relaxRatio - RELAX_RATIO_MIN;
// 1.0 / 最低値 × 現在値
// relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN));
int x1, y1, x2, y2;
int tempx1, tempy1, tempx2, tempy2, tempx3, tempy3;
int longx, shortx, longy, shorty;
long xx1, xx2, yy1, yy2;
int movex, movey;
double dist1, dist2;
double conRatioRatio;
/*
// 最後尾の3データから二つのx, y地点を作る。
x1 = intervalmillisArray[nSampleCount - 1];
y1 = intervalmillisArray[nSampleCount - 2];
x2 = intervalmillisArray[nSampleCount - 2];
y2 = intervalmillisArray[nSampleCount - 3];
*/
x1 = intervalmillis;
y1 = prevInterval;
x2 = prevInterval;
y2 = prevprevInterval;
// ２地点を結ぶ直線の長さを算出する。
if(x1 > x2)
{
longx = x1;
shortx = x2;
}
else {
longx = x2;
shortx = x1;
}
if(y1 > y2)
{
longy = y1;
shorty = y2;
}
else {
longy = y2;
shorty = y1;
}
xx1 = longx - shortx;
yy1 = longy - shorty;
dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1)));
dist2 = longx + longy;
float distRatio = dist1 / dist2;
conRatioNow = 1 - distRatio;
// 平均値算出用配列に値を詰める
float averageConRatio = 0.5;
for(i = 0; i < MAXCONARRAY ; i++)
{
// 最後尾以外の場合
if(i != MAXCONARRAY - 1)
{
// 次の要素の値をコピーする
conArray[i] = conArray[i + 1];
}
// 最後尾の場合
else {
// 最新値を詰める
conArray[i] = distRatio;
}
}
// 先頭が0の間は何もしない
if (conArray[0] != 0)
{
// 平均値算出
averageConRatio = 0;
for(i = 0; i < MAXCONARRAY ; i++)
{
averageConRatio += conArray[i];
}
averageConRatio = averageConRatio / (double)MAXCONARRAY;
}
/*
float averageStressRatio = 0.5;
for(i = 0; i < MAXSTRESSARRAY ; i++)
{
// 最後尾以外の場合
if(i != MAXSTRESSARRAY - 1)
{
// 次の要素の値をコピーする
stressArray[i] = stressArray[i + 1];
}
// 最後尾の場合
else {
// 最新値を詰める
if(x1 > y1) {
stressArray[i] = y1 / x1;
}
else {
stressArray[i] = x1 / y1;
}
}
}
// 先頭が0の間は何もしない
if (stressArray[0] != 0)
{
// 平均値算出
averageStressRatio = 0;
for(i = 0; i < MAXSTRESSARRAY ; i++)
{
averageStressRatio += stressArray[i];
}
averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY;
}
*/
/* float rcFilter = RCFILTER2;
float filteredDistRatio = 0.0;
float a;
if(prevFilteredDistRatio != 0) {
a = rcFilter * prevFilteredDistRatio;
filteredDistRatio = a + (1 - rcFilter) * distRatio;
}
else {
filteredDistRatio = (1 - rcFilter) * distRatio;
}
prevFilteredDistRatio = filteredDistRatio ;
conRatio = filteredDistRatio ;
*/
conRatio = averageConRatio;
conRatio = 1 - conRatio;
nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio);
// nMsgLen = sprintf(msg,"C%0.2f\0", conRatio);
// ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
prevprevInterval = prevInterval;
prevInterval = intervalmillis;
// (x2, y2)が原点となるように座標を変換する
movex = x2 * -1;
movey = y2 * -1;
tempx1 = x1 - movex;
tempy1 = y1 - movey;
tempx2 = x2 - movex;
tempy2 = y2 - movey;
tempx3 = x3 - movex;
tempy3 = y3 - movey;
// (x2, y2)を中心とした円を考える。
// (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3)
double theta = DegreeToRadian(180);
double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta);
double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta);
// ベクトル1とベクトル3の内積を求める
double vecInt = tempx1 * rotx3 + tempy1 * roty3;
// ベクトル1とベクトル3の外積を求める
double vecExt = tempx1 * roty3 - tempy1 * rotx3;
// ベクトル1とベクトル3の角度を求める
theta = atan2(vecExt, vecInt);
double deg = RadianToDegree(theta);
midPoint = degRatio;
if( deg <= 11.25 && deg >= -11.25) {
degPoint = 0.5;
}
else if(deg >= 11.25 && deg <= 33.75) {
degPoint = 0.375;
}
else if(deg >= 33.75 && deg <= 56.25) {
degPoint = 0.25;
}
else if(deg >= 56.25 && deg <= 78.75) {
degPoint = 0.125;
}
else if(deg >= 78.75 && deg <= 101.25) {
degPoint = 0;
}
else if(deg >= 101.25 && deg <= 123.75) {
degPoint = 0.125;
}
else if(deg >= 123.75 && deg <= 146.25) {
degPoint = 0.25;
}
else if(deg >= 146.25 && deg <= 168.75) {
degPoint = 0.375;
}
else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) {
degPoint = 0.5;
}
else if(deg <= -11.25 && deg >= -33.75) {
degPoint = 0.625;
}
else if(deg <= -33.75 && deg >= -56.25) {
degPoint = 0.75;
}
else if(deg <= -56.25 && deg >= -78.75) {
degPoint = 0.875;
}
else if(deg <= -78.75 && deg >= -101.25) {
degPoint = 1;
}
else if(deg <= -101.25 && deg >= -123.75) {
degPoint = 0.875;
}
else if(deg <= -123.75 && deg >= -146.25) {
degPoint = 0.75;
}
else if(deg <= -146.25 && deg >= -168.75) {
degPoint = 0.625;
}
else {
degPoint = 0;
}
degRatio = (deg + 180.0) / 360.0;
float rcFilter = RCFILTER;
float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint;
prevFilteredAngle = filteredAngle ;
happyRatio = filteredAngle - 0.5;
if(happyRatio <= 0){
happyRatio = abs(happyRatio);
}
else {
happyRatio = 0;
}
// happyRatio = averageStressRatio ;// @@@
nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio);
// nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio);
// ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen);
happyRatio = happyRatio * happyRatio * happyRatio;
// happyRatio = happyRatio * happyRatio;
happyRatio *= 10;
// happyRatio *= 2;
// happyRatio *= relaxRatio;
x3 = x2;
y3 = y2;
int HR_range = 0;
int HR_tempval = currentBPM;
// 心拍の係数を算出する
// 最大心拍数以上→最大心拍数に
if( HR_MAX < HR_tempval)
{
HR_tempval = HR_MAX;
}
// 最少心拍数以下→最少心拍数に
if( HR_MIN > HR_tempval)
{
HR_tempval = HR_MIN;
}
// 心拍の幅を計算
HR_range = HR_MAX - HR_MIN;
// 現在値－最少心拍数
HR_tempval = HR_tempval - HR_MIN;
// 心拍の係数を算出。0 ～ 1.0
HR_ratio = (double)HR_tempval / (double)HR_range;
if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/)
{
bConcentration = true;
}
else {
bConcentration = false;
}
prevRelaxRatio = relaxRatio;
int hval = 1023 * happyRatio;
int rval = 1023 * relaxRatio;
int cval = 1023 * conRatio;
// printf("H%d\n", hval);
// printf("R%d\n", rval);
// printf("C%d\n", cval);
sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration);
ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(),
(uint8_t*)msg, nMsgIBILen + nMsgHappyLen + nMsgRelaxLen + nMsgConcentrationLen);
// (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen);
// determine color range
conRatio = conRatio - 0.5;
if(conRatio < 0) {
conRatio = 0;
}
conRatio = conRatio * 2;
// happyRatio = happyRatio * 2;
// happyRatio = happyRatio * 2;
if(happyRatio > 1) {
happyRatio = 1;
}
if(BPM >= HR_MIN) {
happyRatio /= ((float)(BPM * 2) / 255.0) ;
}
// happyRatio = happyRatio * happyRatio;
/*
relaxRatio = relaxRatio - 0.5;
if(relaxRatio < 0) {
relaxRatio = 0;
}
*/
}
unsigned long lightCurrentMillis = t.read_ms();
unsigned long lightMillis = lightCurrentMillis - lightPrevMillis;
lightPrevMillis = lightCurrentMillis;
int rVal = 0;
int gVal = 0;
int bVal = 0;
for( i = 0; i < MAIN_NEOPIXEL_COUNT; i++) {
ColorArrayR[i] = 0;
ColorArrayG[i] = 0;
ColorArrayB[i] = 0;
}
if(BPM > 0) {
getExciteColor(currentBPM, rVal, gVal, bVal);
int conRatioColor = 0;
conRatioColor = conRatio;
if(conRatioColor < 0.5) {
conRatioColor = 0.5;
}
int fBeatIndexMax = 8;
float fRatio = 0.0;
float realIndex = 0.0;
float IndexRatio = 0.0;
fBeatIndex += float(lightMillis) / float(100);
if (fBeatIndex >= fBeatIndexMax) {
fBeatIndex = fBeatIndexMax;
}
for(i = MAIN_NEOPIXEL_COUNT - 1; i >= 0; i--) {
realIndex = fBeatIndex - float(i);
// if((fBeatIndex >= i) && (fBeatIndex <= (i + 1))) {
// fRatio = (conRatio * (1.0 -(fBeatIndex - float(i))));
// IndexRatio = fRatio;
// }
// else if((fBeatIndex <= i) && (fBeatIndex >= (i - 1))) {
// fRatio = (conRatio * (1.0 - (float(i) - fBeatIndex)));
// }
// else if(float(i) < fBeatIndex) {
// fRatio = (0.5 * ((fBeatIndexMax - ceil(fBeatIndex)) / fBeatIndexMax));
// }
if(ceil(fBeatIndex) == i) {
fRatio = (conRatioColor);
}
else {
fRatio = (0.5 * ((fBeatIndexMax - ceil(fBeatIndex)) / fBeatIndexMax));
}
if(fRatio <= 0.05) {
fRatio = 0.05;
}
else if(float(i) < fBeatIndex) {
MainNeopixelColorArrayR[i] = short(rVal * fRatio);
MainNeopixelColorArrayG[i] = short(gVal * fRatio);
MainNeopixelColorArrayB[i] = short(bVal * fRatio);
}
}
for( i = 0; i < MAIN_NEOPIXEL_COUNT; i++)
{
neopixel_set_color(
pixels[i],
0,
min32_of(255, MainNeopixelColorArrayR[i]),
min32_of(255, MainNeopixelColorArrayG[i]),
min32_of(255, MainNeopixelColorArrayB[i]));
neopixel_show(pixels[i]);
}
}
wait_ms(50);
}
}