Masaya Shimoyamada
/
Inp_Fiber_Logo_FOTA_reduce_msg_bytes
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependencies: BLE_API mbed nRF51822
Fork of
Inp_Fiber_Logo_FOTA
by 
inupathy
main.cpp@6:35f536162183, 2017-06-17 (annotated)
- Committer:
- jojisdogakane
- Date:
- Sat Jun 17 08:22:21 2017 +0000
- Revision:
- 6:35f536162183
- Parent:
- 5:b3fe9050a777
- Child:
- 7:4118d5a717bc
Not implemented yet,; ; ?Animation
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| jojisdogakane | 6:35f536162183 | 1 | // 20170617 ストレス値を"R"(relax)から"S"(stress)に変更し、(1.0 - relaxRatio)を送るよう改変 |
| jojisdogakane | 6:35f536162183 | 2 | // 20170617 輝度調整の方法をrgbの合計値を基準にするように調整した。 |
| jojisdogakane | 6:35f536162183 | 3 | |
| jojisdogakane | 0:2f6112d75752 | 4 | #include "mbed.h" |
| jojisdogakane | 0:2f6112d75752 | 5 | #include "neopixel.h" |
| jojisdogakane | 0:2f6112d75752 | 6 | #include "nrf_soc.h" |
| jojisdogakane | 0:2f6112d75752 | 7 | #include "common.h" |
| jojisdogakane | 0:2f6112d75752 | 8 | #include "ble/BLE.h" |
| jojisdogakane | 0:2f6112d75752 | 9 | #include "ble/services/HeartRateService.h" |
| jojisdogakane | 0:2f6112d75752 | 10 | #include "ble/services/BatteryService.h" |
| jojisdogakane | 0:2f6112d75752 | 11 | #include "ble/services/DeviceInformationService.h" |
| jojisdogakane | 0:2f6112d75752 | 12 | #include "UARTService.h" |
| ayaando | 1:f28bdfaf3005 | 13 | #include "ble_gap.h" |
| ayaando | 2:221df6d014f5 | 14 | #include "ble/services/DFUService.h" // ★OTA用 |
| ayaando | 2:221df6d014f5 | 15 | |
| jojisdogakane | 0:2f6112d75752 | 16 | #define RAINBOW_RATIO 1 |
| jojisdogakane | 0:2f6112d75752 | 17 | #define RCFILTER 0.9 |
| jojisdogakane | 0:2f6112d75752 | 18 | #define RCFILTER2 0.9 |
| jojisdogakane | 4:a0150e0f285b | 19 | #define NEOPIXEL_COUNT 6 |
| jojisdogakane | 0:2f6112d75752 | 20 | #define MIN_LIGHT 0 |
| jojisdogakane | 0:2f6112d75752 | 21 | #define PNNX_FACTOR 0.1 // 0.06 |
| jojisdogakane | 4:a0150e0f285b | 22 | #define MAXARRAY 15 //15 // 心拍平均値計算用の配列サイズ //20 |
| jojisdogakane | 4:a0150e0f285b | 23 | #define MAXHRVARRAY 10 //30 // 脈間ゆらぎ平均値計算用の配列サイズ |
| jojisdogakane | 0:2f6112d75752 | 24 | #define MIN_INTERVAL 200 |
| jojisdogakane | 0:2f6112d75752 | 25 | #define INTERVAL 10 // Loopの間隔 通常時推奨1 通信時推奨２０ |
| jojisdogakane | 0:2f6112d75752 | 26 | #define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7) |
| jojisdogakane | 0:2f6112d75752 | 27 | #define MAXCONARRAY 10 |
| jojisdogakane | 0:2f6112d75752 | 28 | #define MAXSTRESSARRAY 3 |
| jojisdogakane | 6:35f536162183 | 29 | |
| jojisdogakane | 6:35f536162183 | 30 | #define BRIGHTNESS_MAX 150 // RGBLED１セットのpwm合計値の上限 ws2812b型のneopixelが６個の場合、150を超えた値にするとセンサーが誤作動する |
| jojisdogakane | 6:35f536162183 | 31 | |
| jojisdogakane | 6:35f536162183 | 32 | #define DIM_LED ((float)(BRIGHTNESS_MAX) / (float)(255)) // R, G, Bそれぞれのpwm値がBRIGHTNESS_MAXを超えない様にする制限 |
| jojisdogakane | 6:35f536162183 | 33 | |
| jojisdogakane | 0:2f6112d75752 | 34 | |
| jojisdogakane | 0:2f6112d75752 | 35 | // 心拍数の最低値、最高値 |
| jojisdogakane | 0:2f6112d75752 | 36 | // 人間の場合は60～150、標準80 |
| jojisdogakane | 0:2f6112d75752 | 37 | // 犬の場合は40～200、標準60 |
| jojisdogakane | 0:2f6112d75752 | 38 | #define HR_MIN 40 |
| jojisdogakane | 0:2f6112d75752 | 39 | #define HR_MAX 150 |
| jojisdogakane | 0:2f6112d75752 | 40 | |
| jojisdogakane | 0:2f6112d75752 | 41 | #define RELAX_RATIO_MIN 0.6 |
| jojisdogakane | 0:2f6112d75752 | 42 | |
| jojisdogakane | 0:2f6112d75752 | 43 | // ラジアンを度に変換する． |
| jojisdogakane | 0:2f6112d75752 | 44 | #define RadianToDegree(radian) ((180 / 3.14159) * (radian)) |
| jojisdogakane | 0:2f6112d75752 | 45 | |
| jojisdogakane | 0:2f6112d75752 | 46 | // 度をラジアンに変換する． |
| jojisdogakane | 0:2f6112d75752 | 47 | #define DegreeToRadian(degree) ((3.14159 / 180) * (degree)) |
| jojisdogakane | 0:2f6112d75752 | 48 | |
| jojisdogakane | 0:2f6112d75752 | 49 | // 色の識別値設定（ピンではない） |
| jojisdogakane | 0:2f6112d75752 | 50 | #define RED 0 |
| jojisdogakane | 0:2f6112d75752 | 51 | #define BLUE 1 |
| jojisdogakane | 0:2f6112d75752 | 52 | #define GREEN 2 |
| jojisdogakane | 0:2f6112d75752 | 53 | |
| jojisdogakane | 0:2f6112d75752 | 54 | #define SHOW_EXCITE 1 |
| jojisdogakane | 0:2f6112d75752 | 55 | #define SHOW_HAPPY 2 |
| jojisdogakane | 0:2f6112d75752 | 56 | #define SHOW_CONCENTRATION 4 |
| jojisdogakane | 0:2f6112d75752 | 57 | |
| jojisdogakane | 0:2f6112d75752 | 58 | |
| jojisdogakane | 0:2f6112d75752 | 59 | BLE ble; |
| ayaando | 2:221df6d014f5 | 60 | DFUService *dfuService; // ★OTA用 |
| jojisdogakane | 0:2f6112d75752 | 61 | |
| jojisdogakane | 0:2f6112d75752 | 62 | AnalogIn HRSigIn(P0_4); |
| jojisdogakane | 0:2f6112d75752 | 63 | |
| jojisdogakane | 0:2f6112d75752 | 64 | |
| ayaando | 1:f28bdfaf3005 | 65 | //const static char DEVICE_NAME[] = "InupLogo"; |
| ayaando | 1:f28bdfaf3005 | 66 | static char DEVICE_NAME[16] = "IND"; |
| jojisdogakane | 0:2f6112d75752 | 67 | static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE, |
| jojisdogakane | 0:2f6112d75752 | 68 | GattService::UUID_DEVICE_INFORMATION_SERVICE}; |
| jojisdogakane | 0:2f6112d75752 | 69 | static volatile bool triggerSensorPolling = true; |
| jojisdogakane | 0:2f6112d75752 | 70 | |
| jojisdogakane | 4:a0150e0f285b | 71 | short ColorArrayR[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 72 | short ColorArrayG[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 73 | short ColorArrayB[NEOPIXEL_COUNT]; |
| jojisdogakane | 0:2f6112d75752 | 74 | |
| jojisdogakane | 4:a0150e0f285b | 75 | short TargetColorArrayR[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 76 | short TargetColorArrayG[NEOPIXEL_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 77 | short TargetColorArrayB[NEOPIXEL_COUNT]; |
| jojisdogakane | 0:2f6112d75752 | 78 | |
| jojisdogakane | 0:2f6112d75752 | 79 | |
| jojisdogakane | 4:a0150e0f285b | 80 | #define NEOPIXEL_COMMANDLEN ((NEOPIXEL_COUNT * 3) + 1 + 1) |
| jojisdogakane | 0:2f6112d75752 | 81 | |
| jojisdogakane | 0:2f6112d75752 | 82 | |
| jojisdogakane | 0:2f6112d75752 | 83 | unsigned long pixelStartCounter; |
| jojisdogakane | 0:2f6112d75752 | 84 | |
| jojisdogakane | 0:2f6112d75752 | 85 | int mainCurrentPixel = 0; |
| jojisdogakane | 0:2f6112d75752 | 86 | int mainPrevPixel = 0; |
| jojisdogakane | 0:2f6112d75752 | 87 | int mainPixelLightCount; |
| jojisdogakane | 0:2f6112d75752 | 88 | int mainPixelStep = 0; |
| jojisdogakane | 0:2f6112d75752 | 89 | int mainStepsRemain = 0; |
| jojisdogakane | 0:2f6112d75752 | 90 | |
| jojisdogakane | 0:2f6112d75752 | 91 | unsigned long currentmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 92 | unsigned long prevmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 93 | unsigned long intervalmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 94 | unsigned long prevIntervalmillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 95 | unsigned long intervalmillisArray[MAXARRAY] = {1000}; |
| jojisdogakane | 4:a0150e0f285b | 96 | unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1000}; |
| jojisdogakane | 0:2f6112d75752 | 97 | float conArray[MAXCONARRAY] = {0.5}; |
| jojisdogakane | 0:2f6112d75752 | 98 | |
| jojisdogakane | 0:2f6112d75752 | 99 | //float stressArray[MAXSTRESSARRAY] = {0.5}; |
| jojisdogakane | 0:2f6112d75752 | 100 | |
| jojisdogakane | 0:2f6112d75752 | 101 | float prevFilteredAngle = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 102 | float happyRatio = 0.0; |
| jojisdogakane | 4:a0150e0f285b | 103 | float happyRatioNow = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 104 | float prevRelaxRatio = 1.0; |
| jojisdogakane | 0:2f6112d75752 | 105 | float prevFilteredDistRatio = 0.0; |
| jojisdogakane | 4:a0150e0f285b | 106 | |
| jojisdogakane | 4:a0150e0f285b | 107 | #define HR_THRESHOLD 2 |
| jojisdogakane | 6:35f536162183 | 108 | int HRThreshold[] = {100, 1000}; |
| jojisdogakane | 4:a0150e0f285b | 109 | |
| jojisdogakane | 0:2f6112d75752 | 110 | |
| jojisdogakane | 4:a0150e0f285b | 111 | // 気分判定フラグ |
| jojisdogakane | 4:a0150e0f285b | 112 | #define IS_RELAX 0 |
| jojisdogakane | 4:a0150e0f285b | 113 | #define IS_EXCITED 1 |
| jojisdogakane | 4:a0150e0f285b | 114 | #define IS_HAPPY 2 |
| jojisdogakane | 4:a0150e0f285b | 115 | #define IS_INTERESTED 3 |
| jojisdogakane | 4:a0150e0f285b | 116 | #define IS_STRESSED 4 // IS_STRESSEDはMOODの個数としても扱うので最後の番号にすること |
| jojisdogakane | 4:a0150e0f285b | 117 | #define MOOD_COUNT IS_STRESSED + 1 |
| jojisdogakane | 0:2f6112d75752 | 118 | |
| jojisdogakane | 4:a0150e0f285b | 119 | // 気分判定係数 |
| jojisdogakane | 4:a0150e0f285b | 120 | #define CONMARGIN 0.85 |
| jojisdogakane | 4:a0150e0f285b | 121 | #define RELAXMARGIN 0.2 |
| jojisdogakane | 4:a0150e0f285b | 122 | #define HAPPYMARGIN 0.225 |
| jojisdogakane | 0:2f6112d75752 | 123 | |
| jojisdogakane | 4:a0150e0f285b | 124 | // 気分判定フラグを索引にする |
| jojisdogakane | 4:a0150e0f285b | 125 | int MoodColorR[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 126 | int MoodColorG[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 127 | int MoodColorB[NEOPIXEL_COUNT][MOOD_COUNT]; |
| jojisdogakane | 4:a0150e0f285b | 128 | |
| jojisdogakane | 0:2f6112d75752 | 129 | |
| jojisdogakane | 4:a0150e0f285b | 130 | unsigned char RainbowR[] = { 50, 50, 50, 127, 200, 255}; |
| jojisdogakane | 4:a0150e0f285b | 131 | unsigned char RainbowG[] = { 50, 127, 255, 127, 100, 50}; |
| jojisdogakane | 4:a0150e0f285b | 132 | unsigned char RainbowB[] = {255, 127, 50, 50, 50, 50}; |
| jojisdogakane | 4:a0150e0f285b | 133 | |
| jojisdogakane | 6:35f536162183 | 134 | unsigned char HappyR[] = { 50, 50, 50, 127, 200, 255}; |
| jojisdogakane | 6:35f536162183 | 135 | unsigned char HappyG[] = { 50, 127, 255, 127, 100, 50}; |
| jojisdogakane | 6:35f536162183 | 136 | unsigned char HappyB[] = {255, 127, 50, 50, 50, 50}; |
| jojisdogakane | 6:35f536162183 | 137 | |
| jojisdogakane | 6:35f536162183 | 138 | unsigned char RelaxR[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 4:a0150e0f285b | 139 | unsigned char RelaxG[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 6:35f536162183 | 140 | unsigned char RelaxB[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 0:2f6112d75752 | 141 | |
| jojisdogakane | 4:a0150e0f285b | 142 | unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 143 | unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100}; |
| jojisdogakane | 6:35f536162183 | 144 | unsigned char ExciteB[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 6:35f536162183 | 145 | |
| jojisdogakane | 6:35f536162183 | 146 | unsigned char StressR[] = {100, 100, 100, 100, 100, 100}; |
| jojisdogakane | 4:a0150e0f285b | 147 | unsigned char StressG[] = { 0, 0, 0, 0, 0, 0}; |
| jojisdogakane | 6:35f536162183 | 148 | unsigned char StressB[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 149 | |
| jojisdogakane | 4:a0150e0f285b | 150 | unsigned char InterestR[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 4:a0150e0f285b | 151 | unsigned char InterestG[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 6:35f536162183 | 152 | unsigned char InterestB[] = {255, 255, 255, 255, 255, 255}; |
| jojisdogakane | 0:2f6112d75752 | 153 | |
| jojisdogakane | 0:2f6112d75752 | 154 | float rainbowOffset = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 155 | |
| jojisdogakane | 0:2f6112d75752 | 156 | bool bConcentration; |
| jojisdogakane | 0:2f6112d75752 | 157 | unsigned long loopCounter = 0; |
| jojisdogakane | 0:2f6112d75752 | 158 | int lightPos = 1; |
| jojisdogakane | 0:2f6112d75752 | 159 | int prevLightPos = 1; |
| jojisdogakane | 0:2f6112d75752 | 160 | |
| jojisdogakane | 0:2f6112d75752 | 161 | double relaxRatio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 162 | float conRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 163 | float conRatioNow = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 164 | double HR_ratio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 165 | |
| jojisdogakane | 0:2f6112d75752 | 166 | unsigned long IBI; |
| jojisdogakane | 0:2f6112d75752 | 167 | |
| jojisdogakane | 0:2f6112d75752 | 168 | int x3, y3; |
| jojisdogakane | 0:2f6112d75752 | 169 | |
| jojisdogakane | 0:2f6112d75752 | 170 | int mainPixelCounter; |
| jojisdogakane | 0:2f6112d75752 | 171 | |
| jojisdogakane | 0:2f6112d75752 | 172 | int prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 173 | int prevprevInterval; |
| jojisdogakane | 0:2f6112d75752 | 174 | |
| jojisdogakane | 0:2f6112d75752 | 175 | void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) |
| jojisdogakane | 0:2f6112d75752 | 176 | { |
| jojisdogakane | 0:2f6112d75752 | 177 | ble.gap().startAdvertising(); // restart advertising |
| jojisdogakane | 0:2f6112d75752 | 178 | } |
| jojisdogakane | 0:2f6112d75752 | 179 | /* |
| jojisdogakane | 0:2f6112d75752 | 180 | void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback) |
| jojisdogakane | 0:2f6112d75752 | 181 | { |
| jojisdogakane | 0:2f6112d75752 | 182 | ble.gap().startAdvertising(); // restart advertising |
| jojisdogakane | 0:2f6112d75752 | 183 | } |
| jojisdogakane | 0:2f6112d75752 | 184 | */ |
| jojisdogakane | 0:2f6112d75752 | 185 | |
| jojisdogakane | 0:2f6112d75752 | 186 | uint8_t BPM = 0; |
| jojisdogakane | 0:2f6112d75752 | 187 | float sensorValue = 0; |
| jojisdogakane | 0:2f6112d75752 | 188 | Timer t; |
| jojisdogakane | 0:2f6112d75752 | 189 | Timer tRand; |
| jojisdogakane | 0:2f6112d75752 | 190 | |
| jojisdogakane | 0:2f6112d75752 | 191 | float redVal = 0; |
| jojisdogakane | 0:2f6112d75752 | 192 | float blueVal = 255; |
| jojisdogakane | 0:2f6112d75752 | 193 | float greenVal = 0; |
| jojisdogakane | 0:2f6112d75752 | 194 | |
| jojisdogakane | 0:2f6112d75752 | 195 | Ticker neopixelTick; |
| jojisdogakane | 0:2f6112d75752 | 196 | |
| jojisdogakane | 0:2f6112d75752 | 197 | UARTService *uartServicePtr; |
| jojisdogakane | 0:2f6112d75752 | 198 | |
| jojisdogakane | 0:2f6112d75752 | 199 | void neopixelTick_Handler() { |
| jojisdogakane | 0:2f6112d75752 | 200 | return; |
| jojisdogakane | 0:2f6112d75752 | 201 | } |
| jojisdogakane | 0:2f6112d75752 | 202 | |
| jojisdogakane | 4:a0150e0f285b | 203 | void SetMoodColor() { |
| jojisdogakane | 4:a0150e0f285b | 204 | int i = 0; |
| jojisdogakane | 4:a0150e0f285b | 205 | |
| jojisdogakane | 4:a0150e0f285b | 206 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 207 | MoodColorR[i][IS_RELAX] = RelaxR[i]; |
| jojisdogakane | 4:a0150e0f285b | 208 | MoodColorG[i][IS_RELAX] = RelaxG[i]; |
| jojisdogakane | 4:a0150e0f285b | 209 | MoodColorB[i][IS_RELAX] = RelaxB[i]; |
| jojisdogakane | 4:a0150e0f285b | 210 | } |
| jojisdogakane | 4:a0150e0f285b | 211 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 212 | MoodColorR[i][IS_EXCITED] = ExciteR[i]; |
| jojisdogakane | 4:a0150e0f285b | 213 | MoodColorG[i][IS_EXCITED] = ExciteG[i]; |
| jojisdogakane | 4:a0150e0f285b | 214 | MoodColorB[i][IS_EXCITED] = ExciteB[i]; |
| jojisdogakane | 4:a0150e0f285b | 215 | } |
| jojisdogakane | 4:a0150e0f285b | 216 | |
| jojisdogakane | 4:a0150e0f285b | 217 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 218 | MoodColorR[i][IS_HAPPY] = HappyR[i]; |
| jojisdogakane | 4:a0150e0f285b | 219 | MoodColorG[i][IS_HAPPY] = HappyG[i]; |
| jojisdogakane | 4:a0150e0f285b | 220 | MoodColorB[i][IS_HAPPY] = HappyB[i]; |
| jojisdogakane | 4:a0150e0f285b | 221 | } |
| jojisdogakane | 4:a0150e0f285b | 222 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 223 | MoodColorR[i][IS_INTERESTED] = InterestR[i]; |
| jojisdogakane | 4:a0150e0f285b | 224 | MoodColorG[i][IS_INTERESTED] = InterestG[i]; |
| jojisdogakane | 4:a0150e0f285b | 225 | MoodColorB[i][IS_INTERESTED] = InterestB[i]; |
| jojisdogakane | 4:a0150e0f285b | 226 | } |
| jojisdogakane | 4:a0150e0f285b | 227 | for(i = 0; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 228 | MoodColorR[i][IS_STRESSED] = StressR[i]; |
| jojisdogakane | 4:a0150e0f285b | 229 | MoodColorG[i][IS_STRESSED] = StressG[i]; |
| jojisdogakane | 4:a0150e0f285b | 230 | MoodColorB[i][IS_STRESSED] = StressB[i]; |
| jojisdogakane | 4:a0150e0f285b | 231 | } |
| jojisdogakane | 0:2f6112d75752 | 232 | } |
| jojisdogakane | 0:2f6112d75752 | 233 | |
| jojisdogakane | 4:a0150e0f285b | 234 | // 現在の気分の判定 |
| jojisdogakane | 4:a0150e0f285b | 235 | int getMood(int bpm, double relaxRatio, double happyRatio, double conRatio) { |
| jojisdogakane | 4:a0150e0f285b | 236 | // 第一優先：集中 |
| jojisdogakane | 4:a0150e0f285b | 237 | // 集中係数が0.95以上なら集中と判定 |
| jojisdogakane | 4:a0150e0f285b | 238 | if(conRatio >= CONMARGIN) |
| jojisdogakane | 4:a0150e0f285b | 239 | { |
| jojisdogakane | 4:a0150e0f285b | 240 | return IS_INTERESTED; |
| jojisdogakane | 4:a0150e0f285b | 241 | } |
| jojisdogakane | 0:2f6112d75752 | 242 | |
| jojisdogakane | 4:a0150e0f285b | 243 | // 第二優先：Stress |
| jojisdogakane | 4:a0150e0f285b | 244 | // リラックス係数が0.2以下 |
| jojisdogakane | 4:a0150e0f285b | 245 | if(relaxRatio <= RELAXMARGIN) { |
| jojisdogakane | 4:a0150e0f285b | 246 | return IS_STRESSED; |
| jojisdogakane | 4:a0150e0f285b | 247 | } |
| jojisdogakane | 0:2f6112d75752 | 248 | |
| jojisdogakane | 4:a0150e0f285b | 249 | // 第三優先：Happy |
| jojisdogakane | 4:a0150e0f285b | 250 | // Happy係数が0.2以上 |
| jojisdogakane | 4:a0150e0f285b | 251 | if(happyRatio >= HAPPYMARGIN && relaxRatio > 0.5) { |
| jojisdogakane | 4:a0150e0f285b | 252 | return IS_HAPPY; |
| jojisdogakane | 4:a0150e0f285b | 253 | } |
| jojisdogakane | 0:2f6112d75752 | 254 | |
| jojisdogakane | 4:a0150e0f285b | 255 | // それ以外は興奮度合い |
| jojisdogakane | 4:a0150e0f285b | 256 | // 心拍数の閾値判定 |
| jojisdogakane | 4:a0150e0f285b | 257 | int i = 0; |
| jojisdogakane | 4:a0150e0f285b | 258 | for(i = 0; i < HR_THRESHOLD; i++) { |
| jojisdogakane | 4:a0150e0f285b | 259 | if (BPM <= HRThreshold[i]) { |
| jojisdogakane | 0:2f6112d75752 | 260 | break; |
| jojisdogakane | 0:2f6112d75752 | 261 | } |
| jojisdogakane | 0:2f6112d75752 | 262 | } |
| jojisdogakane | 4:a0150e0f285b | 263 | switch(i) { |
| jojisdogakane | 4:a0150e0f285b | 264 | // 第１領域：NORMAL |
| jojisdogakane | 4:a0150e0f285b | 265 | case 0: |
| jojisdogakane | 4:a0150e0f285b | 266 | return IS_RELAX; |
| jojisdogakane | 4:a0150e0f285b | 267 | // 第２領域：EXCITED1 |
| jojisdogakane | 4:a0150e0f285b | 268 | case 1: |
| jojisdogakane | 4:a0150e0f285b | 269 | return IS_EXCITED; |
| jojisdogakane | 0:2f6112d75752 | 270 | } |
| jojisdogakane | 0:2f6112d75752 | 271 | |
| jojisdogakane | 4:a0150e0f285b | 272 | return IS_RELAX; |
| jojisdogakane | 4:a0150e0f285b | 273 | } |
| jojisdogakane | 0:2f6112d75752 | 274 | |
| jojisdogakane | 4:a0150e0f285b | 275 | //Serial mySerial(USBTX, USBRX); |
| jojisdogakane | 4:a0150e0f285b | 276 | //Serial mySerial(P0_9, P0_11); |
| jojisdogakane | 0:2f6112d75752 | 277 | |
| ayaando | 1:f28bdfaf3005 | 278 | // MACアドレスを取得してBLEのデバイス名に組み込む |
| ayaando | 1:f28bdfaf3005 | 279 | void setBleDeviceName(char *deviceName) |
| ayaando | 1:f28bdfaf3005 | 280 | { |
| ayaando | 1:f28bdfaf3005 | 281 | int i; |
| ayaando | 1:f28bdfaf3005 | 282 | |
| ayaando | 1:f28bdfaf3005 | 283 | //BLEProtocol::AddressType_t type; //< The type of the BLE address. |
| ayaando | 1:f28bdfaf3005 | 284 | //BLEProtocol::AddressBytes_t mac_address; //< The BLE address. // typedef uint8_t AddressBytes_t[ADDR_LEN] |
| ayaando | 1:f28bdfaf3005 | 285 | |
| ayaando | 1:f28bdfaf3005 | 286 | ble_gap_addr_t mac_address; |
| ayaando | 1:f28bdfaf3005 | 287 | |
| ayaando | 1:f28bdfaf3005 | 288 | if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){ |
| ayaando | 1:f28bdfaf3005 | 289 | return; |
| ayaando | 1:f28bdfaf3005 | 290 | } |
| ayaando | 1:f28bdfaf3005 | 291 | |
| ayaando | 1:f28bdfaf3005 | 292 | // ADDR_LENは6 |
| ayaando | 1:f28bdfaf3005 | 293 | for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){ |
| ayaando | 1:f28bdfaf3005 | 294 | sprintf(deviceName, "%s%02x", deviceName, (mac_address.addr)[i]); |
| ayaando | 1:f28bdfaf3005 | 295 | } |
| ayaando | 1:f28bdfaf3005 | 296 | sprintf(deviceName, "%s%s", deviceName, "\0"); |
| ayaando | 1:f28bdfaf3005 | 297 | |
| ayaando | 1:f28bdfaf3005 | 298 | /* |
| ayaando | 1:f28bdfaf3005 | 299 | if(ble.getAddress(&type, mac_address) == BLE_ERROR_NONE){ |
| ayaando | 1:f28bdfaf3005 | 300 | // ADDR_LENは6 |
| ayaando | 1:f28bdfaf3005 | 301 | for(i=BLEProtocol::ADDR_LEN-1; i>=0; i--){ |
| ayaando | 1:f28bdfaf3005 | 302 | sprintf(deviceName, "%s%02x", deviceName, mac_address[i]); |
| ayaando | 1:f28bdfaf3005 | 303 | } |
| ayaando | 1:f28bdfaf3005 | 304 | sprintf(deviceName, "%s%s", deviceName, "\0"); |
| ayaando | 1:f28bdfaf3005 | 305 | } |
| ayaando | 1:f28bdfaf3005 | 306 | */ |
| ayaando | 1:f28bdfaf3005 | 307 | } |
| jojisdogakane | 0:2f6112d75752 | 308 | |
| jojisdogakane | 0:2f6112d75752 | 309 | int main(void) |
| jojisdogakane | 0:2f6112d75752 | 310 | { |
| jojisdogakane | 0:2f6112d75752 | 311 | char msgIBI[100]; |
| jojisdogakane | 0:2f6112d75752 | 312 | int nMsgIBILen = 0; |
| jojisdogakane | 0:2f6112d75752 | 313 | char msgHappy[10]; |
| jojisdogakane | 0:2f6112d75752 | 314 | int nMsgHappyLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 315 | char msgConcentration[10]; |
| jojisdogakane | 0:2f6112d75752 | 316 | int nMsgConcentrationLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 317 | char msgRelax[10]; |
| jojisdogakane | 0:2f6112d75752 | 318 | int nMsgRelaxLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 319 | |
| jojisdogakane | 0:2f6112d75752 | 320 | char msg[256]; |
| jojisdogakane | 0:2f6112d75752 | 321 | int nMsgLen = 0; |
| jojisdogakane | 0:2f6112d75752 | 322 | |
| jojisdogakane | 0:2f6112d75752 | 323 | float Brightness = 1.0; |
| jojisdogakane | 0:2f6112d75752 | 324 | bool brighten = true; |
| jojisdogakane | 0:2f6112d75752 | 325 | float brightenSpeedMs = 1000.0; |
| jojisdogakane | 0:2f6112d75752 | 326 | float colorSpeedMs = 1000.0; |
| jojisdogakane | 0:2f6112d75752 | 327 | float conRatioShow = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 328 | |
| jojisdogakane | 0:2f6112d75752 | 329 | int i; |
| jojisdogakane | 0:2f6112d75752 | 330 | unsigned long lightPrevMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 331 | |
| jojisdogakane | 0:2f6112d75752 | 332 | int currentBPM; |
| jojisdogakane | 0:2f6112d75752 | 333 | |
| jojisdogakane | 0:2f6112d75752 | 334 | float fBeatIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 335 | |
| jojisdogakane | 0:2f6112d75752 | 336 | static uint8_t rrIntervalH = 0; |
| jojisdogakane | 0:2f6112d75752 | 337 | static uint8_t rrIntervalL = 1; |
| jojisdogakane | 6:35f536162183 | 338 | |
| jojisdogakane | 6:35f536162183 | 339 | int r = 0; |
| jojisdogakane | 6:35f536162183 | 340 | int g = 0; |
| jojisdogakane | 6:35f536162183 | 341 | int b = 0; |
| jojisdogakane | 6:35f536162183 | 342 | int brightnessSum = 0; |
| jojisdogakane | 6:35f536162183 | 343 | int brightnessMax = 0; |
| jojisdogakane | 0:2f6112d75752 | 344 | |
| jojisdogakane | 0:2f6112d75752 | 345 | neopixel_strip_t neopixel1; |
| jojisdogakane | 0:2f6112d75752 | 346 | neopixel_init(&neopixel1, P0_7, 1); |
| jojisdogakane | 0:2f6112d75752 | 347 | neopixel_clear(&neopixel1); |
| jojisdogakane | 0:2f6112d75752 | 348 | |
| jojisdogakane | 0:2f6112d75752 | 349 | neopixel_strip_t neopixel2; |
| jojisdogakane | 0:2f6112d75752 | 350 | neopixel_init(&neopixel2, P0_8, 1); |
| jojisdogakane | 0:2f6112d75752 | 351 | neopixel_clear(&neopixel2); |
| jojisdogakane | 0:2f6112d75752 | 352 | |
| jojisdogakane | 0:2f6112d75752 | 353 | neopixel_strip_t neopixel3; |
| jojisdogakane | 0:2f6112d75752 | 354 | neopixel_init(&neopixel3, P0_10, 1); |
| jojisdogakane | 0:2f6112d75752 | 355 | neopixel_clear(&neopixel3); |
| jojisdogakane | 0:2f6112d75752 | 356 | |
| jojisdogakane | 0:2f6112d75752 | 357 | neopixel_strip_t neopixel4; |
| jojisdogakane | 0:2f6112d75752 | 358 | neopixel_init(&neopixel4, P0_9, 1); |
| jojisdogakane | 0:2f6112d75752 | 359 | neopixel_clear(&neopixel4); |
| jojisdogakane | 0:2f6112d75752 | 360 | |
| jojisdogakane | 0:2f6112d75752 | 361 | neopixel_strip_t neopixel5; |
| jojisdogakane | 0:2f6112d75752 | 362 | neopixel_init(&neopixel5, P0_15, 1); |
| jojisdogakane | 0:2f6112d75752 | 363 | neopixel_clear(&neopixel5); |
| jojisdogakane | 0:2f6112d75752 | 364 | |
| jojisdogakane | 0:2f6112d75752 | 365 | neopixel_strip_t neopixel6; |
| jojisdogakane | 0:2f6112d75752 | 366 | neopixel_init(&neopixel6, P0_11, 1); |
| jojisdogakane | 0:2f6112d75752 | 367 | neopixel_clear(&neopixel6); |
| jojisdogakane | 0:2f6112d75752 | 368 | |
| jojisdogakane | 0:2f6112d75752 | 369 | neopixel_strip_t *pixels[6]; |
| jojisdogakane | 0:2f6112d75752 | 370 | |
| jojisdogakane | 0:2f6112d75752 | 371 | pixels[0] = &neopixel1; |
| jojisdogakane | 0:2f6112d75752 | 372 | pixels[1] = &neopixel2; |
| jojisdogakane | 0:2f6112d75752 | 373 | pixels[2] = &neopixel3; |
| jojisdogakane | 0:2f6112d75752 | 374 | pixels[3] = &neopixel4; |
| jojisdogakane | 0:2f6112d75752 | 375 | pixels[4] = &neopixel5; |
| jojisdogakane | 0:2f6112d75752 | 376 | pixels[5] = &neopixel6; |
| jojisdogakane | 0:2f6112d75752 | 377 | |
| jojisdogakane | 4:a0150e0f285b | 378 | |
| jojisdogakane | 6:35f536162183 | 379 | |
| jojisdogakane | 4:a0150e0f285b | 380 | SetMoodColor(); |
| jojisdogakane | 4:a0150e0f285b | 381 | |
| jojisdogakane | 0:2f6112d75752 | 382 | ble.init(); |
| jojisdogakane | 0:2f6112d75752 | 383 | ble.gap().onDisconnection(disconnectionCallback); |
| jojisdogakane | 0:2f6112d75752 | 384 | |
| jojisdogakane | 4:a0150e0f285b | 385 | |
| jojisdogakane | 0:2f6112d75752 | 386 | UARTService uartService(ble); |
| jojisdogakane | 0:2f6112d75752 | 387 | uartServicePtr = &uartService; |
| jojisdogakane | 0:2f6112d75752 | 388 | |
| ayaando | 2:221df6d014f5 | 389 | // ★OTA用 |
| ayaando | 2:221df6d014f5 | 390 | dfuService = new DFUService(ble, NULL); |
| jojisdogakane | 4:a0150e0f285b | 391 | |
| jojisdogakane | 0:2f6112d75752 | 392 | /* Setup primary service. */ |
| jojisdogakane | 0:2f6112d75752 | 393 | HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST); |
| jojisdogakane | 0:2f6112d75752 | 394 | // HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER); |
| jojisdogakane | 0:2f6112d75752 | 395 | |
| jojisdogakane | 0:2f6112d75752 | 396 | // BatteryService batteryService(ble, 100); |
| jojisdogakane | 0:2f6112d75752 | 397 | |
| jojisdogakane | 0:2f6112d75752 | 398 | /* Setup auxiliary service. */ |
| ayaando | 2:221df6d014f5 | 399 | //DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1"); |
| ayaando | 2:221df6d014f5 | 400 | // ★ |
| ayaando | 2:221df6d014f5 | 401 | DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", "SN1", "0.0.1", "0.0.1", "0.0.1"); |
| jojisdogakane | 0:2f6112d75752 | 402 | |
| ayaando | 1:f28bdfaf3005 | 403 | // MACアドレスをDEVICE_NAMEに組み込む |
| ayaando | 1:f28bdfaf3005 | 404 | setBleDeviceName(DEVICE_NAME); |
| ayaando | 1:f28bdfaf3005 | 405 | |
| jojisdogakane | 0:2f6112d75752 | 406 | /* Setup advertising. */ |
| jojisdogakane | 0:2f6112d75752 | 407 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); |
| jojisdogakane | 0:2f6112d75752 | 408 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); |
| jojisdogakane | 0:2f6112d75752 | 409 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); |
| jojisdogakane | 0:2f6112d75752 | 410 | ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); |
| jojisdogakane | 0:2f6112d75752 | 411 | ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); |
| jojisdogakane | 0:2f6112d75752 | 412 | ble.gap().setAdvertisingInterval(800); /* 1000ms */ |
| jojisdogakane | 0:2f6112d75752 | 413 | ble.gap().startAdvertising(); |
| jojisdogakane | 0:2f6112d75752 | 414 | |
| jojisdogakane | 4:a0150e0f285b | 415 | for( i = 0; i < NEOPIXEL_COUNT; i++) |
| jojisdogakane | 6:35f536162183 | 416 | { |
| jojisdogakane | 6:35f536162183 | 417 | // 起動時の色パターンを設定 |
| jojisdogakane | 6:35f536162183 | 418 | ColorArrayR[i] = RainbowR[i]; |
| jojisdogakane | 6:35f536162183 | 419 | ColorArrayG[i] = RainbowG[i]; |
| jojisdogakane | 6:35f536162183 | 420 | ColorArrayB[i] = RainbowB[i]; |
| jojisdogakane | 6:35f536162183 | 421 | |
| jojisdogakane | 4:a0150e0f285b | 422 | TargetColorArrayR[i] = ColorArrayR[i]; |
| jojisdogakane | 4:a0150e0f285b | 423 | TargetColorArrayG[i] = ColorArrayG[i]; |
| jojisdogakane | 4:a0150e0f285b | 424 | TargetColorArrayB[i] = ColorArrayB[i]; |
| jojisdogakane | 6:35f536162183 | 425 | |
| jojisdogakane | 6:35f536162183 | 426 | // 輝度調整 |
| jojisdogakane | 6:35f536162183 | 427 | r = ColorArrayR[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 428 | g = ColorArrayG[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 429 | b = ColorArrayB[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 430 | |
| jojisdogakane | 6:35f536162183 | 431 | brightnessSum = r + g + b; |
| jojisdogakane | 6:35f536162183 | 432 | |
| jojisdogakane | 6:35f536162183 | 433 | // 輝度の合計値が上限を超えた場合は補正する |
| jojisdogakane | 6:35f536162183 | 434 | if(brightnessSum > BRIGHTNESS_MAX) { |
| jojisdogakane | 6:35f536162183 | 435 | r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 6:35f536162183 | 436 | g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 6:35f536162183 | 437 | b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 6:35f536162183 | 438 | } |
| jojisdogakane | 6:35f536162183 | 439 | |
| jojisdogakane | 0:2f6112d75752 | 440 | neopixel_set_color( |
| jojisdogakane | 0:2f6112d75752 | 441 | pixels[i], |
| jojisdogakane | 0:2f6112d75752 | 442 | 0, |
| jojisdogakane | 6:35f536162183 | 443 | short(r), |
| jojisdogakane | 6:35f536162183 | 444 | short(g), |
| jojisdogakane | 6:35f536162183 | 445 | short(b)); |
| jojisdogakane | 0:2f6112d75752 | 446 | neopixel_show(pixels[i]); |
| jojisdogakane | 0:2f6112d75752 | 447 | } |
| jojisdogakane | 4:a0150e0f285b | 448 | |
| jojisdogakane | 0:2f6112d75752 | 449 | t.start(); |
| jojisdogakane | 0:2f6112d75752 | 450 | |
| jojisdogakane | 0:2f6112d75752 | 451 | prevmillis = t.read_ms(); |
| jojisdogakane | 0:2f6112d75752 | 452 | |
| jojisdogakane | 0:2f6112d75752 | 453 | bool bPulseEvent = false; |
| jojisdogakane | 0:2f6112d75752 | 454 | |
| jojisdogakane | 0:2f6112d75752 | 455 | float degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 456 | float degRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 457 | |
| jojisdogakane | 0:2f6112d75752 | 458 | bool bSwingR = false; |
| jojisdogakane | 0:2f6112d75752 | 459 | |
| jojisdogakane | 0:2f6112d75752 | 460 | float swingIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 461 | |
| jojisdogakane | 0:2f6112d75752 | 462 | float midPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 463 | |
| jojisdogakane | 0:2f6112d75752 | 464 | // neopixelTick.attach(neopixelTick_Handler, 0.005); |
| jojisdogakane | 0:2f6112d75752 | 465 | |
| jojisdogakane | 0:2f6112d75752 | 466 | while (1) { |
| jojisdogakane | 0:2f6112d75752 | 467 | |
| jojisdogakane | 0:2f6112d75752 | 468 | // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); |
| jojisdogakane | 0:2f6112d75752 | 469 | |
| jojisdogakane | 0:2f6112d75752 | 470 | float currentValue = HRSigIn; |
| jojisdogakane | 0:2f6112d75752 | 471 | |
| jojisdogakane | 0:2f6112d75752 | 472 | bPulseEvent = false; |
| jojisdogakane | 0:2f6112d75752 | 473 | |
| jojisdogakane | 4:a0150e0f285b | 474 | if ((currentValue <= 0.3) && (sensorValue > 0.3)) |
| jojisdogakane | 0:2f6112d75752 | 475 | { |
| jojisdogakane | 0:2f6112d75752 | 476 | bPulseEvent = true; |
| jojisdogakane | 0:2f6112d75752 | 477 | } |
| jojisdogakane | 0:2f6112d75752 | 478 | |
| jojisdogakane | 0:2f6112d75752 | 479 | int sens = 1023 * currentValue; |
| jojisdogakane | 0:2f6112d75752 | 480 | |
| jojisdogakane | 0:2f6112d75752 | 481 | // printf("S%d", sens); |
| jojisdogakane | 0:2f6112d75752 | 482 | // printf("S%d\n", sens); |
| jojisdogakane | 0:2f6112d75752 | 483 | |
| jojisdogakane | 0:2f6112d75752 | 484 | sensorValue = currentValue; |
| jojisdogakane | 0:2f6112d75752 | 485 | |
| jojisdogakane | 0:2f6112d75752 | 486 | if(bPulseEvent) { |
| jojisdogakane | 0:2f6112d75752 | 487 | fBeatIndex = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 488 | // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); |
| jojisdogakane | 0:2f6112d75752 | 489 | |
| jojisdogakane | 0:2f6112d75752 | 490 | // 現在の時刻を取得 |
| jojisdogakane | 0:2f6112d75752 | 491 | // t.stop(); // stop, startは時間計測の結果を有意に短くしてしまうので使わない |
| jojisdogakane | 0:2f6112d75752 | 492 | currentmillis = t.read_ms(); |
| jojisdogakane | 0:2f6112d75752 | 493 | // t.start(); |
| jojisdogakane | 0:2f6112d75752 | 494 | // 現在時刻から過去時刻を引き、間隔を求める |
| jojisdogakane | 0:2f6112d75752 | 495 | intervalmillis = currentmillis - prevmillis; |
| jojisdogakane | 0:2f6112d75752 | 496 | |
| jojisdogakane | 0:2f6112d75752 | 497 | // 心拍の間の間隔 |
| jojisdogakane | 0:2f6112d75752 | 498 | IBI = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 499 | |
| jojisdogakane | 0:2f6112d75752 | 500 | // 現在の時刻を過去時刻として記録 |
| jojisdogakane | 0:2f6112d75752 | 501 | prevmillis = currentmillis; |
| jojisdogakane | 0:2f6112d75752 | 502 | |
| jojisdogakane | 0:2f6112d75752 | 503 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 504 | double averageIntervalMillis; |
| jojisdogakane | 0:2f6112d75752 | 505 | |
| jojisdogakane | 0:2f6112d75752 | 506 | for(i = 0; i < MAXARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 507 | { |
| jojisdogakane | 0:2f6112d75752 | 508 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 509 | if(i != MAXARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 510 | { |
| jojisdogakane | 0:2f6112d75752 | 511 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 512 | intervalmillisArray[i] = intervalmillisArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 513 | } |
| jojisdogakane | 0:2f6112d75752 | 514 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 515 | else { |
| jojisdogakane | 0:2f6112d75752 | 516 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 517 | intervalmillisArray[i] = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 518 | } |
| jojisdogakane | 0:2f6112d75752 | 519 | } |
| jojisdogakane | 0:2f6112d75752 | 520 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 521 | unsigned long sumIntervalMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 522 | unsigned long sumIntervalMillisV = 0; |
| jojisdogakane | 0:2f6112d75752 | 523 | if (intervalmillisArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 524 | { |
| jojisdogakane | 0:2f6112d75752 | 525 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 526 | averageIntervalMillis = 0; |
| jojisdogakane | 0:2f6112d75752 | 527 | for(i = 0; i < MAXARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 528 | { |
| jojisdogakane | 0:2f6112d75752 | 529 | sumIntervalMillis += intervalmillisArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 530 | sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 531 | } |
| jojisdogakane | 0:2f6112d75752 | 532 | double squ; |
| jojisdogakane | 0:2f6112d75752 | 533 | double var; |
| jojisdogakane | 0:2f6112d75752 | 534 | double stdDev; |
| jojisdogakane | 0:2f6112d75752 | 535 | |
| jojisdogakane | 0:2f6112d75752 | 536 | averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY; |
| jojisdogakane | 0:2f6112d75752 | 537 | |
| jojisdogakane | 0:2f6112d75752 | 538 | squ = sumIntervalMillisV - (double)(sumIntervalMillis * sumIntervalMillis) / (double)MAXARRAY; |
| jojisdogakane | 0:2f6112d75752 | 539 | var = squ / (double)(MAXARRAY - 1); |
| jojisdogakane | 0:2f6112d75752 | 540 | stdDev = sqrt(var); |
| jojisdogakane | 0:2f6112d75752 | 541 | |
| jojisdogakane | 0:2f6112d75752 | 542 | BPM = min32_of(60000 / averageIntervalMillis, 255); |
| jojisdogakane | 0:2f6112d75752 | 543 | // if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) { |
| jojisdogakane | 0:2f6112d75752 | 544 | |
| jojisdogakane | 0:2f6112d75752 | 545 | currentBPM = min32_of(60000 / intervalmillis, 255); |
| jojisdogakane | 0:2f6112d75752 | 546 | /* } |
| jojisdogakane | 0:2f6112d75752 | 547 | else { |
| jojisdogakane | 0:2f6112d75752 | 548 | currentBPM = BPM; |
| jojisdogakane | 0:2f6112d75752 | 549 | } */ |
| jojisdogakane | 0:2f6112d75752 | 550 | } |
| jojisdogakane | 0:2f6112d75752 | 551 | /* |
| jojisdogakane | 0:2f6112d75752 | 552 | int IBI1024 = IBI * (1024/1000); |
| jojisdogakane | 0:2f6112d75752 | 553 | |
| jojisdogakane | 0:2f6112d75752 | 554 | bpm[1] = BPM; |
| jojisdogakane | 0:2f6112d75752 | 555 | bpm[6] = (IBI1024 & 0xFF00) >> 8; |
| jojisdogakane | 0:2f6112d75752 | 556 | bpm[7] = (IBI1024 & 0xFF); |
| jojisdogakane | 0:2f6112d75752 | 557 | */ |
| jojisdogakane | 0:2f6112d75752 | 558 | // update bpm |
| jojisdogakane | 0:2f6112d75752 | 559 | if(ble.gap().getState().connected == 1) |
| jojisdogakane | 0:2f6112d75752 | 560 | { |
| jojisdogakane | 0:2f6112d75752 | 561 | hrServiceBPM.updateHeartRate(BPM); |
| jojisdogakane | 0:2f6112d75752 | 562 | // hrServiceIBI.updateHeartRate(IBI); |
| jojisdogakane | 0:2f6112d75752 | 563 | } |
| jojisdogakane | 0:2f6112d75752 | 564 | |
| jojisdogakane | 0:2f6112d75752 | 565 | // printf("B%d\n", BPM); |
| jojisdogakane | 0:2f6112d75752 | 566 | // printf("Q%d\n", IBI); |
| jojisdogakane | 0:2f6112d75752 | 567 | |
| jojisdogakane | 0:2f6112d75752 | 568 | // nMsgLen = sprintf(msg,"B%d\0", BPM); |
| jojisdogakane | 0:2f6112d75752 | 569 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 570 | |
| jojisdogakane | 0:2f6112d75752 | 571 | nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI); |
| jojisdogakane | 0:2f6112d75752 | 572 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 573 | |
| jojisdogakane | 0:2f6112d75752 | 574 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 575 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 576 | { |
| jojisdogakane | 0:2f6112d75752 | 577 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 578 | if(i != MAXHRVARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 579 | { |
| jojisdogakane | 0:2f6112d75752 | 580 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 581 | intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 582 | } |
| jojisdogakane | 0:2f6112d75752 | 583 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 584 | else { |
| jojisdogakane | 0:2f6112d75752 | 585 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 586 | intervalmillisHRVArray[i] = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 587 | } |
| jojisdogakane | 0:2f6112d75752 | 588 | } |
| jojisdogakane | 0:2f6112d75752 | 589 | |
| jojisdogakane | 0:2f6112d75752 | 590 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 591 | int nNNInterval; |
| jojisdogakane | 0:2f6112d75752 | 592 | unsigned long ulIntervalSum = 0; |
| jojisdogakane | 0:2f6112d75752 | 593 | int nSampleCount = 0; |
| jojisdogakane | 0:2f6112d75752 | 594 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 595 | { |
| jojisdogakane | 0:2f6112d75752 | 596 | if(intervalmillisHRVArray[i] > 0) |
| jojisdogakane | 0:2f6112d75752 | 597 | { |
| jojisdogakane | 0:2f6112d75752 | 598 | ulIntervalSum += intervalmillisHRVArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 599 | nSampleCount++; |
| jojisdogakane | 0:2f6112d75752 | 600 | } |
| jojisdogakane | 0:2f6112d75752 | 601 | } |
| jojisdogakane | 0:2f6112d75752 | 602 | |
| jojisdogakane | 0:2f6112d75752 | 603 | float IntervalAverage = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 604 | if(nSampleCount > 0){ |
| jojisdogakane | 0:2f6112d75752 | 605 | IntervalAverage = (float)ulIntervalSum / (float)nSampleCount; |
| jojisdogakane | 0:2f6112d75752 | 606 | } |
| jojisdogakane | 0:2f6112d75752 | 607 | |
| jojisdogakane | 0:2f6112d75752 | 608 | nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR); |
| jojisdogakane | 0:2f6112d75752 | 609 | |
| jojisdogakane | 0:2f6112d75752 | 610 | |
| jojisdogakane | 0:2f6112d75752 | 611 | bool bContinue = false; |
| jojisdogakane | 0:2f6112d75752 | 612 | int nNNxCount = 0; |
| jojisdogakane | 0:2f6112d75752 | 613 | for(i = 0; i < MAXHRVARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 614 | { |
| jojisdogakane | 0:2f6112d75752 | 615 | // if not end |
| jojisdogakane | 0:2f6112d75752 | 616 | if(i < MAXHRVARRAY - 1) { |
| jojisdogakane | 0:2f6112d75752 | 617 | if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval) |
| jojisdogakane | 0:2f6112d75752 | 618 | { |
| jojisdogakane | 0:2f6112d75752 | 619 | if(bContinue == false) |
| jojisdogakane | 0:2f6112d75752 | 620 | { |
| jojisdogakane | 0:2f6112d75752 | 621 | nNNxCount++; |
| jojisdogakane | 0:2f6112d75752 | 622 | } |
| jojisdogakane | 0:2f6112d75752 | 623 | nNNxCount++; |
| jojisdogakane | 0:2f6112d75752 | 624 | bContinue = true; |
| jojisdogakane | 0:2f6112d75752 | 625 | } |
| jojisdogakane | 0:2f6112d75752 | 626 | else { |
| jojisdogakane | 0:2f6112d75752 | 627 | bContinue = false; |
| jojisdogakane | 0:2f6112d75752 | 628 | } |
| jojisdogakane | 0:2f6112d75752 | 629 | } |
| jojisdogakane | 0:2f6112d75752 | 630 | } |
| jojisdogakane | 0:2f6112d75752 | 631 | |
| jojisdogakane | 0:2f6112d75752 | 632 | float diffRatio = (float)nNNxCount / (float)nSampleCount; |
| jojisdogakane | 0:2f6112d75752 | 633 | |
| jojisdogakane | 0:2f6112d75752 | 634 | if(diffRatio > 1) |
| jojisdogakane | 0:2f6112d75752 | 635 | { |
| jojisdogakane | 0:2f6112d75752 | 636 | diffRatio = 1; |
| jojisdogakane | 0:2f6112d75752 | 637 | } |
| jojisdogakane | 0:2f6112d75752 | 638 | |
| jojisdogakane | 0:2f6112d75752 | 639 | // リラックスの係数を算出する |
| jojisdogakane | 0:2f6112d75752 | 640 | relaxRatio = diffRatio; |
| jojisdogakane | 0:2f6112d75752 | 641 | |
| jojisdogakane | 6:35f536162183 | 642 | nMsgRelaxLen = sprintf(msgRelax,"S%0.2f\0", (1-relaxRatio)); |
| jojisdogakane | 0:2f6112d75752 | 643 | |
| jojisdogakane | 0:2f6112d75752 | 644 | // nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio); |
| jojisdogakane | 0:2f6112d75752 | 645 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 646 | |
| jojisdogakane | 0:2f6112d75752 | 647 | // 現在値から最低値を引く |
| jojisdogakane | 0:2f6112d75752 | 648 | // relaxRatio = relaxRatio - RELAX_RATIO_MIN; |
| jojisdogakane | 0:2f6112d75752 | 649 | |
| jojisdogakane | 0:2f6112d75752 | 650 | // 1.0 / 最低値 × 現在値 |
| jojisdogakane | 0:2f6112d75752 | 651 | // relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN)); |
| jojisdogakane | 0:2f6112d75752 | 652 | |
| jojisdogakane | 0:2f6112d75752 | 653 | int x1, y1, x2, y2; |
| jojisdogakane | 0:2f6112d75752 | 654 | int tempx1, tempy1, tempx2, tempy2, tempx3, tempy3; |
| jojisdogakane | 0:2f6112d75752 | 655 | int longx, shortx, longy, shorty; |
| jojisdogakane | 0:2f6112d75752 | 656 | long xx1, xx2, yy1, yy2; |
| jojisdogakane | 0:2f6112d75752 | 657 | |
| jojisdogakane | 0:2f6112d75752 | 658 | int movex, movey; |
| jojisdogakane | 0:2f6112d75752 | 659 | |
| jojisdogakane | 0:2f6112d75752 | 660 | double dist1, dist2; |
| jojisdogakane | 0:2f6112d75752 | 661 | double conRatioRatio; |
| jojisdogakane | 0:2f6112d75752 | 662 | |
| jojisdogakane | 0:2f6112d75752 | 663 | /* |
| jojisdogakane | 0:2f6112d75752 | 664 | // 最後尾の3データから二つのx, y地点を作る。 |
| jojisdogakane | 0:2f6112d75752 | 665 | x1 = intervalmillisArray[nSampleCount - 1]; |
| jojisdogakane | 0:2f6112d75752 | 666 | y1 = intervalmillisArray[nSampleCount - 2]; |
| jojisdogakane | 0:2f6112d75752 | 667 | x2 = intervalmillisArray[nSampleCount - 2]; |
| jojisdogakane | 0:2f6112d75752 | 668 | y2 = intervalmillisArray[nSampleCount - 3]; |
| jojisdogakane | 0:2f6112d75752 | 669 | */ |
| jojisdogakane | 0:2f6112d75752 | 670 | |
| jojisdogakane | 0:2f6112d75752 | 671 | x1 = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 672 | y1 = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 673 | x2 = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 674 | y2 = prevprevInterval; |
| jojisdogakane | 0:2f6112d75752 | 675 | |
| jojisdogakane | 0:2f6112d75752 | 676 | // ２地点を結ぶ直線の長さを算出する。 |
| jojisdogakane | 0:2f6112d75752 | 677 | if(x1 > x2) |
| jojisdogakane | 0:2f6112d75752 | 678 | { |
| jojisdogakane | 0:2f6112d75752 | 679 | longx = x1; |
| jojisdogakane | 0:2f6112d75752 | 680 | shortx = x2; |
| jojisdogakane | 0:2f6112d75752 | 681 | } |
| jojisdogakane | 0:2f6112d75752 | 682 | else { |
| jojisdogakane | 0:2f6112d75752 | 683 | longx = x2; |
| jojisdogakane | 0:2f6112d75752 | 684 | shortx = x1; |
| jojisdogakane | 0:2f6112d75752 | 685 | } |
| jojisdogakane | 0:2f6112d75752 | 686 | |
| jojisdogakane | 0:2f6112d75752 | 687 | if(y1 > y2) |
| jojisdogakane | 0:2f6112d75752 | 688 | { |
| jojisdogakane | 0:2f6112d75752 | 689 | longy = y1; |
| jojisdogakane | 0:2f6112d75752 | 690 | shorty = y2; |
| jojisdogakane | 0:2f6112d75752 | 691 | } |
| jojisdogakane | 0:2f6112d75752 | 692 | else { |
| jojisdogakane | 0:2f6112d75752 | 693 | longy = y2; |
| jojisdogakane | 0:2f6112d75752 | 694 | shorty = y1; |
| jojisdogakane | 0:2f6112d75752 | 695 | } |
| jojisdogakane | 0:2f6112d75752 | 696 | |
| jojisdogakane | 0:2f6112d75752 | 697 | xx1 = longx - shortx; |
| jojisdogakane | 0:2f6112d75752 | 698 | yy1 = longy - shorty; |
| jojisdogakane | 0:2f6112d75752 | 699 | |
| jojisdogakane | 0:2f6112d75752 | 700 | dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1))); |
| jojisdogakane | 0:2f6112d75752 | 701 | |
| jojisdogakane | 0:2f6112d75752 | 702 | dist2 = longx + longy; |
| jojisdogakane | 0:2f6112d75752 | 703 | float distRatio = dist1 / dist2; |
| jojisdogakane | 0:2f6112d75752 | 704 | |
| jojisdogakane | 0:2f6112d75752 | 705 | conRatioNow = 1 - distRatio; |
| jojisdogakane | 0:2f6112d75752 | 706 | |
| jojisdogakane | 0:2f6112d75752 | 707 | // 平均値算出用配列に値を詰める |
| jojisdogakane | 0:2f6112d75752 | 708 | float averageConRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 709 | for(i = 0; i < MAXCONARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 710 | { |
| jojisdogakane | 0:2f6112d75752 | 711 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 712 | if(i != MAXCONARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 713 | { |
| jojisdogakane | 0:2f6112d75752 | 714 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 715 | conArray[i] = conArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 716 | } |
| jojisdogakane | 0:2f6112d75752 | 717 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 718 | else { |
| jojisdogakane | 0:2f6112d75752 | 719 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 720 | conArray[i] = distRatio; |
| jojisdogakane | 0:2f6112d75752 | 721 | } |
| jojisdogakane | 0:2f6112d75752 | 722 | } |
| jojisdogakane | 0:2f6112d75752 | 723 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 724 | if (conArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 725 | { |
| jojisdogakane | 0:2f6112d75752 | 726 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 727 | averageConRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 728 | for(i = 0; i < MAXCONARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 729 | { |
| jojisdogakane | 0:2f6112d75752 | 730 | averageConRatio += conArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 731 | } |
| jojisdogakane | 0:2f6112d75752 | 732 | averageConRatio = averageConRatio / (double)MAXCONARRAY; |
| jojisdogakane | 0:2f6112d75752 | 733 | } |
| jojisdogakane | 0:2f6112d75752 | 734 | /* |
| jojisdogakane | 0:2f6112d75752 | 735 | float averageStressRatio = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 736 | for(i = 0; i < MAXSTRESSARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 737 | { |
| jojisdogakane | 0:2f6112d75752 | 738 | // 最後尾以外の場合 |
| jojisdogakane | 0:2f6112d75752 | 739 | if(i != MAXSTRESSARRAY - 1) |
| jojisdogakane | 0:2f6112d75752 | 740 | { |
| jojisdogakane | 0:2f6112d75752 | 741 | // 次の要素の値をコピーする |
| jojisdogakane | 0:2f6112d75752 | 742 | stressArray[i] = stressArray[i + 1]; |
| jojisdogakane | 0:2f6112d75752 | 743 | } |
| jojisdogakane | 0:2f6112d75752 | 744 | // 最後尾の場合 |
| jojisdogakane | 0:2f6112d75752 | 745 | else { |
| jojisdogakane | 0:2f6112d75752 | 746 | // 最新値を詰める |
| jojisdogakane | 0:2f6112d75752 | 747 | if(x1 > y1) { |
| jojisdogakane | 0:2f6112d75752 | 748 | stressArray[i] = y1 / x1; |
| jojisdogakane | 0:2f6112d75752 | 749 | } |
| jojisdogakane | 0:2f6112d75752 | 750 | else { |
| jojisdogakane | 0:2f6112d75752 | 751 | stressArray[i] = x1 / y1; |
| jojisdogakane | 0:2f6112d75752 | 752 | } |
| jojisdogakane | 0:2f6112d75752 | 753 | } |
| jojisdogakane | 0:2f6112d75752 | 754 | } |
| jojisdogakane | 0:2f6112d75752 | 755 | // 先頭が0の間は何もしない |
| jojisdogakane | 0:2f6112d75752 | 756 | if (stressArray[0] != 0) |
| jojisdogakane | 0:2f6112d75752 | 757 | { |
| jojisdogakane | 0:2f6112d75752 | 758 | // 平均値算出 |
| jojisdogakane | 0:2f6112d75752 | 759 | averageStressRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 760 | for(i = 0; i < MAXSTRESSARRAY ; i++) |
| jojisdogakane | 0:2f6112d75752 | 761 | { |
| jojisdogakane | 0:2f6112d75752 | 762 | averageStressRatio += stressArray[i]; |
| jojisdogakane | 0:2f6112d75752 | 763 | } |
| jojisdogakane | 0:2f6112d75752 | 764 | averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY; |
| jojisdogakane | 0:2f6112d75752 | 765 | } |
| jojisdogakane | 0:2f6112d75752 | 766 | */ |
| jojisdogakane | 0:2f6112d75752 | 767 | |
| jojisdogakane | 0:2f6112d75752 | 768 | /* float rcFilter = RCFILTER2; |
| jojisdogakane | 0:2f6112d75752 | 769 | float filteredDistRatio = 0.0; |
| jojisdogakane | 0:2f6112d75752 | 770 | float a; |
| jojisdogakane | 0:2f6112d75752 | 771 | if(prevFilteredDistRatio != 0) { |
| jojisdogakane | 0:2f6112d75752 | 772 | a = rcFilter * prevFilteredDistRatio; |
| jojisdogakane | 0:2f6112d75752 | 773 | filteredDistRatio = a + (1 - rcFilter) * distRatio; |
| jojisdogakane | 0:2f6112d75752 | 774 | } |
| jojisdogakane | 0:2f6112d75752 | 775 | else { |
| jojisdogakane | 0:2f6112d75752 | 776 | filteredDistRatio = (1 - rcFilter) * distRatio; |
| jojisdogakane | 0:2f6112d75752 | 777 | } |
| jojisdogakane | 0:2f6112d75752 | 778 | prevFilteredDistRatio = filteredDistRatio ; |
| jojisdogakane | 0:2f6112d75752 | 779 | |
| jojisdogakane | 0:2f6112d75752 | 780 | conRatio = filteredDistRatio ; |
| jojisdogakane | 0:2f6112d75752 | 781 | */ |
| jojisdogakane | 0:2f6112d75752 | 782 | conRatio = averageConRatio; |
| jojisdogakane | 0:2f6112d75752 | 783 | conRatio = 1 - conRatio; |
| jojisdogakane | 0:2f6112d75752 | 784 | |
| jojisdogakane | 0:2f6112d75752 | 785 | nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio); |
| jojisdogakane | 0:2f6112d75752 | 786 | |
| jojisdogakane | 0:2f6112d75752 | 787 | // nMsgLen = sprintf(msg,"C%0.2f\0", conRatio); |
| jojisdogakane | 0:2f6112d75752 | 788 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 789 | |
| jojisdogakane | 0:2f6112d75752 | 790 | |
| jojisdogakane | 0:2f6112d75752 | 791 | prevprevInterval = prevInterval; |
| jojisdogakane | 0:2f6112d75752 | 792 | prevInterval = intervalmillis; |
| jojisdogakane | 0:2f6112d75752 | 793 | |
| jojisdogakane | 0:2f6112d75752 | 794 | // (x2, y2)が原点となるように座標を変換する |
| jojisdogakane | 0:2f6112d75752 | 795 | movex = x2 * -1; |
| jojisdogakane | 0:2f6112d75752 | 796 | movey = y2 * -1; |
| jojisdogakane | 0:2f6112d75752 | 797 | |
| jojisdogakane | 0:2f6112d75752 | 798 | tempx1 = x1 - movex; |
| jojisdogakane | 0:2f6112d75752 | 799 | tempy1 = y1 - movey; |
| jojisdogakane | 0:2f6112d75752 | 800 | tempx2 = x2 - movex; |
| jojisdogakane | 0:2f6112d75752 | 801 | tempy2 = y2 - movey; |
| jojisdogakane | 0:2f6112d75752 | 802 | tempx3 = x3 - movex; |
| jojisdogakane | 0:2f6112d75752 | 803 | tempy3 = y3 - movey; |
| jojisdogakane | 0:2f6112d75752 | 804 | |
| jojisdogakane | 0:2f6112d75752 | 805 | // (x2, y2)を中心とした円を考える。 |
| jojisdogakane | 0:2f6112d75752 | 806 | |
| jojisdogakane | 0:2f6112d75752 | 807 | // (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3) |
| jojisdogakane | 0:2f6112d75752 | 808 | double theta = DegreeToRadian(180); |
| jojisdogakane | 0:2f6112d75752 | 809 | |
| jojisdogakane | 0:2f6112d75752 | 810 | double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta); |
| jojisdogakane | 0:2f6112d75752 | 811 | double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta); |
| jojisdogakane | 0:2f6112d75752 | 812 | |
| jojisdogakane | 0:2f6112d75752 | 813 | // ベクトル1とベクトル3の内積を求める |
| jojisdogakane | 0:2f6112d75752 | 814 | double vecInt = tempx1 * rotx3 + tempy1 * roty3; |
| jojisdogakane | 0:2f6112d75752 | 815 | |
| jojisdogakane | 0:2f6112d75752 | 816 | // ベクトル1とベクトル3の外積を求める |
| jojisdogakane | 0:2f6112d75752 | 817 | double vecExt = tempx1 * roty3 - tempy1 * rotx3; |
| jojisdogakane | 0:2f6112d75752 | 818 | |
| jojisdogakane | 0:2f6112d75752 | 819 | // ベクトル1とベクトル3の角度を求める |
| jojisdogakane | 0:2f6112d75752 | 820 | theta = atan2(vecExt, vecInt); |
| jojisdogakane | 0:2f6112d75752 | 821 | |
| jojisdogakane | 0:2f6112d75752 | 822 | double deg = RadianToDegree(theta); |
| jojisdogakane | 0:2f6112d75752 | 823 | |
| jojisdogakane | 0:2f6112d75752 | 824 | midPoint = degRatio; |
| jojisdogakane | 0:2f6112d75752 | 825 | |
| jojisdogakane | 0:2f6112d75752 | 826 | if( deg <= 11.25 && deg >= -11.25) { |
| jojisdogakane | 0:2f6112d75752 | 827 | degPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 828 | } |
| jojisdogakane | 0:2f6112d75752 | 829 | else if(deg >= 11.25 && deg <= 33.75) { |
| jojisdogakane | 0:2f6112d75752 | 830 | degPoint = 0.375; |
| jojisdogakane | 0:2f6112d75752 | 831 | } |
| jojisdogakane | 0:2f6112d75752 | 832 | else if(deg >= 33.75 && deg <= 56.25) { |
| jojisdogakane | 0:2f6112d75752 | 833 | degPoint = 0.25; |
| jojisdogakane | 0:2f6112d75752 | 834 | } |
| jojisdogakane | 0:2f6112d75752 | 835 | else if(deg >= 56.25 && deg <= 78.75) { |
| jojisdogakane | 0:2f6112d75752 | 836 | degPoint = 0.125; |
| jojisdogakane | 0:2f6112d75752 | 837 | } |
| jojisdogakane | 0:2f6112d75752 | 838 | else if(deg >= 78.75 && deg <= 101.25) { |
| jojisdogakane | 0:2f6112d75752 | 839 | degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 840 | } |
| jojisdogakane | 0:2f6112d75752 | 841 | else if(deg >= 101.25 && deg <= 123.75) { |
| jojisdogakane | 0:2f6112d75752 | 842 | degPoint = 0.125; |
| jojisdogakane | 0:2f6112d75752 | 843 | } |
| jojisdogakane | 0:2f6112d75752 | 844 | else if(deg >= 123.75 && deg <= 146.25) { |
| jojisdogakane | 0:2f6112d75752 | 845 | degPoint = 0.25; |
| jojisdogakane | 0:2f6112d75752 | 846 | } |
| jojisdogakane | 0:2f6112d75752 | 847 | else if(deg >= 146.25 && deg <= 168.75) { |
| jojisdogakane | 0:2f6112d75752 | 848 | degPoint = 0.375; |
| jojisdogakane | 0:2f6112d75752 | 849 | } |
| jojisdogakane | 0:2f6112d75752 | 850 | else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) { |
| jojisdogakane | 0:2f6112d75752 | 851 | degPoint = 0.5; |
| jojisdogakane | 0:2f6112d75752 | 852 | } |
| jojisdogakane | 0:2f6112d75752 | 853 | else if(deg <= -11.25 && deg >= -33.75) { |
| jojisdogakane | 0:2f6112d75752 | 854 | degPoint = 0.625; |
| jojisdogakane | 0:2f6112d75752 | 855 | } |
| jojisdogakane | 0:2f6112d75752 | 856 | else if(deg <= -33.75 && deg >= -56.25) { |
| jojisdogakane | 0:2f6112d75752 | 857 | degPoint = 0.75; |
| jojisdogakane | 0:2f6112d75752 | 858 | } |
| jojisdogakane | 0:2f6112d75752 | 859 | else if(deg <= -56.25 && deg >= -78.75) { |
| jojisdogakane | 0:2f6112d75752 | 860 | degPoint = 0.875; |
| jojisdogakane | 0:2f6112d75752 | 861 | } |
| jojisdogakane | 0:2f6112d75752 | 862 | else if(deg <= -78.75 && deg >= -101.25) { |
| jojisdogakane | 0:2f6112d75752 | 863 | degPoint = 1; |
| jojisdogakane | 0:2f6112d75752 | 864 | } |
| jojisdogakane | 0:2f6112d75752 | 865 | else if(deg <= -101.25 && deg >= -123.75) { |
| jojisdogakane | 0:2f6112d75752 | 866 | degPoint = 0.875; |
| jojisdogakane | 0:2f6112d75752 | 867 | } |
| jojisdogakane | 0:2f6112d75752 | 868 | else if(deg <= -123.75 && deg >= -146.25) { |
| jojisdogakane | 0:2f6112d75752 | 869 | degPoint = 0.75; |
| jojisdogakane | 0:2f6112d75752 | 870 | } |
| jojisdogakane | 0:2f6112d75752 | 871 | else if(deg <= -146.25 && deg >= -168.75) { |
| jojisdogakane | 0:2f6112d75752 | 872 | degPoint = 0.625; |
| jojisdogakane | 0:2f6112d75752 | 873 | } |
| jojisdogakane | 0:2f6112d75752 | 874 | else { |
| jojisdogakane | 0:2f6112d75752 | 875 | degPoint = 0; |
| jojisdogakane | 0:2f6112d75752 | 876 | } |
| jojisdogakane | 0:2f6112d75752 | 877 | |
| jojisdogakane | 4:a0150e0f285b | 878 | happyRatioNow = degPoint; |
| jojisdogakane | 4:a0150e0f285b | 879 | |
| jojisdogakane | 0:2f6112d75752 | 880 | degRatio = (deg + 180.0) / 360.0; |
| jojisdogakane | 0:2f6112d75752 | 881 | |
| jojisdogakane | 0:2f6112d75752 | 882 | float rcFilter = RCFILTER; |
| jojisdogakane | 0:2f6112d75752 | 883 | float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint; |
| jojisdogakane | 0:2f6112d75752 | 884 | prevFilteredAngle = filteredAngle ; |
| jojisdogakane | 0:2f6112d75752 | 885 | |
| jojisdogakane | 0:2f6112d75752 | 886 | happyRatio = filteredAngle - 0.5; |
| jojisdogakane | 0:2f6112d75752 | 887 | |
| jojisdogakane | 0:2f6112d75752 | 888 | if(happyRatio <= 0){ |
| jojisdogakane | 0:2f6112d75752 | 889 | happyRatio = abs(happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 890 | } |
| jojisdogakane | 0:2f6112d75752 | 891 | else { |
| jojisdogakane | 0:2f6112d75752 | 892 | happyRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 893 | } |
| jojisdogakane | 0:2f6112d75752 | 894 | |
| jojisdogakane | 0:2f6112d75752 | 895 | // happyRatio = averageStressRatio ;// @@@ |
| jojisdogakane | 0:2f6112d75752 | 896 | |
| jojisdogakane | 0:2f6112d75752 | 897 | nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 898 | |
| jojisdogakane | 0:2f6112d75752 | 899 | // nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio); |
| jojisdogakane | 0:2f6112d75752 | 900 | // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); |
| jojisdogakane | 0:2f6112d75752 | 901 | |
| jojisdogakane | 4:a0150e0f285b | 902 | // happyRatio = happyRatio * happyRatio * happyRatio; |
| jojisdogakane | 0:2f6112d75752 | 903 | |
| jojisdogakane | 4:a0150e0f285b | 904 | // happyRatio *= 10; |
| jojisdogakane | 0:2f6112d75752 | 905 | |
| jojisdogakane | 0:2f6112d75752 | 906 | x3 = x2; |
| jojisdogakane | 0:2f6112d75752 | 907 | y3 = y2; |
| jojisdogakane | 0:2f6112d75752 | 908 | |
| jojisdogakane | 0:2f6112d75752 | 909 | int HR_range = 0; |
| jojisdogakane | 0:2f6112d75752 | 910 | |
| jojisdogakane | 0:2f6112d75752 | 911 | int HR_tempval = currentBPM; |
| jojisdogakane | 0:2f6112d75752 | 912 | |
| jojisdogakane | 0:2f6112d75752 | 913 | |
| jojisdogakane | 0:2f6112d75752 | 914 | // 心拍の係数を算出する |
| jojisdogakane | 0:2f6112d75752 | 915 | |
| jojisdogakane | 0:2f6112d75752 | 916 | // 最大心拍数以上→最大心拍数に |
| jojisdogakane | 0:2f6112d75752 | 917 | if( HR_MAX < HR_tempval) |
| jojisdogakane | 0:2f6112d75752 | 918 | { |
| jojisdogakane | 0:2f6112d75752 | 919 | HR_tempval = HR_MAX; |
| jojisdogakane | 0:2f6112d75752 | 920 | } |
| jojisdogakane | 0:2f6112d75752 | 921 | |
| jojisdogakane | 0:2f6112d75752 | 922 | // 最少心拍数以下→最少心拍数に |
| jojisdogakane | 0:2f6112d75752 | 923 | if( HR_MIN > HR_tempval) |
| jojisdogakane | 0:2f6112d75752 | 924 | { |
| jojisdogakane | 0:2f6112d75752 | 925 | HR_tempval = HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 926 | } |
| jojisdogakane | 0:2f6112d75752 | 927 | |
| jojisdogakane | 0:2f6112d75752 | 928 | // 心拍の幅を計算 |
| jojisdogakane | 0:2f6112d75752 | 929 | HR_range = HR_MAX - HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 930 | |
| jojisdogakane | 0:2f6112d75752 | 931 | // 現在値－最少心拍数 |
| jojisdogakane | 0:2f6112d75752 | 932 | HR_tempval = HR_tempval - HR_MIN; |
| jojisdogakane | 0:2f6112d75752 | 933 | |
| jojisdogakane | 0:2f6112d75752 | 934 | // 心拍の係数を算出。0 ～ 1.0 |
| jojisdogakane | 0:2f6112d75752 | 935 | HR_ratio = (double)HR_tempval / (double)HR_range; |
| jojisdogakane | 0:2f6112d75752 | 936 | |
| jojisdogakane | 0:2f6112d75752 | 937 | if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/) |
| jojisdogakane | 0:2f6112d75752 | 938 | { |
| jojisdogakane | 0:2f6112d75752 | 939 | bConcentration = true; |
| jojisdogakane | 0:2f6112d75752 | 940 | } |
| jojisdogakane | 0:2f6112d75752 | 941 | else { |
| jojisdogakane | 0:2f6112d75752 | 942 | bConcentration = false; |
| jojisdogakane | 0:2f6112d75752 | 943 | } |
| jojisdogakane | 0:2f6112d75752 | 944 | prevRelaxRatio = relaxRatio; |
| jojisdogakane | 0:2f6112d75752 | 945 | |
| jojisdogakane | 0:2f6112d75752 | 946 | int hval = 1023 * happyRatio; |
| jojisdogakane | 0:2f6112d75752 | 947 | int rval = 1023 * relaxRatio; |
| jojisdogakane | 0:2f6112d75752 | 948 | int cval = 1023 * conRatio; |
| jojisdogakane | 0:2f6112d75752 | 949 | |
| jojisdogakane | 0:2f6112d75752 | 950 | // printf("H%d\n", hval); |
| jojisdogakane | 0:2f6112d75752 | 951 | // printf("R%d\n", rval); |
| jojisdogakane | 0:2f6112d75752 | 952 | // printf("C%d\n", cval); |
| jojisdogakane | 0:2f6112d75752 | 953 | |
| jojisdogakane | 0:2f6112d75752 | 954 | sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration); |
| jojisdogakane | 0:2f6112d75752 | 955 | ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), |
| jojisdogakane | 0:2f6112d75752 | 956 | (uint8_t*)msg, nMsgIBILen + nMsgHappyLen + nMsgRelaxLen + nMsgConcentrationLen); |
| jojisdogakane | 0:2f6112d75752 | 957 | // (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen); |
| jojisdogakane | 0:2f6112d75752 | 958 | |
| jojisdogakane | 0:2f6112d75752 | 959 | // determine color range |
| jojisdogakane | 0:2f6112d75752 | 960 | |
| jojisdogakane | 0:2f6112d75752 | 961 | conRatio = conRatio - 0.5; |
| jojisdogakane | 0:2f6112d75752 | 962 | |
| jojisdogakane | 0:2f6112d75752 | 963 | if(conRatio < 0) { |
| jojisdogakane | 0:2f6112d75752 | 964 | conRatio = 0; |
| jojisdogakane | 0:2f6112d75752 | 965 | } |
| jojisdogakane | 0:2f6112d75752 | 966 | |
| jojisdogakane | 0:2f6112d75752 | 967 | conRatio = conRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 968 | |
| jojisdogakane | 0:2f6112d75752 | 969 | // happyRatio = happyRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 970 | // happyRatio = happyRatio * 2; |
| jojisdogakane | 0:2f6112d75752 | 971 | if(happyRatio > 1) { |
| jojisdogakane | 0:2f6112d75752 | 972 | happyRatio = 1; |
| jojisdogakane | 0:2f6112d75752 | 973 | } |
| jojisdogakane | 4:a0150e0f285b | 974 | /* |
| jojisdogakane | 0:2f6112d75752 | 975 | if(BPM >= HR_MIN) { |
| jojisdogakane | 0:2f6112d75752 | 976 | happyRatio /= ((float)(BPM * 2) / 255.0) ; |
| jojisdogakane | 0:2f6112d75752 | 977 | } |
| jojisdogakane | 4:a0150e0f285b | 978 | */ |
| jojisdogakane | 0:2f6112d75752 | 979 | } |
| jojisdogakane | 0:2f6112d75752 | 980 | |
| jojisdogakane | 4:a0150e0f285b | 981 | if(BPM > 0 && bPulseEvent) { |
| jojisdogakane | 4:a0150e0f285b | 982 | int NewMood = getMood(BPM, relaxRatio, happyRatio, conRatio) ; |
| jojisdogakane | 4:a0150e0f285b | 983 | |
| jojisdogakane | 4:a0150e0f285b | 984 | for(i = 0 ; i < NEOPIXEL_COUNT; i++) { |
| jojisdogakane | 4:a0150e0f285b | 985 | TargetColorArrayR[i] = MoodColorR[i][NewMood]; |
| jojisdogakane | 4:a0150e0f285b | 986 | TargetColorArrayG[i] = MoodColorG[i][NewMood]; |
| jojisdogakane | 4:a0150e0f285b | 987 | TargetColorArrayB[i] = MoodColorB[i][NewMood]; |
| jojisdogakane | 0:2f6112d75752 | 988 | } |
| jojisdogakane | 4:a0150e0f285b | 989 | } |
| jojisdogakane | 0:2f6112d75752 | 990 | |
| jojisdogakane | 6:35f536162183 | 991 | for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) { |
| jojisdogakane | 6:35f536162183 | 992 | if(ColorArrayR[i] < TargetColorArrayR[i]) { |
| jojisdogakane | 6:35f536162183 | 993 | ColorArrayR[i]++; |
| jojisdogakane | 6:35f536162183 | 994 | } |
| jojisdogakane | 6:35f536162183 | 995 | else if(ColorArrayR[i] > TargetColorArrayR[i]) { |
| jojisdogakane | 6:35f536162183 | 996 | ColorArrayR[i]--; |
| jojisdogakane | 4:a0150e0f285b | 997 | } |
| jojisdogakane | 6:35f536162183 | 998 | if(ColorArrayG[i] < TargetColorArrayG[i]) { |
| jojisdogakane | 6:35f536162183 | 999 | ColorArrayG[i]++; |
| jojisdogakane | 4:a0150e0f285b | 1000 | } |
| jojisdogakane | 6:35f536162183 | 1001 | else if(ColorArrayG[i] > TargetColorArrayG[i]) { |
| jojisdogakane | 6:35f536162183 | 1002 | ColorArrayG[i]--; |
| jojisdogakane | 6:35f536162183 | 1003 | } |
| jojisdogakane | 6:35f536162183 | 1004 | if(ColorArrayB[i] < TargetColorArrayB[i]) { |
| jojisdogakane | 6:35f536162183 | 1005 | ColorArrayB[i]++; |
| jojisdogakane | 4:a0150e0f285b | 1006 | } |
| jojisdogakane | 6:35f536162183 | 1007 | else if(ColorArrayB[i] > TargetColorArrayB[i]) { |
| jojisdogakane | 6:35f536162183 | 1008 | ColorArrayB[i]--; |
| jojisdogakane | 4:a0150e0f285b | 1009 | } |
| jojisdogakane | 6:35f536162183 | 1010 | |
| jojisdogakane | 6:35f536162183 | 1011 | // 輝度調整 |
| jojisdogakane | 6:35f536162183 | 1012 | r = ColorArrayR[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 1013 | g = ColorArrayG[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 1014 | b = ColorArrayB[i] * DIM_LED; |
| jojisdogakane | 6:35f536162183 | 1015 | |
| jojisdogakane | 6:35f536162183 | 1016 | brightnessSum = r + g + b; |
| jojisdogakane | 6:35f536162183 | 1017 | |
| jojisdogakane | 6:35f536162183 | 1018 | // 輝度の合計値が上限を超えた場合は補正する |
| jojisdogakane | 6:35f536162183 | 1019 | if(brightnessSum > BRIGHTNESS_MAX) { |
| jojisdogakane | 6:35f536162183 | 1020 | r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 6:35f536162183 | 1021 | g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 6:35f536162183 | 1022 | b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); |
| jojisdogakane | 0:2f6112d75752 | 1023 | } |
| jojisdogakane | 0:2f6112d75752 | 1024 | |
| jojisdogakane | 4:a0150e0f285b | 1025 | neopixel_set_color( |
| jojisdogakane | 6:35f536162183 | 1026 | pixels[i], |
| jojisdogakane | 4:a0150e0f285b | 1027 | 0, |
| jojisdogakane | 6:35f536162183 | 1028 | short(r), |
| jojisdogakane | 6:35f536162183 | 1029 | short(g), |
| jojisdogakane | 6:35f536162183 | 1030 | short(b)); |
| jojisdogakane | 6:35f536162183 | 1031 | neopixel_show(pixels[i]); |
| jojisdogakane | 0:2f6112d75752 | 1032 | } |
| jojisdogakane | 4:a0150e0f285b | 1033 | //wait_ms(1); |
| jojisdogakane | 6:35f536162183 | 1034 | wait_ms(10); // 500usまで下げてもセンサー共振は起こらないことを確認 |
| jojisdogakane | 0:2f6112d75752 | 1035 | } |
| jojisdogakane | 0:2f6112d75752 | 1036 | } |