Masaya Shimoyamada
reduced message bytes for BLE
Dependencies: BLE_API mbed nRF51822
Fork of
Inp_Fiber_Logo_FOTA
by 
inupathy
Embed:
(wiki syntax)
Show/hide line numbers
main.cpp
00001 // 20170703 BRIGHTNESS_MAXを100に変更 00002 // シリアル通信のプロトコルを、文字列→バイト列に変更 00003 // 20170619 DeviceInformationのSerialNumberにIND+Macアドレスを適用し、DEVICENAMEはINUPATHYとした 00004 // 20170617 ストレス値を"R"(relax)から"S"(stress)に変更し、(1.0 - relaxRatio)を送るよう改変 00005 // 20170617 輝度調整の方法をrgbの合計値を基準にするように調整した。 00006 00007 #include "mbed.h" 00008 #include "neopixel.h" 00009 #include "nrf_soc.h" 00010 #include "common.h" 00011 #include "ble/BLE.h" 00012 #include "ble/services/HeartRateService.h" 00013 #include "ble/services/BatteryService.h" 00014 #include "ble/services/DeviceInformationService.h" 00015 #include "UARTService.h" 00016 #include "ble_gap.h" 00017 #include "ble/services/DFUService.h" // ★OTA用 00018 00019 #define RAINBOW_RATIO 1 00020 #define RCFILTER 0.9 00021 #define RCFILTER2 0.9 00022 #define NEOPIXEL_COUNT 6 00023 #define MIN_LIGHT 0 00024 #define PNNX_FACTOR 0.1 // 0.06 00025 #define MAXARRAY 15 //15 // 心拍平均値計算用の配列サイズ //20 00026 #define MAXHRVARRAY 10 //30 // 脈間ゆらぎ平均値計算用の配列サイズ 00027 #define MIN_INTERVAL 200 00028 #define INTERVAL 10 // Loopの間隔 通常時推奨1 通信時推奨２０ 00029 #define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7) 00030 #define MAXCONARRAY 10 00031 #define MAXSTRESSARRAY 3 00032 00033 #define BRIGHTNESS_MAX 100 // RGBLED１セットのpwm合計値の上限 ws2812b型のneopixelが６個の場合、150を超えた値にするとセンサーが誤作動する 00034 00035 #define DIM_LED ((float)(BRIGHTNESS_MAX) / (float)(255)) // R, G, Bそれぞれのpwm値がBRIGHTNESS_MAXを超えない様にする制限 00036 00037 00038 // 心拍数の最低値、最高値 00039 // 人間の場合は60～150、標準80 00040 // 犬の場合は40～200、標準60 00041 #define HR_MIN 40 00042 #define HR_MAX 150 00043 00044 #define RELAX_RATIO_MIN 0.6 00045 00046 // ラジアンを度に変換する． 00047 #define RadianToDegree(radian) ((180 / 3.14159) * (radian)) 00048 00049 // 度をラジアンに変換する． 00050 #define DegreeToRadian(degree) ((3.14159 / 180) * (degree)) 00051 00052 // 色の識別値設定（ピンではない） 00053 #define RED 0 00054 #define BLUE 1 00055 #define GREEN 2 00056 00057 #define SHOW_EXCITE 1 00058 #define SHOW_HAPPY 2 00059 #define SHOW_CONCENTRATION 4 00060 00061 00062 BLE ble; 00063 DFUService *dfuService; // ★OTA用 00064 00065 AnalogIn HRSigIn(P0_4); 00066 00067 00068 const static char DEVICE_NAME[] = "INUPATHY"; 00069 static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE, 00070 GattService::UUID_DEVICE_INFORMATION_SERVICE}; 00071 static volatile bool triggerSensorPolling = true; 00072 00073 short ColorArrayR[NEOPIXEL_COUNT]; 00074 short ColorArrayG[NEOPIXEL_COUNT]; 00075 short ColorArrayB[NEOPIXEL_COUNT]; 00076 00077 short TargetColorArrayR[NEOPIXEL_COUNT]; 00078 short TargetColorArrayG[NEOPIXEL_COUNT]; 00079 short TargetColorArrayB[NEOPIXEL_COUNT]; 00080 00081 00082 #define NEOPIXEL_COMMANDLEN ((NEOPIXEL_COUNT * 3) + 1 + 1) 00083 00084 00085 unsigned long pixelStartCounter; 00086 00087 int mainCurrentPixel = 0; 00088 int mainPrevPixel = 0; 00089 int mainPixelLightCount; 00090 int mainPixelStep = 0; 00091 int mainStepsRemain = 0; 00092 00093 unsigned long currentmillis = 0; 00094 unsigned long prevmillis = 0; 00095 unsigned long intervalmillis = 0; 00096 unsigned long prevIntervalmillis = 0; 00097 unsigned long intervalmillisArray[MAXARRAY] = {1000}; 00098 unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1000}; 00099 float conArray[MAXCONARRAY] = {0.5}; 00100 00101 //float stressArray[MAXSTRESSARRAY] = {0.5}; 00102 00103 float prevFilteredAngle = 0.5; 00104 float happyRatio = 0.0; 00105 float happyRatioNow = 0.0; 00106 float prevRelaxRatio = 1.0; 00107 float prevFilteredDistRatio = 0.0; 00108 00109 #define HR_THRESHOLD 2 00110 int HRThreshold[] = {120, 1000}; 00111 00112 00113 // 気分判定フラグ 00114 #define IS_RELAX 0 00115 #define IS_EXCITED 1 00116 #define IS_HAPPY 2 00117 #define IS_INTERESTED 3 00118 #define IS_STRESSED 4 // IS_STRESSEDはMOODの個数としても扱うので最後の番号にすること 00119 #define MOOD_COUNT IS_STRESSED + 1 00120 00121 // 気分判定係数 00122 // 気分判定係数 00123 #define CONMARGIN 0.9 // 0.85 00124 #define RELAXMARGIN 0.35 00125 #define HAPPYMARGIN 0.25 // 0.225 00126 #define EXCITEMARGIN 1.5 // 1.15 00127 00128 #define HRM_AVERAGE_COUNT 100 // MAX 125 00129 short HRM_Average[HRM_AVERAGE_COUNT] = {70}; 00130 00131 // 気分判定フラグを索引にする 00132 int MoodColorR[NEOPIXEL_COUNT][MOOD_COUNT]; 00133 int MoodColorG[NEOPIXEL_COUNT][MOOD_COUNT]; 00134 int MoodColorB[NEOPIXEL_COUNT][MOOD_COUNT]; 00135 00136 00137 unsigned char RainbowR[] = { 50, 50, 50, 127, 200, 255}; 00138 unsigned char RainbowG[] = { 50, 127, 255, 127, 100, 50}; 00139 unsigned char RainbowB[] = {255, 127, 50, 50, 50, 50}; 00140 00141 unsigned char HappyR[] = { 50, 50, 50, 127, 200, 255}; 00142 unsigned char HappyG[] = { 50, 127, 255, 127, 100, 50}; 00143 unsigned char HappyB[] = {255, 127, 50, 50, 50, 50}; 00144 00145 unsigned char RelaxR[] = { 0, 0, 0, 0, 0, 0}; 00146 unsigned char RelaxG[] = {255, 255, 255, 255, 255, 255}; 00147 unsigned char RelaxB[] = { 0, 0, 0, 0, 0, 0}; 00148 00149 unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255}; 00150 unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100}; 00151 unsigned char ExciteB[] = { 0, 0, 0, 0, 0, 0}; 00152 00153 unsigned char StressR[] = {100, 100, 100, 100, 100, 100}; 00154 unsigned char StressG[] = { 0, 0, 0, 0, 0, 0}; 00155 unsigned char StressB[] = {255, 255, 255, 255, 255, 255}; 00156 00157 unsigned char InterestR[] = {255, 255, 255, 255, 255, 255}; 00158 unsigned char InterestG[] = {255, 255, 255, 255, 255, 255}; 00159 unsigned char InterestB[] = {150, 150, 150, 150, 150, 150}; 00160 00161 float rainbowOffset = 0.0; 00162 00163 bool bConcentration; 00164 unsigned long loopCounter = 0; 00165 int lightPos = 1; 00166 int prevLightPos = 1; 00167 00168 double relaxRatio = 0.0; 00169 float conRatio = 0.5; 00170 float conRatioNow = 0.5; 00171 double HR_ratio = 0.0; 00172 00173 unsigned long IBI; 00174 00175 int x3, y3; 00176 00177 int mainPixelCounter; 00178 00179 int prevInterval; 00180 int prevprevInterval; 00181 00182 void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) 00183 { 00184 ble.gap().startAdvertising(); // restart advertising 00185 } 00186 /* 00187 void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback) 00188 { 00189 ble.gap().startAdvertising(); // restart advertising 00190 } 00191 */ 00192 00193 uint8_t BPM = 0; 00194 float sensorValue = 0; 00195 Timer t; 00196 Timer tRand; 00197 00198 float redVal = 0; 00199 float blueVal = 255; 00200 float greenVal = 0; 00201 00202 Ticker neopixelTick; 00203 00204 UARTService *uartServicePtr; 00205 00206 void neopixelTick_Handler() { 00207 return; 00208 } 00209 00210 void SetMoodColor() { 00211 int i = 0; 00212 00213 for(i = 0; i < NEOPIXEL_COUNT; i++) { 00214 MoodColorR[i][IS_RELAX] = RelaxR[i]; 00215 MoodColorG[i][IS_RELAX] = RelaxG[i]; 00216 MoodColorB[i][IS_RELAX] = RelaxB[i]; 00217 } 00218 for(i = 0; i < NEOPIXEL_COUNT; i++) { 00219 MoodColorR[i][IS_EXCITED] = ExciteR[i]; 00220 MoodColorG[i][IS_EXCITED] = ExciteG[i]; 00221 MoodColorB[i][IS_EXCITED] = ExciteB[i]; 00222 } 00223 00224 for(i = 0; i < NEOPIXEL_COUNT; i++) { 00225 MoodColorR[i][IS_HAPPY] = HappyR[i]; 00226 MoodColorG[i][IS_HAPPY] = HappyG[i]; 00227 MoodColorB[i][IS_HAPPY] = HappyB[i]; 00228 } 00229 for(i = 0; i < NEOPIXEL_COUNT; i++) { 00230 MoodColorR[i][IS_INTERESTED] = InterestR[i]; 00231 MoodColorG[i][IS_INTERESTED] = InterestG[i]; 00232 MoodColorB[i][IS_INTERESTED] = InterestB[i]; 00233 } 00234 for(i = 0; i < NEOPIXEL_COUNT; i++) { 00235 MoodColorR[i][IS_STRESSED] = StressR[i]; 00236 MoodColorG[i][IS_STRESSED] = StressG[i]; 00237 MoodColorB[i][IS_STRESSED] = StressB[i]; 00238 } 00239 } 00240 00241 00242 float BPM_Average = 0.0; 00243 int currentBPM = 0; 00244 00245 // 現在の気分の判定 00246 int getMood(int bpm, double relaxRatio, double happyRatio, double conRatio) { 00247 // 第一優先：集中 00248 // 集中係数が0.95以上なら集中と判定 00249 if(conRatio >= CONMARGIN) 00250 { 00251 return IS_INTERESTED; 00252 } 00253 00254 // 第二優先：Stress 00255 // リラックス係数が0.2以下 00256 if(relaxRatio <= RELAXMARGIN) { 00257 return IS_STRESSED; 00258 } 00259 00260 // 第三優先：Happy 00261 // Happy係数が0.2以上 00262 if(happyRatio >= HAPPYMARGIN && relaxRatio > 0.5) { 00263 return IS_HAPPY; 00264 } 00265 00266 // 平均の心拍より1.2倍以上だと興奮 00267 // それ以外は安静 00268 if((float)currentBPM / BPM_Average >= EXCITEMARGIN) { 00269 return IS_EXCITED; 00270 } 00271 // もしくは一定以上の心拍数だと興奮 00272 else if(BPM >= HRThreshold[0]) { 00273 return IS_EXCITED; 00274 } 00275 else { 00276 return IS_RELAX; 00277 } 00278 } 00279 00280 //Serial mySerial(USBTX, USBRX); 00281 //Serial mySerial(P0_9, P0_11); 00282 00283 // MACアドレスを取得してBLEのSerialNumberに組み込む 00284 void setBleSerialNumber(char *serialNumber) 00285 { 00286 int i; 00287 00288 ble_gap_addr_t mac_address; 00289 00290 if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){ 00291 return; 00292 } 00293 00294 // ADDR_LENは6 00295 for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){ 00296 sprintf(serialNumber, "%s%02x", serialNumber, (mac_address.addr)[i]); 00297 } 00298 sprintf(serialNumber, "%s%s", serialNumber, "\0"); 00299 00300 } 00301 00302 int main(void) 00303 { 00304 /* 00305 char msgIBI[100]; 00306 int nMsgIBILen = 0; 00307 char msgHappy[10]; 00308 int nMsgHappyLen = 0; 00309 char msgConcentration[10]; 00310 int nMsgConcentrationLen = 0; 00311 char msgRelax[10]; 00312 int nMsgRelaxLen = 0; 00313 */ 00314 byte_t msgHeader[3] = {0x49,0x4e,0x50}; 00315 00316 // byte_t msg[19]; 00317 byte_t msg[10]; 00318 // int nMsgLen = 0; 00319 00320 int i; 00321 00322 int r = 0; 00323 int g = 0; 00324 int b = 0; 00325 int brightnessSum = 0; 00326 00327 neopixel_strip_t neopixel1; 00328 neopixel_init(&neopixel1, P0_7, 1); 00329 neopixel_clear(&neopixel1); 00330 00331 neopixel_strip_t neopixel2; 00332 neopixel_init(&neopixel2, P0_8, 1); 00333 neopixel_clear(&neopixel2); 00334 00335 neopixel_strip_t neopixel3; 00336 neopixel_init(&neopixel3, P0_10, 1); 00337 neopixel_clear(&neopixel3); 00338 00339 neopixel_strip_t neopixel4; 00340 neopixel_init(&neopixel4, P0_9, 1); 00341 neopixel_clear(&neopixel4); 00342 00343 neopixel_strip_t neopixel5; 00344 neopixel_init(&neopixel5, P0_15, 1); 00345 neopixel_clear(&neopixel5); 00346 00347 neopixel_strip_t neopixel6; 00348 neopixel_init(&neopixel6, P0_11, 1); 00349 neopixel_clear(&neopixel6); 00350 00351 neopixel_strip_t *pixels[6]; 00352 00353 pixels[0] = &neopixel1; 00354 pixels[1] = &neopixel2; 00355 pixels[2] = &neopixel3; 00356 pixels[3] = &neopixel4; 00357 pixels[4] = &neopixel5; 00358 pixels[5] = &neopixel6; 00359 00360 00361 00362 SetMoodColor(); 00363 00364 ble.init(); 00365 ble.gap().onDisconnection(disconnectionCallback); 00366 00367 00368 UARTService uartService(ble); 00369 uartServicePtr = &uartService; 00370 00371 // ★OTA用 00372 dfuService = new DFUService(ble, NULL); 00373 00374 /* Setup primary service. */ 00375 HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST); 00376 // HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER); 00377 00378 // BatteryService batteryService(ble, 100); 00379 00380 /* Setup auxiliary service. */ 00381 //DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1"); 00382 00383 char serialNumber[16] = "IND"; 00384 // SerialNumberStringにMACアドレスを組み込む 00385 setBleSerialNumber(serialNumber); 00386 00387 // ★ 00388 DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", serialNumber, "0.0.1", "0.0.1", "0.0.1"); 00389 00390 /* Setup advertising. */ 00391 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); 00392 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); 00393 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); 00394 ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); 00395 ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); 00396 ble.gap().setAdvertisingInterval(800); /* 1000ms */ 00397 ble.gap().startAdvertising(); 00398 00399 for( i = 0; i < NEOPIXEL_COUNT; i++) 00400 { 00401 // 起動時の色パターンを設定 00402 ColorArrayR[i] = RainbowR[i]; 00403 ColorArrayG[i] = RainbowG[i]; 00404 ColorArrayB[i] = RainbowB[i]; 00405 00406 TargetColorArrayR[i] = ColorArrayR[i]; 00407 TargetColorArrayG[i] = ColorArrayG[i]; 00408 TargetColorArrayB[i] = ColorArrayB[i]; 00409 00410 // 輝度調整 00411 r = ColorArrayR[i] * DIM_LED; 00412 g = ColorArrayG[i] * DIM_LED; 00413 b = ColorArrayB[i] * DIM_LED; 00414 00415 brightnessSum = r + g + b; 00416 00417 // 輝度の合計値が上限を超えた場合は補正する 00418 if(brightnessSum > BRIGHTNESS_MAX) { 00419 r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 00420 g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 00421 b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 00422 } 00423 00424 neopixel_set_color( 00425 pixels[i], 00426 0, 00427 short(r), 00428 short(g), 00429 short(b)); 00430 neopixel_show(pixels[i]); 00431 } 00432 00433 t.start(); 00434 00435 prevmillis = t.read_ms(); 00436 00437 bool bPulseEvent = false; 00438 00439 float degPoint = 0; 00440 00441 // neopixelTick.attach(neopixelTick_Handler, 0.005); 00442 00443 while (1) { 00444 00445 // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); 00446 00447 float currentValue = HRSigIn; 00448 00449 bPulseEvent = false; 00450 00451 if ((currentValue <= 0.3) && (sensorValue > 0.3)) 00452 { 00453 bPulseEvent = true; 00454 } 00455 00456 // printf("S%d", sens); 00457 // printf("S%d\n", sens); 00458 00459 sensorValue = currentValue; 00460 00461 if(bPulseEvent) { 00462 // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); 00463 00464 // 現在の時刻を取得 00465 // t.stop(); // stop, startは時間計測の結果を有意に短くしてしまうので使わない 00466 currentmillis = t.read_ms(); 00467 // t.start(); 00468 // 現在時刻から過去時刻を引き、間隔を求める 00469 intervalmillis = currentmillis - prevmillis; 00470 00471 // 心拍の間の間隔 00472 IBI = intervalmillis; 00473 00474 // 現在の時刻を過去時刻として記録 00475 prevmillis = currentmillis; 00476 00477 // 平均値算出用配列に値を詰める 00478 double averageIntervalMillis; 00479 00480 for(i = 0; i < MAXARRAY ; i++) 00481 { 00482 // 最後尾以外の場合 00483 if(i != MAXARRAY - 1) 00484 { 00485 // 次の要素の値をコピーする 00486 intervalmillisArray[i] = intervalmillisArray[i + 1]; 00487 } 00488 // 最後尾の場合 00489 else { 00490 // 最新値を詰める 00491 intervalmillisArray[i] = intervalmillis; 00492 } 00493 } 00494 // 先頭が0の間は何もしない 00495 unsigned long sumIntervalMillis = 0; 00496 unsigned long sumIntervalMillisV = 0; 00497 if (intervalmillisArray[0] != 0) 00498 { 00499 // 平均値算出 00500 averageIntervalMillis = 0; 00501 for(i = 0; i < MAXARRAY ; i++) 00502 { 00503 sumIntervalMillis += intervalmillisArray[i]; 00504 sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i]; 00505 } 00506 00507 averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY; 00508 00509 BPM = min32_of(60000 / averageIntervalMillis, 255); 00510 // if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) { 00511 00512 currentBPM = min32_of(60000 / intervalmillis, 255); 00513 /* } 00514 else { 00515 currentBPM = BPM; 00516 } */ 00517 } 00518 00519 00520 // BPMの平均値計算 00521 int BPM_sum = 0; 00522 for(i = 0; i < HRM_AVERAGE_COUNT ; i++) 00523 { 00524 BPM_sum += HRM_Average[i]; 00525 } 00526 BPM_Average = (float)BPM_sum/(float)HRM_AVERAGE_COUNT; 00527 00528 00529 if(currentBPM > 0) { 00530 for(i = 0; i < HRM_AVERAGE_COUNT ; i++) 00531 { 00532 // 最後尾以外の場合 00533 if(i != HRM_AVERAGE_COUNT - 1) 00534 { 00535 // 次の要素の値をコピーする 00536 HRM_Average[i] = HRM_Average[i + 1]; 00537 } 00538 // 最後尾の場合 00539 else { 00540 // 最新値を詰める 00541 HRM_Average[i] = currentBPM; 00542 } 00543 } 00544 } 00545 00546 /* 00547 int IBI1024 = IBI * (1024/1000); 00548 00549 bpm[1] = BPM; 00550 bpm[6] = (IBI1024 & 0xFF00) >> 8; 00551 bpm[7] = (IBI1024 & 0xFF); 00552 */ 00553 00554 // printf("B%d\n", BPM); 00555 // printf("Q%d\n", IBI); 00556 00557 // nMsgLen = sprintf(msg,"B%d\0", BPM); 00558 // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); 00559 00560 // nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI); 00561 // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); 00562 00563 // 平均値算出用配列に値を詰める 00564 for(i = 0; i < MAXHRVARRAY ; i++) 00565 { 00566 // 最後尾以外の場合 00567 if(i != MAXHRVARRAY - 1) 00568 { 00569 // 次の要素の値をコピーする 00570 intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1]; 00571 } 00572 // 最後尾の場合 00573 else { 00574 // 最新値を詰める 00575 intervalmillisHRVArray[i] = intervalmillis; 00576 } 00577 } 00578 00579 // 平均値算出 00580 int nNNInterval; 00581 unsigned long ulIntervalSum = 0; 00582 int nSampleCount = 0; 00583 for(i = 0; i < MAXHRVARRAY ; i++) 00584 { 00585 if(intervalmillisHRVArray[i] > 0) 00586 { 00587 ulIntervalSum += intervalmillisHRVArray[i]; 00588 nSampleCount++; 00589 } 00590 } 00591 00592 float IntervalAverage = 0.0; 00593 if(nSampleCount > 0){ 00594 IntervalAverage = (float)ulIntervalSum / (float)nSampleCount; 00595 } 00596 00597 nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR); 00598 00599 00600 bool bContinue = false; 00601 int nNNxCount = 0; 00602 for(i = 0; i < MAXHRVARRAY ; i++) 00603 { 00604 // if not end 00605 if(i < MAXHRVARRAY - 1) { 00606 if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval) 00607 { 00608 if(bContinue == false) 00609 { 00610 nNNxCount++; 00611 } 00612 nNNxCount++; 00613 bContinue = true; 00614 } 00615 else { 00616 bContinue = false; 00617 } 00618 } 00619 } 00620 00621 float diffRatio = (float)nNNxCount / (float)nSampleCount; 00622 00623 if(diffRatio > 1) 00624 { 00625 diffRatio = 1; 00626 } 00627 00628 // リラックスの係数を算出する 00629 relaxRatio = diffRatio; 00630 00631 // nMsgRelaxLen = sprintf(msgRelax,"S%0.2f\0", (1-relaxRatio)); 00632 00633 // nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio); 00634 // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); 00635 00636 // 現在値から最低値を引く 00637 // relaxRatio = relaxRatio - RELAX_RATIO_MIN; 00638 00639 // 1.0 / 最低値 × 現在値 00640 // relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN)); 00641 00642 int x1, y1, x2, y2; 00643 int tempx1, tempy1, tempx3, tempy3; 00644 int longx, shortx, longy, shorty; 00645 long xx1, yy1; 00646 00647 int movex, movey; 00648 00649 double dist1, dist2; 00650 00651 /* 00652 // 最後尾の3データから二つのx, y地点を作る。 00653 x1 = intervalmillisArray[nSampleCount - 1]; 00654 y1 = intervalmillisArray[nSampleCount - 2]; 00655 x2 = intervalmillisArray[nSampleCount - 2]; 00656 y2 = intervalmillisArray[nSampleCount - 3]; 00657 */ 00658 00659 x1 = intervalmillis; 00660 y1 = prevInterval; 00661 x2 = prevInterval; 00662 y2 = prevprevInterval; 00663 00664 // ２地点を結ぶ直線の長さを算出する。 00665 if(x1 > x2) 00666 { 00667 longx = x1; 00668 shortx = x2; 00669 } 00670 else { 00671 longx = x2; 00672 shortx = x1; 00673 } 00674 00675 if(y1 > y2) 00676 { 00677 longy = y1; 00678 shorty = y2; 00679 } 00680 else { 00681 longy = y2; 00682 shorty = y1; 00683 } 00684 00685 xx1 = longx - shortx; 00686 yy1 = longy - shorty; 00687 00688 dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1))); 00689 00690 dist2 = longx + longy; 00691 float distRatio = dist1 / dist2; 00692 00693 conRatioNow = 1 - distRatio; 00694 00695 // 平均値算出用配列に値を詰める 00696 float averageConRatio = 0.5; 00697 for(i = 0; i < MAXCONARRAY ; i++) 00698 { 00699 // 最後尾以外の場合 00700 if(i != MAXCONARRAY - 1) 00701 { 00702 // 次の要素の値をコピーする 00703 conArray[i] = conArray[i + 1]; 00704 } 00705 // 最後尾の場合 00706 else { 00707 // 最新値を詰める 00708 conArray[i] = distRatio; 00709 } 00710 } 00711 // 先頭が0の間は何もしない 00712 if (conArray[0] != 0) 00713 { 00714 // 平均値算出 00715 averageConRatio = 0; 00716 for(i = 0; i < MAXCONARRAY ; i++) 00717 { 00718 averageConRatio += conArray[i]; 00719 } 00720 averageConRatio = averageConRatio / (double)MAXCONARRAY; 00721 } 00722 /* 00723 float averageStressRatio = 0.5; 00724 for(i = 0; i < MAXSTRESSARRAY ; i++) 00725 { 00726 // 最後尾以外の場合 00727 if(i != MAXSTRESSARRAY - 1) 00728 { 00729 // 次の要素の値をコピーする 00730 stressArray[i] = stressArray[i + 1]; 00731 } 00732 // 最後尾の場合 00733 else { 00734 // 最新値を詰める 00735 if(x1 > y1) { 00736 stressArray[i] = y1 / x1; 00737 } 00738 else { 00739 stressArray[i] = x1 / y1; 00740 } 00741 } 00742 } 00743 // 先頭が0の間は何もしない 00744 if (stressArray[0] != 0) 00745 { 00746 // 平均値算出 00747 averageStressRatio = 0; 00748 for(i = 0; i < MAXSTRESSARRAY ; i++) 00749 { 00750 averageStressRatio += stressArray[i]; 00751 } 00752 averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY; 00753 } 00754 */ 00755 00756 /* float rcFilter = RCFILTER2; 00757 float filteredDistRatio = 0.0; 00758 float a; 00759 if(prevFilteredDistRatio != 0) { 00760 a = rcFilter * prevFilteredDistRatio; 00761 filteredDistRatio = a + (1 - rcFilter) * distRatio; 00762 } 00763 else { 00764 filteredDistRatio = (1 - rcFilter) * distRatio; 00765 } 00766 prevFilteredDistRatio = filteredDistRatio ; 00767 00768 conRatio = filteredDistRatio ; 00769 */ 00770 conRatio = averageConRatio; 00771 conRatio = 1 - conRatio; 00772 00773 // nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio); 00774 00775 // nMsgLen = sprintf(msg,"C%0.2f\0", conRatio); 00776 // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); 00777 00778 00779 prevprevInterval = prevInterval; 00780 prevInterval = intervalmillis; 00781 00782 // (x2, y2)が原点となるように座標を変換する 00783 movex = x2 * -1; 00784 movey = y2 * -1; 00785 00786 tempx1 = x1 - movex; 00787 tempy1 = y1 - movey; 00788 tempx3 = x3 - movex; 00789 tempy3 = y3 - movey; 00790 00791 // (x2, y2)を中心とした円を考える。 00792 00793 // (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3) 00794 double theta = DegreeToRadian(180); 00795 00796 double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta); 00797 double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta); 00798 00799 // ベクトル1とベクトル3の内積を求める 00800 double vecInt = tempx1 * rotx3 + tempy1 * roty3; 00801 00802 // ベクトル1とベクトル3の外積を求める 00803 double vecExt = tempx1 * roty3 - tempy1 * rotx3; 00804 00805 // ベクトル1とベクトル3の角度を求める 00806 theta = atan2(vecExt, vecInt); 00807 00808 double deg = RadianToDegree(theta); 00809 00810 if( deg <= 11.25 && deg >= -11.25) { 00811 degPoint = 0.5; 00812 } 00813 else if(deg >= 11.25 && deg <= 33.75) { 00814 degPoint = 0.375; 00815 } 00816 else if(deg >= 33.75 && deg <= 56.25) { 00817 degPoint = 0.25; 00818 } 00819 else if(deg >= 56.25 && deg <= 78.75) { 00820 degPoint = 0.125; 00821 } 00822 else if(deg >= 78.75 && deg <= 101.25) { 00823 degPoint = 0; 00824 } 00825 else if(deg >= 101.25 && deg <= 123.75) { 00826 degPoint = 0.125; 00827 } 00828 else if(deg >= 123.75 && deg <= 146.25) { 00829 degPoint = 0.25; 00830 } 00831 else if(deg >= 146.25 && deg <= 168.75) { 00832 degPoint = 0.375; 00833 } 00834 else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) { 00835 degPoint = 0.5; 00836 } 00837 else if(deg <= -11.25 && deg >= -33.75) { 00838 degPoint = 0.625; 00839 } 00840 else if(deg <= -33.75 && deg >= -56.25) { 00841 degPoint = 0.75; 00842 } 00843 else if(deg <= -56.25 && deg >= -78.75) { 00844 degPoint = 0.875; 00845 } 00846 else if(deg <= -78.75 && deg >= -101.25) { 00847 degPoint = 1; 00848 } 00849 else if(deg <= -101.25 && deg >= -123.75) { 00850 degPoint = 0.875; 00851 } 00852 else if(deg <= -123.75 && deg >= -146.25) { 00853 degPoint = 0.75; 00854 } 00855 else if(deg <= -146.25 && deg >= -168.75) { 00856 degPoint = 0.625; 00857 } 00858 else { 00859 degPoint = 0; 00860 } 00861 00862 happyRatioNow = degPoint; 00863 00864 float rcFilter = RCFILTER; 00865 float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint; 00866 prevFilteredAngle = filteredAngle ; 00867 00868 happyRatio = filteredAngle - 0.5; 00869 00870 if(happyRatio <= 0){ 00871 happyRatio = abs(happyRatio); 00872 } 00873 else { 00874 happyRatio = 0; 00875 } 00876 00877 // happyRatio = averageStressRatio ;// @@@ 00878 00879 // nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio); 00880 00881 // nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio); 00882 // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); 00883 00884 // happyRatio = happyRatio * happyRatio * happyRatio; 00885 00886 // happyRatio *= 10; 00887 00888 x3 = x2; 00889 y3 = y2; 00890 00891 int HR_range = 0; 00892 00893 int HR_tempval = currentBPM; 00894 00895 00896 // 心拍の係数を算出する 00897 00898 // 最大心拍数以上→最大心拍数に 00899 if( HR_MAX < HR_tempval) 00900 { 00901 HR_tempval = HR_MAX; 00902 } 00903 00904 // 最少心拍数以下→最少心拍数に 00905 if( HR_MIN > HR_tempval) 00906 { 00907 HR_tempval = HR_MIN; 00908 } 00909 00910 // 心拍の幅を計算 00911 HR_range = HR_MAX - HR_MIN; 00912 00913 // 現在値－最少心拍数 00914 HR_tempval = HR_tempval - HR_MIN; 00915 00916 // 心拍の係数を算出。0 ～ 1.0 00917 HR_ratio = (double)HR_tempval / (double)HR_range; 00918 00919 if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/) 00920 { 00921 bConcentration = true; 00922 } 00923 else { 00924 bConcentration = false; 00925 } 00926 prevRelaxRatio = relaxRatio; 00927 00928 // printf("H%d\n", hval); 00929 // printf("R%d\n", rval); 00930 // printf("C%d\n", cval); 00931 00932 // sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration); 00933 00934 unsigned char happyRatioChar = (unsigned char)(happyRatio * 100); 00935 unsigned char stressRatioChar = (unsigned char)((1 - relaxRatio) * 100); 00936 unsigned char conRatioChar = (unsigned char)(conRatio * 100); 00937 00938 memcpy(msg, msgHeader, sizeof(msgHeader)); 00939 memcpy(&(msg[3]), &IBI , sizeof(IBI) ); 00940 memcpy(&(msg[7]), &happyRatioChar , sizeof(happyRatioChar)); 00941 memcpy(&(msg[8]), &stressRatioChar , sizeof(stressRatioChar)); 00942 memcpy(&(msg[9]), &conRatioChar , sizeof(conRatioChar) ); 00943 ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), 00944 (uint8_t*)msg, sizeof(msg)); 00945 // (uint8_t*)msg, nMsgIBILen + nMsgHappyLen + nMsgRelaxLen + nMsgConcentrationLen); 00946 // (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen); 00947 // update bpm 00948 00949 if(ble.gap().getState().connected == 1) 00950 { 00951 hrServiceBPM.updateHeartRate(BPM); 00952 // hrServiceIBI.updateHeartRate(IBI); 00953 } 00954 00955 // determine color range 00956 00957 conRatio = conRatio - 0.5; 00958 00959 if(conRatio < 0) { 00960 conRatio = 0; 00961 } 00962 00963 conRatio = conRatio * 2; 00964 00965 // happyRatio = happyRatio * 2; 00966 // happyRatio = happyRatio * 2; 00967 if(happyRatio > 1) { 00968 happyRatio = 1; 00969 } 00970 /* 00971 if(BPM >= HR_MIN) { 00972 happyRatio /= ((float)(BPM * 2) / 255.0) ; 00973 } 00974 */ 00975 } 00976 00977 if(BPM > 0 && bPulseEvent) { 00978 int NewMood = getMood(BPM, relaxRatio, happyRatio, conRatio) ; 00979 00980 for(i = 0 ; i < NEOPIXEL_COUNT; i++) { 00981 TargetColorArrayR[i] = MoodColorR[i][NewMood]; 00982 TargetColorArrayG[i] = MoodColorG[i][NewMood]; 00983 TargetColorArrayB[i] = MoodColorB[i][NewMood]; 00984 } 00985 } 00986 00987 int deltaVal = 2; 00988 for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) { 00989 if(ColorArrayR[i] < TargetColorArrayR[i]) { 00990 ColorArrayR[i]+=deltaVal; 00991 if(ColorArrayR[i] > TargetColorArrayR[i]) { 00992 ColorArrayR[i] = TargetColorArrayR[i]; 00993 } 00994 } 00995 else if(ColorArrayR[i] > TargetColorArrayR[i]) { 00996 ColorArrayR[i]-=deltaVal; 00997 if(ColorArrayR[i] < TargetColorArrayR[i]) { 00998 ColorArrayR[i] = TargetColorArrayR[i]; 00999 } 01000 } 01001 if(ColorArrayG[i] < TargetColorArrayG[i]) { 01002 ColorArrayG[i]+=deltaVal; 01003 if(ColorArrayG[i] > TargetColorArrayG[i]) { 01004 ColorArrayG[i] = TargetColorArrayG[i]; 01005 } 01006 } 01007 else if(ColorArrayG[i] > TargetColorArrayG[i]) { 01008 ColorArrayG[i]-=deltaVal; 01009 if(ColorArrayG[i] < TargetColorArrayG[i]) { 01010 ColorArrayG[i] = TargetColorArrayG[i]; 01011 } 01012 } 01013 if(ColorArrayB[i] < TargetColorArrayB[i]) { 01014 ColorArrayB[i]+=deltaVal; 01015 if(ColorArrayB[i] > TargetColorArrayB[i]) { 01016 ColorArrayB[i] = TargetColorArrayB[i]; 01017 } 01018 } 01019 else if(ColorArrayB[i] > TargetColorArrayB[i]) { 01020 ColorArrayB[i]-=deltaVal; 01021 if(ColorArrayB[i] < TargetColorArrayB[i]) { 01022 ColorArrayB[i] = TargetColorArrayB[i]; 01023 } 01024 } 01025 01026 // 輝度調整 01027 r = ColorArrayR[i] * DIM_LED; 01028 g = ColorArrayG[i] * DIM_LED; 01029 b = ColorArrayB[i] * DIM_LED; 01030 01031 brightnessSum = r + g + b; 01032 01033 // 輝度の合計値が上限を超えた場合は補正する 01034 if(brightnessSum > BRIGHTNESS_MAX) { 01035 r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 01036 g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 01037 b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); 01038 } 01039 01040 neopixel_set_color( 01041 pixels[i], 01042 0, 01043 short(r), 01044 short(g), 01045 short(b)); 01046 neopixel_show(pixels[i]); 01047 } 01048 //wait_ms(1); 01049 wait_ms(10); // 500usまで下げてもセンサー共振は起こらないことを確認 01050 } 01051 } 01052
Generated on Mon Aug 22 2022 18:58:14 by
1.7.2