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;
}
