SWITCH_DECLARE(void) switch_time_calibrate_clock(void) { int x; switch_interval_time_t avg, val = 1000, want = 1000; int over = 0, under = 0, good = 0, step = 50, diff = 0, retry = 0, lastgood = 0, one_k = 0; #ifdef HAVE_CLOCK_GETRES struct timespec ts; long res = 0; clock_getres(CLOCK_MONOTONIC, &ts); res = ts.tv_nsec / 1000; if (res > 900 && res < 1100) { one_k = 1; } if (res > 1500) { STEP_MS = res / 1000; STEP_MIC = res; switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_WARNING, "Timer resolution of %ld microseconds detected!\n" "Do you have your kernel timer frequency set to lower than 1,000Hz? " "You may experience audio problems. Step MS %d\n", ts.tv_nsec / 1000, STEP_MS); do_sleep(5000000); switch_time_set_cond_yield(SWITCH_TRUE); return; } #endif top: val = 1000; step = 50; over = under = good = 0; OFFSET = 0; for (x = 0; x < 100; x++) { avg = average_time(val, 50); switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_CONSOLE, "Test: %ld Average: %ld Step: %d\n", (long) val, (long) avg, step); diff = abs((int) (want - avg)); if (diff > 1500) { switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_WARNING, "Abnormally large timer gap %d detected!\n" "Do you have your kernel timer frequency set to lower than 1,000Hz? You may experience audio problems.\n", diff); do_sleep(5000000); switch_time_set_cond_yield(SWITCH_TRUE); return; } if (diff <= 100) { lastgood = (int) val; } if (diff <= 2) { under = over = 0; lastgood = (int) val; if (++good > 10) { break; } } else if (avg > want) { if (under) { calc_step(); } under = good = 0; if ((val - step) < 0) { if (++retry > 2) break; goto top; } val -= step; over++; } else if (avg < want) { if (over) { calc_step(); } over = good = 0; if ((val - step) < 0) { if (++retry > 2) break; goto top; } val += step; under++; } } if (good >= 10) { OFFSET = (int) (want - val); switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_CONSOLE, "Timer offset of %d calculated\n", OFFSET); } else if (lastgood) { OFFSET = (int) (want - lastgood); switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_CONSOLE, "Timer offset of %d calculated (fallback)\n", OFFSET); switch_time_set_cond_yield(SWITCH_TRUE); } else if (one_k) { OFFSET = 900; switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_CONSOLE, "Timer offset CANNOT BE DETECTED, forcing OFFSET to 900\n"); switch_time_set_cond_yield(SWITCH_TRUE); } else { switch_log_printf(SWITCH_CHANNEL_LOG, SWITCH_LOG_CONSOLE, "Timer offset NOT calculated\n"); switch_time_set_cond_yield(SWITCH_TRUE); } }
bool calc_pose(void) { uchar ucType = watcher_data->step.activity.base.type; static uint16 nConfirmCount_STAND = 0,nConfirmCount_LIE = 0; //判断站、躺计数值 static uint16 nStaticCount = 0, //判断静计数值 nActivityCount = 0;//判断动计数阀值 static uint16 angleBuf = 0, //合加速度 nStaticSampleCont = 0; static uint8 nFallDelayCount = 0; uint32 nArea; //加速度平方和 short x,y,z; //xyz的加速度值 uint8 i; static bool suddenly_flag = false; for(i = 0; i < FIFO_COUNT; ++i) { x = (short)(watcher_data->fifo_buf[i].x_MSB<<8|watcher_data->fifo_buf[i].x_LSB)>>2; y = (short)(watcher_data->fifo_buf[i].y_MSB<<8|watcher_data->fifo_buf[i].y_LSB)>>2; z = (short)(watcher_data->fifo_buf[i].z_MSB<<8|watcher_data->fifo_buf[i].z_LSB)>>2; nArea = x * x + y * y + z * z; angleBuf = sqrt((double)nArea); #ifdef TRACE // sprintf(watcher_data->trace_data, "MMA8451: x = %d, y = %d, z = %d, SUM = %d",x, y, z,angleBuf); // post_trace(watcher_data->trace_data); #endif if(watcher_data->flag & FLAG_SAMPLES_SHOW){ watcher_data->flag &= ~FLAG_SAMPLES_SHOW; sprintf(watcher_data->trace_data, "x = %d\ny = %d\nz = %d\nsum = %d",x, y, z,angleBuf); draw_rectangle_color(0, 80, 63, 48, 0); show_string(0,112,(char*)watcher_data->trace_data); } ++pose_count; if(x > BODY_STATE_ANGLE_THRESHOLD ) {//计算X轴的夹角 ++nConfirmCount_STAND; if(nConfirmCount_STAND > BODY_STAND_COUNT) { if(suddenly_flag == true){ suddenly_flag = false; //watcher_data->flag |= flag_suddenly; } if(abs(nConfirmCount_STAND - nConfirmCount_LIE) <= BODY_STATE_COUNT){ nConfirmCount_LIE = 0;nConfirmCount_STAND = 0; }else{ nConfirmCount_STAND = 0;nConfirmCount_LIE = 0; ucType |= TYPE_STAND; /// post_trace("stand"); } } } else if(x < BODY_LIE_ANGLE_THRESHOLD){ ++nConfirmCount_LIE; if(nConfirmCount_LIE > BODY_LIE_COUNT) { if(suddenly_flag == true){ suddenly_flag = false; } if(nConfirmCount_LIE - nConfirmCount_STAND <= BODY_STATE_COUNT){ nConfirmCount_LIE = 0;nConfirmCount_STAND = 0; }else{ nConfirmCount_LIE = 0;nConfirmCount_STAND = 0; ucType &= ~TYPE_STAND; // post_trace("lie"); } } } if(angleBuf < (2048 + BODY_CHANGE_THRESHOLD) && angleBuf > 2048 - BODY_CHANGE_THRESHOLD){ if(++nStaticCount > BODY_STATIC_THRESHOLD) { ucType &= ~(TYPE_ACTIVITY | TYPE_WALK); nStaticCount = 0;nActivityCount = 0; ++nStaticSampleCont; if(nStaticSampleCont > CHECK_STATIC_SPAN){ if(nlh_count_temp <= 1){ nlh_count_temp = LH_8COUNT; } nStaticSampleCont = 0; } } } else { if(nlh_count_temp >= LH_8COUNT){//检测到运动,启动正常采样率 if(angleBuf >= BODY_SUNDDENLY){//猛起 suddenly_flag = true; } nlh_count_temp = 1; }else if(++nActivityCount >= BODY_ACTIVE_THRESHOLD){ nStaticCount = 0; nStaticSampleCont = 0;nActivityCount = 0; ucType |= TYPE_ACTIVITY; } } if(ucType & TYPE_ACTIVITY) { calc_activity(&ucType, angleBuf); calc_step(&ucType, angleBuf); } if(ucType != watcher_data->step.activity.base.type) { save_pose(); watcher_data->step.activity.base.type = ucType; } if(angleBuf <= FREEFALL_THRESHOLD /*&& ((watcher_data->step.activity.base.type & TYPE_STAND) == TYPE_STAND)*/){ //出现自由落体状态 ++nFallDelayCount; //#ifdef TRACE // sprintf(watcher_data->trace_data, " freefall = %d", angleBuf); // post_trace(watcher_data->trace_data); //#endif } // if((watcher_data->step.activity.base.type & TYPE_STAND) == TYPE_STAND){ // nArea = y * y + z * z; // if(sqrt((double)nArea) > BODY_STAGGER){ // watcher_data->flag |= flag_stagger; //#ifdef TRACE //// sprintf(watcher_data->trace_data, " flag_stagger = %d", angleBuf); //// post_trace(watcher_data->trace_data); //#endif // } // } if((nFallDelayCount >=FREEFALLTIME_THRESHOLD && angleBuf >= FALL_THRESHOLD) && ((watcher_data->step.activity.base.type & TYPE_STAND) == TYPE_STAND)){ //如果发生大冲击表示可能摔倒 //watcher_data->flag |= flag_sensor_transient; nFallDelayCount = 0; #ifdef TRACE // sprintf(watcher_data->trace_data, " fall = %d", angleBuf); // post_trace(watcher_data->trace_data); #endif }else if(nFallDelayCount >= FREEFALLTIME_THRESHOLD + 10){ nFallDelayCount = 0; } /* if((watcher_data->flag & flag_sensor_transient) == flag_sensor_transient){ if(is_fall()){ watcher_data->flag |= flag_auto_alarm; //dispatch(); } }*/ } return true; }