reduced message bytes for BLE
Dependencies: BLE_API mbed nRF51822
Fork of Inp_Fiber_Logo_FOTA by
main.cpp
- Committer:
- shimoyamada
- Date:
- 2017-08-31
- Revision:
- 14:698909b62531
- Parent:
- 13:eb29ef0d97e5
File content as of revision 14:698909b62531:
// 20170703 BRIGHTNESS_MAXを100に変更 // シリアル通信のプロトコルを、文字列→バイト列に変更 // 20170619 DeviceInformationのSerialNumberにIND+Macアドレスを適用し、DEVICENAMEはINUPATHYとした // 20170617 ストレス値を"R"(relax)から"S"(stress)に変更し、(1.0 - relaxRatio)を送るよう改変 // 20170617 輝度調整の方法をrgbの合計値を基準にするように調整した。 #include "mbed.h" #include "neopixel.h" #include "nrf_soc.h" #include "common.h" #include "ble/BLE.h" #include "ble/services/HeartRateService.h" #include "ble/services/BatteryService.h" #include "ble/services/DeviceInformationService.h" #include "UARTService.h" #include "ble_gap.h" #include "ble/services/DFUService.h" // ★OTA用 #define RAINBOW_RATIO 1 #define RCFILTER 0.9 #define RCFILTER2 0.9 #define NEOPIXEL_COUNT 6 #define MIN_LIGHT 0 #define PNNX_FACTOR 0.1 // 0.06 #define MAXARRAY 15 //15 // 心拍平均値計算用の配列サイズ //20 #define MAXHRVARRAY 10 //30 // 脈間ゆらぎ平均値計算用の配列サイズ #define MIN_INTERVAL 200 #define INTERVAL 10 // Loopの間隔 通常時推奨1 通信時推奨20 #define MAXCOLORARRAY 1 //(MIN_INTERVAL / INTERVAL) // 色平均値計算用の配列サイズ (MIN_INTERVAL / 7) #define MAXCONARRAY 10 #define MAXSTRESSARRAY 3 #define BRIGHTNESS_MAX 100 // RGBLED1セットのpwm合計値の上限 ws2812b型のneopixelが6個の場合、150を超えた値にするとセンサーが誤作動する #define DIM_LED ((float)(BRIGHTNESS_MAX) / (float)(255)) // R, G, Bそれぞれのpwm値がBRIGHTNESS_MAXを超えない様にする制限 // 心拍数の最低値、最高値 // 人間の場合は60~150、標準80 // 犬の場合は40~200、標準60 #define HR_MIN 40 #define HR_MAX 150 #define RELAX_RATIO_MIN 0.6 // ラジアンを度に変換する. #define RadianToDegree(radian) ((180 / 3.14159) * (radian)) // 度をラジアンに変換する. #define DegreeToRadian(degree) ((3.14159 / 180) * (degree)) // 色の識別値設定(ピンではない) #define RED 0 #define BLUE 1 #define GREEN 2 #define SHOW_EXCITE 1 #define SHOW_HAPPY 2 #define SHOW_CONCENTRATION 4 BLE ble; DFUService *dfuService; // ★OTA用 AnalogIn HRSigIn(P0_4); const static char DEVICE_NAME[] = "INUPATHY"; static const uint16_t uuid16_list[] = {GattService::UUID_HEART_RATE_SERVICE, GattService::UUID_DEVICE_INFORMATION_SERVICE}; static volatile bool triggerSensorPolling = true; short ColorArrayR[NEOPIXEL_COUNT]; short ColorArrayG[NEOPIXEL_COUNT]; short ColorArrayB[NEOPIXEL_COUNT]; short TargetColorArrayR[NEOPIXEL_COUNT]; short TargetColorArrayG[NEOPIXEL_COUNT]; short TargetColorArrayB[NEOPIXEL_COUNT]; #define NEOPIXEL_COMMANDLEN ((NEOPIXEL_COUNT * 3) + 1 + 1) unsigned long pixelStartCounter; int mainCurrentPixel = 0; int mainPrevPixel = 0; int mainPixelLightCount; int mainPixelStep = 0; int mainStepsRemain = 0; unsigned long currentmillis = 0; unsigned long prevmillis = 0; unsigned long intervalmillis = 0; unsigned long prevIntervalmillis = 0; unsigned long intervalmillisArray[MAXARRAY] = {1000}; unsigned long intervalmillisHRVArray[MAXHRVARRAY] = {1000}; float conArray[MAXCONARRAY] = {0.5}; //float stressArray[MAXSTRESSARRAY] = {0.5}; float prevFilteredAngle = 0.5; float happyRatio = 0.0; float happyRatioNow = 0.0; float prevRelaxRatio = 1.0; float prevFilteredDistRatio = 0.0; #define HR_THRESHOLD 2 int HRThreshold[] = {120, 1000}; // 気分判定フラグ #define IS_RELAX 0 #define IS_EXCITED 1 #define IS_HAPPY 2 #define IS_INTERESTED 3 #define IS_STRESSED 4 // IS_STRESSEDはMOODの個数としても扱うので最後の番号にすること #define MOOD_COUNT IS_STRESSED + 1 // 気分判定係数 // 気分判定係数 #define CONMARGIN 0.9 // 0.85 #define RELAXMARGIN 0.35 #define HAPPYMARGIN 0.25 // 0.225 #define EXCITEMARGIN 1.5 // 1.15 #define HRM_AVERAGE_COUNT 100 // MAX 125 short HRM_Average[HRM_AVERAGE_COUNT] = {70}; // 気分判定フラグを索引にする int MoodColorR[NEOPIXEL_COUNT][MOOD_COUNT]; int MoodColorG[NEOPIXEL_COUNT][MOOD_COUNT]; int MoodColorB[NEOPIXEL_COUNT][MOOD_COUNT]; unsigned char RainbowR[] = { 50, 50, 50, 127, 200, 255}; unsigned char RainbowG[] = { 50, 127, 255, 127, 100, 50}; unsigned char RainbowB[] = {255, 127, 50, 50, 50, 50}; unsigned char HappyR[] = { 50, 50, 50, 127, 200, 255}; unsigned char HappyG[] = { 50, 127, 255, 127, 100, 50}; unsigned char HappyB[] = {255, 127, 50, 50, 50, 50}; unsigned char RelaxR[] = { 0, 0, 0, 0, 0, 0}; unsigned char RelaxG[] = {255, 255, 255, 255, 255, 255}; unsigned char RelaxB[] = { 0, 0, 0, 0, 0, 0}; unsigned char ExciteR[] = {255, 255, 255, 255, 255, 255}; unsigned char ExciteG[] = {100, 100, 100, 100, 100, 100}; unsigned char ExciteB[] = { 0, 0, 0, 0, 0, 0}; unsigned char StressR[] = {100, 100, 100, 100, 100, 100}; unsigned char StressG[] = { 0, 0, 0, 0, 0, 0}; unsigned char StressB[] = {255, 255, 255, 255, 255, 255}; unsigned char InterestR[] = {255, 255, 255, 255, 255, 255}; unsigned char InterestG[] = {255, 255, 255, 255, 255, 255}; unsigned char InterestB[] = {150, 150, 150, 150, 150, 150}; float rainbowOffset = 0.0; bool bConcentration; unsigned long loopCounter = 0; int lightPos = 1; int prevLightPos = 1; double relaxRatio = 0.0; float conRatio = 0.5; float conRatioNow = 0.5; double HR_ratio = 0.0; unsigned long IBI; int x3, y3; int mainPixelCounter; int prevInterval; int prevprevInterval; void disconnectionCallback(Gap::Handle_t handle, Gap::DisconnectionReason_t reason) { ble.gap().startAdvertising(); // restart advertising } /* void ConnectTimeoutCallback(Gap::EventCallback_t timeoutCallback) { ble.gap().startAdvertising(); // restart advertising } */ uint8_t BPM = 0; float sensorValue = 0; Timer t; Timer tRand; float redVal = 0; float blueVal = 255; float greenVal = 0; Ticker neopixelTick; UARTService *uartServicePtr; void neopixelTick_Handler() { return; } void SetMoodColor() { int i = 0; for(i = 0; i < NEOPIXEL_COUNT; i++) { MoodColorR[i][IS_RELAX] = RelaxR[i]; MoodColorG[i][IS_RELAX] = RelaxG[i]; MoodColorB[i][IS_RELAX] = RelaxB[i]; } for(i = 0; i < NEOPIXEL_COUNT; i++) { MoodColorR[i][IS_EXCITED] = ExciteR[i]; MoodColorG[i][IS_EXCITED] = ExciteG[i]; MoodColorB[i][IS_EXCITED] = ExciteB[i]; } for(i = 0; i < NEOPIXEL_COUNT; i++) { MoodColorR[i][IS_HAPPY] = HappyR[i]; MoodColorG[i][IS_HAPPY] = HappyG[i]; MoodColorB[i][IS_HAPPY] = HappyB[i]; } for(i = 0; i < NEOPIXEL_COUNT; i++) { MoodColorR[i][IS_INTERESTED] = InterestR[i]; MoodColorG[i][IS_INTERESTED] = InterestG[i]; MoodColorB[i][IS_INTERESTED] = InterestB[i]; } for(i = 0; i < NEOPIXEL_COUNT; i++) { MoodColorR[i][IS_STRESSED] = StressR[i]; MoodColorG[i][IS_STRESSED] = StressG[i]; MoodColorB[i][IS_STRESSED] = StressB[i]; } } float BPM_Average = 0.0; int currentBPM = 0; // 現在の気分の判定 int getMood(int bpm, double relaxRatio, double happyRatio, double conRatio) { // 第一優先:集中 // 集中係数が0.95以上なら集中と判定 if(conRatio >= CONMARGIN) { return IS_INTERESTED; } // 第二優先:Stress // リラックス係数が0.2以下 if(relaxRatio <= RELAXMARGIN) { return IS_STRESSED; } // 第三優先:Happy // Happy係数が0.2以上 if(happyRatio >= HAPPYMARGIN && relaxRatio > 0.5) { return IS_HAPPY; } // 平均の心拍より1.2倍以上だと興奮 // それ以外は安静 if((float)currentBPM / BPM_Average >= EXCITEMARGIN) { return IS_EXCITED; } // もしくは一定以上の心拍数だと興奮 else if(BPM >= HRThreshold[0]) { return IS_EXCITED; } else { return IS_RELAX; } } //Serial mySerial(USBTX, USBRX); //Serial mySerial(P0_9, P0_11); // MACアドレスを取得してBLEのSerialNumberに組み込む void setBleSerialNumber(char *serialNumber) { int i; ble_gap_addr_t mac_address; if(sd_ble_gap_address_get(&mac_address) != NRF_SUCCESS){ return; } // ADDR_LENは6 for(i=BLE_GAP_ADDR_LEN-1; i>=0; i--){ sprintf(serialNumber, "%s%02x", serialNumber, (mac_address.addr)[i]); } sprintf(serialNumber, "%s%s", serialNumber, "\0"); } int main(void) { /* char msgIBI[100]; int nMsgIBILen = 0; char msgHappy[10]; int nMsgHappyLen = 0; char msgConcentration[10]; int nMsgConcentrationLen = 0; char msgRelax[10]; int nMsgRelaxLen = 0; */ byte_t msgHeader[3] = {0x49,0x4e,0x50}; // byte_t msg[19]; byte_t msg[10]; // int nMsgLen = 0; int i; int r = 0; int g = 0; int b = 0; int brightnessSum = 0; neopixel_strip_t neopixel1; neopixel_init(&neopixel1, P0_7, 1); neopixel_clear(&neopixel1); neopixel_strip_t neopixel2; neopixel_init(&neopixel2, P0_8, 1); neopixel_clear(&neopixel2); neopixel_strip_t neopixel3; neopixel_init(&neopixel3, P0_10, 1); neopixel_clear(&neopixel3); neopixel_strip_t neopixel4; neopixel_init(&neopixel4, P0_9, 1); neopixel_clear(&neopixel4); neopixel_strip_t neopixel5; neopixel_init(&neopixel5, P0_15, 1); neopixel_clear(&neopixel5); neopixel_strip_t neopixel6; neopixel_init(&neopixel6, P0_11, 1); neopixel_clear(&neopixel6); neopixel_strip_t *pixels[6]; pixels[0] = &neopixel1; pixels[1] = &neopixel2; pixels[2] = &neopixel3; pixels[3] = &neopixel4; pixels[4] = &neopixel5; pixels[5] = &neopixel6; SetMoodColor(); ble.init(); ble.gap().onDisconnection(disconnectionCallback); UARTService uartService(ble); uartServicePtr = &uartService; // ★OTA用 dfuService = new DFUService(ble, NULL); /* Setup primary service. */ HeartRateService hrServiceBPM(ble, BPM, HeartRateService::LOCATION_CHEST); // HeartRateService hrServiceIBI(ble, IBI, HeartRateService::LOCATION_FINGER); // BatteryService batteryService(ble, 100); /* Setup auxiliary service. */ //DeviceInformationService deviceInfo(ble, "ARM", "Model1", "SN1", "hw-rev1", "fw-rev1", "soft-rev1"); char serialNumber[16] = "IND"; // SerialNumberStringにMACアドレスを組み込む setBleSerialNumber(serialNumber); // ★ DeviceInformationService deviceInfo(ble, "INUPATHY01", "0.0.1", serialNumber, "0.0.1", "0.0.1", "0.0.1"); /* Setup advertising. */ ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::BREDR_NOT_SUPPORTED | GapAdvertisingData::LE_GENERAL_DISCOVERABLE); ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LIST_16BIT_SERVICE_IDS, (uint8_t *)uuid16_list, sizeof(uuid16_list)); ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::GENERIC_HEART_RATE_SENSOR); ble.gap().accumulateAdvertisingPayload(GapAdvertisingData::COMPLETE_LOCAL_NAME, (uint8_t *)DEVICE_NAME, sizeof(DEVICE_NAME)); ble.gap().setAdvertisingType(GapAdvertisingParams::ADV_CONNECTABLE_UNDIRECTED); ble.gap().setAdvertisingInterval(800); /* 1000ms */ ble.gap().startAdvertising(); for( i = 0; i < NEOPIXEL_COUNT; i++) { // 起動時の色パターンを設定 ColorArrayR[i] = RainbowR[i]; ColorArrayG[i] = RainbowG[i]; ColorArrayB[i] = RainbowB[i]; TargetColorArrayR[i] = ColorArrayR[i]; TargetColorArrayG[i] = ColorArrayG[i]; TargetColorArrayB[i] = ColorArrayB[i]; // 輝度調整 r = ColorArrayR[i] * DIM_LED; g = ColorArrayG[i] * DIM_LED; b = ColorArrayB[i] * DIM_LED; brightnessSum = r + g + b; // 輝度の合計値が上限を超えた場合は補正する if(brightnessSum > BRIGHTNESS_MAX) { r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); } neopixel_set_color( pixels[i], 0, short(r), short(g), short(b)); neopixel_show(pixels[i]); } t.start(); prevmillis = t.read_ms(); bool bPulseEvent = false; float degPoint = 0; // neopixelTick.attach(neopixelTick_Handler, 0.005); while (1) { // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); float currentValue = HRSigIn; bPulseEvent = false; if ((currentValue <= 0.3) && (sensorValue > 0.3)) { bPulseEvent = true; } // printf("S%d", sens); // printf("S%d\n", sens); sensorValue = currentValue; if(bPulseEvent) { // batteryService.updateBatteryLevel( (uint8_t)read_battery_level()); // 現在の時刻を取得 // t.stop(); // stop, startは時間計測の結果を有意に短くしてしまうので使わない currentmillis = t.read_ms(); // t.start(); // 現在時刻から過去時刻を引き、間隔を求める intervalmillis = currentmillis - prevmillis; // 心拍の間の間隔 IBI = intervalmillis; // 現在の時刻を過去時刻として記録 prevmillis = currentmillis; // 平均値算出用配列に値を詰める double averageIntervalMillis; for(i = 0; i < MAXARRAY ; i++) { // 最後尾以外の場合 if(i != MAXARRAY - 1) { // 次の要素の値をコピーする intervalmillisArray[i] = intervalmillisArray[i + 1]; } // 最後尾の場合 else { // 最新値を詰める intervalmillisArray[i] = intervalmillis; } } // 先頭が0の間は何もしない unsigned long sumIntervalMillis = 0; unsigned long sumIntervalMillisV = 0; if (intervalmillisArray[0] != 0) { // 平均値算出 averageIntervalMillis = 0; for(i = 0; i < MAXARRAY ; i++) { sumIntervalMillis += intervalmillisArray[i]; sumIntervalMillisV += intervalmillisArray[i] * intervalmillisArray[i]; } averageIntervalMillis = (double)sumIntervalMillis / (double)MAXARRAY; BPM = min32_of(60000 / averageIntervalMillis, 255); // if(intervalmillis < averageIntervalMillis - stdDev) { // || intervalmillis > averageIntervalMillis + stdDev) { currentBPM = min32_of(60000 / intervalmillis, 255); /* } else { currentBPM = BPM; } */ } // BPMの平均値計算 int BPM_sum = 0; for(i = 0; i < HRM_AVERAGE_COUNT ; i++) { BPM_sum += HRM_Average[i]; } BPM_Average = (float)BPM_sum/(float)HRM_AVERAGE_COUNT; if(currentBPM > 0) { for(i = 0; i < HRM_AVERAGE_COUNT ; i++) { // 最後尾以外の場合 if(i != HRM_AVERAGE_COUNT - 1) { // 次の要素の値をコピーする HRM_Average[i] = HRM_Average[i + 1]; } // 最後尾の場合 else { // 最新値を詰める HRM_Average[i] = currentBPM; } } } /* int IBI1024 = IBI * (1024/1000); bpm[1] = BPM; bpm[6] = (IBI1024 & 0xFF00) >> 8; bpm[7] = (IBI1024 & 0xFF); */ // printf("B%d\n", BPM); // printf("Q%d\n", IBI); // nMsgLen = sprintf(msg,"B%d\0", BPM); // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); // nMsgIBILen = sprintf(msgIBI,"Q%d\0", IBI); // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); // 平均値算出用配列に値を詰める for(i = 0; i < MAXHRVARRAY ; i++) { // 最後尾以外の場合 if(i != MAXHRVARRAY - 1) { // 次の要素の値をコピーする intervalmillisHRVArray[i] = intervalmillisHRVArray[i + 1]; } // 最後尾の場合 else { // 最新値を詰める intervalmillisHRVArray[i] = intervalmillis; } } // 平均値算出 int nNNInterval; unsigned long ulIntervalSum = 0; int nSampleCount = 0; for(i = 0; i < MAXHRVARRAY ; i++) { if(intervalmillisHRVArray[i] > 0) { ulIntervalSum += intervalmillisHRVArray[i]; nSampleCount++; } } float IntervalAverage = 0.0; if(nSampleCount > 0){ IntervalAverage = (float)ulIntervalSum / (float)nSampleCount; } nNNInterval = (int)((float)IntervalAverage * PNNX_FACTOR); bool bContinue = false; int nNNxCount = 0; for(i = 0; i < MAXHRVARRAY ; i++) { // if not end if(i < MAXHRVARRAY - 1) { if(abs((int)(intervalmillisHRVArray[i] - intervalmillisHRVArray[i + 1])) >= nNNInterval) { if(bContinue == false) { nNNxCount++; } nNNxCount++; bContinue = true; } else { bContinue = false; } } } float diffRatio = (float)nNNxCount / (float)nSampleCount; if(diffRatio > 1) { diffRatio = 1; } // リラックスの係数を算出する relaxRatio = diffRatio; // nMsgRelaxLen = sprintf(msgRelax,"S%0.2f\0", (1-relaxRatio)); // nMsgLen = sprintf(msg,"R%0.2f\0", relaxRatio); // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); // 現在値から最低値を引く // relaxRatio = relaxRatio - RELAX_RATIO_MIN; // 1.0 / 最低値 × 現在値 // relaxRatio = relaxRatio * (1.0 / (1.0 - RELAX_RATIO_MIN)); int x1, y1, x2, y2; int tempx1, tempy1, tempx3, tempy3; int longx, shortx, longy, shorty; long xx1, yy1; int movex, movey; double dist1, dist2; /* // 最後尾の3データから二つのx, y地点を作る。 x1 = intervalmillisArray[nSampleCount - 1]; y1 = intervalmillisArray[nSampleCount - 2]; x2 = intervalmillisArray[nSampleCount - 2]; y2 = intervalmillisArray[nSampleCount - 3]; */ x1 = intervalmillis; y1 = prevInterval; x2 = prevInterval; y2 = prevprevInterval; // 2地点を結ぶ直線の長さを算出する。 if(x1 > x2) { longx = x1; shortx = x2; } else { longx = x2; shortx = x1; } if(y1 > y2) { longy = y1; shorty = y2; } else { longy = y2; shorty = y1; } xx1 = longx - shortx; yy1 = longy - shorty; dist1 = sqrt((double)((xx1 * xx1) + (yy1 * yy1))); dist2 = longx + longy; float distRatio = dist1 / dist2; conRatioNow = 1 - distRatio; // 平均値算出用配列に値を詰める float averageConRatio = 0.5; for(i = 0; i < MAXCONARRAY ; i++) { // 最後尾以外の場合 if(i != MAXCONARRAY - 1) { // 次の要素の値をコピーする conArray[i] = conArray[i + 1]; } // 最後尾の場合 else { // 最新値を詰める conArray[i] = distRatio; } } // 先頭が0の間は何もしない if (conArray[0] != 0) { // 平均値算出 averageConRatio = 0; for(i = 0; i < MAXCONARRAY ; i++) { averageConRatio += conArray[i]; } averageConRatio = averageConRatio / (double)MAXCONARRAY; } /* float averageStressRatio = 0.5; for(i = 0; i < MAXSTRESSARRAY ; i++) { // 最後尾以外の場合 if(i != MAXSTRESSARRAY - 1) { // 次の要素の値をコピーする stressArray[i] = stressArray[i + 1]; } // 最後尾の場合 else { // 最新値を詰める if(x1 > y1) { stressArray[i] = y1 / x1; } else { stressArray[i] = x1 / y1; } } } // 先頭が0の間は何もしない if (stressArray[0] != 0) { // 平均値算出 averageStressRatio = 0; for(i = 0; i < MAXSTRESSARRAY ; i++) { averageStressRatio += stressArray[i]; } averageStressRatio = averageStressRatio / (double)MAXSTRESSARRAY; } */ /* float rcFilter = RCFILTER2; float filteredDistRatio = 0.0; float a; if(prevFilteredDistRatio != 0) { a = rcFilter * prevFilteredDistRatio; filteredDistRatio = a + (1 - rcFilter) * distRatio; } else { filteredDistRatio = (1 - rcFilter) * distRatio; } prevFilteredDistRatio = filteredDistRatio ; conRatio = filteredDistRatio ; */ conRatio = averageConRatio; conRatio = 1 - conRatio; // nMsgConcentrationLen = sprintf(msgConcentration,"C%0.2f\0", conRatio); // nMsgLen = sprintf(msg,"C%0.2f\0", conRatio); // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); prevprevInterval = prevInterval; prevInterval = intervalmillis; // (x2, y2)が原点となるように座標を変換する movex = x2 * -1; movey = y2 * -1; tempx1 = x1 - movex; tempy1 = y1 - movey; tempx3 = x3 - movex; tempy3 = y3 - movey; // (x2, y2)を中心とした円を考える。 // (x3, y3)を(x2, y2)を中心に180°回転する。→(rotx3, roty3) double theta = DegreeToRadian(180); double rotx3 = (tempx3) * cos(theta) - (tempy3) * sin(theta); double roty3 = (tempx3) * sin(theta) + (tempy3) * cos(theta); // ベクトル1とベクトル3の内積を求める double vecInt = tempx1 * rotx3 + tempy1 * roty3; // ベクトル1とベクトル3の外積を求める double vecExt = tempx1 * roty3 - tempy1 * rotx3; // ベクトル1とベクトル3の角度を求める theta = atan2(vecExt, vecInt); double deg = RadianToDegree(theta); if( deg <= 11.25 && deg >= -11.25) { degPoint = 0.5; } else if(deg >= 11.25 && deg <= 33.75) { degPoint = 0.375; } else if(deg >= 33.75 && deg <= 56.25) { degPoint = 0.25; } else if(deg >= 56.25 && deg <= 78.75) { degPoint = 0.125; } else if(deg >= 78.75 && deg <= 101.25) { degPoint = 0; } else if(deg >= 101.25 && deg <= 123.75) { degPoint = 0.125; } else if(deg >= 123.75 && deg <= 146.25) { degPoint = 0.25; } else if(deg >= 146.25 && deg <= 168.75) { degPoint = 0.375; } else if(deg >= 168.75 && deg <= 180 || deg <= 0 && deg >= -11.25) { degPoint = 0.5; } else if(deg <= -11.25 && deg >= -33.75) { degPoint = 0.625; } else if(deg <= -33.75 && deg >= -56.25) { degPoint = 0.75; } else if(deg <= -56.25 && deg >= -78.75) { degPoint = 0.875; } else if(deg <= -78.75 && deg >= -101.25) { degPoint = 1; } else if(deg <= -101.25 && deg >= -123.75) { degPoint = 0.875; } else if(deg <= -123.75 && deg >= -146.25) { degPoint = 0.75; } else if(deg <= -146.25 && deg >= -168.75) { degPoint = 0.625; } else { degPoint = 0; } happyRatioNow = degPoint; float rcFilter = RCFILTER; float filteredAngle = rcFilter * prevFilteredAngle + (1 - rcFilter) * degPoint; prevFilteredAngle = filteredAngle ; happyRatio = filteredAngle - 0.5; if(happyRatio <= 0){ happyRatio = abs(happyRatio); } else { happyRatio = 0; } // happyRatio = averageStressRatio ;// @@@ // nMsgHappyLen = sprintf(msgHappy,"H%0.2f\0", happyRatio); // nMsgLen = sprintf(msg,"H%0.2f\0", happyRatio); // ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, nMsgLen); // happyRatio = happyRatio * happyRatio * happyRatio; // happyRatio *= 10; x3 = x2; y3 = y2; int HR_range = 0; int HR_tempval = currentBPM; // 心拍の係数を算出する // 最大心拍数以上→最大心拍数に if( HR_MAX < HR_tempval) { HR_tempval = HR_MAX; } // 最少心拍数以下→最少心拍数に if( HR_MIN > HR_tempval) { HR_tempval = HR_MIN; } // 心拍の幅を計算 HR_range = HR_MAX - HR_MIN; // 現在値-最少心拍数 HR_tempval = HR_tempval - HR_MIN; // 心拍の係数を算出。0 ~ 1.0 HR_ratio = (double)HR_tempval / (double)HR_range; if(conRatioNow >= 0.95 /* && averageAngle >= 0.7*/) { bConcentration = true; } else { bConcentration = false; } prevRelaxRatio = relaxRatio; // printf("H%d\n", hval); // printf("R%d\n", rval); // printf("C%d\n", cval); // sprintf(msg,"%s%s%s%s\0", msgIBI, msgHappy, msgRelax, msgConcentration); unsigned char happyRatioChar = (unsigned char)(happyRatio * 100); unsigned char stressRatioChar = (unsigned char)((1 - relaxRatio) * 100); unsigned char conRatioChar = (unsigned char)(conRatio * 100); memcpy(msg, msgHeader, sizeof(msgHeader)); memcpy(&(msg[3]), &IBI , sizeof(IBI) ); memcpy(&(msg[7]), &happyRatioChar , sizeof(happyRatioChar)); memcpy(&(msg[8]), &stressRatioChar , sizeof(stressRatioChar)); memcpy(&(msg[9]), &conRatioChar , sizeof(conRatioChar) ); ble.updateCharacteristicValue(uartServicePtr->getRXCharacteristicHandle(), (uint8_t*)msg, sizeof(msg)); // (uint8_t*)msg, nMsgIBILen + nMsgHappyLen + nMsgRelaxLen + nMsgConcentrationLen); // (uint8_t*)msg, nMsgIBILen + 1 + nMsgHappyLen + 1 + nMsgRelaxLen + 1 + nMsgConcentrationLen); // update bpm if(ble.gap().getState().connected == 1) { hrServiceBPM.updateHeartRate(BPM); // hrServiceIBI.updateHeartRate(IBI); } // determine color range conRatio = conRatio - 0.5; if(conRatio < 0) { conRatio = 0; } conRatio = conRatio * 2; // happyRatio = happyRatio * 2; // happyRatio = happyRatio * 2; if(happyRatio > 1) { happyRatio = 1; } /* if(BPM >= HR_MIN) { happyRatio /= ((float)(BPM * 2) / 255.0) ; } */ } if(BPM > 0 && bPulseEvent) { int NewMood = getMood(BPM, relaxRatio, happyRatio, conRatio) ; for(i = 0 ; i < NEOPIXEL_COUNT; i++) { TargetColorArrayR[i] = MoodColorR[i][NewMood]; TargetColorArrayG[i] = MoodColorG[i][NewMood]; TargetColorArrayB[i] = MoodColorB[i][NewMood]; } } int deltaVal = 2; for(i = NEOPIXEL_COUNT - 1; i >= 0; i--) { if(ColorArrayR[i] < TargetColorArrayR[i]) { ColorArrayR[i]+=deltaVal; if(ColorArrayR[i] > TargetColorArrayR[i]) { ColorArrayR[i] = TargetColorArrayR[i]; } } else if(ColorArrayR[i] > TargetColorArrayR[i]) { ColorArrayR[i]-=deltaVal; if(ColorArrayR[i] < TargetColorArrayR[i]) { ColorArrayR[i] = TargetColorArrayR[i]; } } if(ColorArrayG[i] < TargetColorArrayG[i]) { ColorArrayG[i]+=deltaVal; if(ColorArrayG[i] > TargetColorArrayG[i]) { ColorArrayG[i] = TargetColorArrayG[i]; } } else if(ColorArrayG[i] > TargetColorArrayG[i]) { ColorArrayG[i]-=deltaVal; if(ColorArrayG[i] < TargetColorArrayG[i]) { ColorArrayG[i] = TargetColorArrayG[i]; } } if(ColorArrayB[i] < TargetColorArrayB[i]) { ColorArrayB[i]+=deltaVal; if(ColorArrayB[i] > TargetColorArrayB[i]) { ColorArrayB[i] = TargetColorArrayB[i]; } } else if(ColorArrayB[i] > TargetColorArrayB[i]) { ColorArrayB[i]-=deltaVal; if(ColorArrayB[i] < TargetColorArrayB[i]) { ColorArrayB[i] = TargetColorArrayB[i]; } } // 輝度調整 r = ColorArrayR[i] * DIM_LED; g = ColorArrayG[i] * DIM_LED; b = ColorArrayB[i] * DIM_LED; brightnessSum = r + g + b; // 輝度の合計値が上限を超えた場合は補正する if(brightnessSum > BRIGHTNESS_MAX) { r = (int)(r * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); g = (int)(g * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); b = (int)(b * ((float)BRIGHTNESS_MAX / (float)brightnessSum)); } neopixel_set_color( pixels[i], 0, short(r), short(g), short(b)); neopixel_show(pixels[i]); } //wait_ms(1); wait_ms(10); // 500usまで下げてもセンサー共振は起こらないことを確認 } }