rvtr/ctr_mcu
1
Fork 0
mirror of https://github.com/rvtr/ctr_mcu.git synced 2025-10-31 13:51:10 -04:00
Code Issues Actions Packages Projects Releases Wiki Activity
e520e4d3e2
ctr_mcu/branches/0.13(sdk0.9_plus_E3)/pedo_alg_thre_det2.c

412 lines
10 KiB
C
Raw Blame History

/* ********************************************************
********************************************************* */
#pragma MUL
#pragma BCD
#include "incs.h"
#include <math.h>
#include "accero.h"
#include "pedometer.h"
static void hosu_increment();
u16 get_long_hour();
extern u16 pool[];
/*=========================================================
　歩数計
========================================================*/
void pedometer()
{
static s16 th_H = 15000; // 閾値。暫定。動的変更とかしたい…ので変数
static s16 th_L = 11000;
static u16 acc_norm[3]; // 加速度の大きさのヒストリ。数字が大きい方が古い
static u16 acc_norm_temp;
static u8 interval_hh; // 山-山間の時間。短過ぎたらはじく。
static u8 time_l; // 前回の極小からの経過時間
static u16 peak_l; // 谷の深さ
static u16 peak_h; // 山の高さ
static u16 norm_hist[TAP];
static u8 hist_indx;
static u8 t_rise;
signed long filterd;
u8 i;
signed long temp1,temp2,temp3;
signed long temp4,temp5,temp6;
u16 sx16 = abs( (u16)vreg_ctr[VREG_C_ACC_XH] * 256 + vreg_ctr[VREG_C_ACC_XL] );
u16 sy16 = abs( (u16)vreg_ctr[VREG_C_ACC_YH] * 256 + vreg_ctr[VREG_C_ACC_YL] );
u16 sz16 = abs( (u16)vreg_ctr[VREG_C_ACC_ZH] * 256 + vreg_ctr[VREG_C_ACC_ZL] );
// ベクトルのノルム
#ifdef _mcu_
norm_hist[ hist_indx & TAP-1 ] = sqrt( (long)sx16 * ( sx16 / 2 ) +
(long)sy16 * ( sy16 / 2 ) +
(long)sz16 * ( sz16 / 2 )
);
#endif
#ifdef _pc_
norm_hist[ hist_indx & TAP-1 ] = normh * 256 + norml;
#endif
hist_indx += 1;
// ヒストリにフィルタを掛けて、今回の値を求める
filterd = 0;
// for( i = 8; i != 55; i++ ) // 係数が０ばかりのため
for( i = 0; i != 46; i++ ) // 係数テーブルをいじりました。パラメータ調整時注意
{
filterd += (signed long)norm_hist[ ( hist_indx + i ) & TAP-1 ] * lpf_coeff[ i ];
}
filterd += (4096)*512;
acc_norm_temp = (s16)( filterd /1024 & 0xFFFF ); // ←FIL_COEFF_QUANTから正規化
/*
if( acc_norm[0] < acc_norm_temp )
{
t_rise += 1;
if( t_rise == 0 )
t_rise == 254;
}
else
{
t_rise = 0;
}
*/
if( acc_norm[0] != acc_norm_temp )
{
acc_norm[2] = acc_norm[1]; // ヒストリ
acc_norm[1] = acc_norm[0];
acc_norm[0] = acc_norm_temp;
}
if( acc_norm[2] <= acc_norm[1] && acc_norm[1] > acc_norm[0]
&& acc_norm[0] > th_H )
// 極大で、閾値を超えていた
{
if( 21 < interval_hh )
// 前回の極大からの間隔がほどよい
{
if(( interval_hh < 160 ) && ( time_l < interval_hh ))
// 谷を挟んでいる
{
if( acc_norm[0] - peak_l > 4200 ){
// 一歩増えました
hosu_increment();
}
}
interval_hh = 0;
}
if( acc_norm[0] > 18000 )
{
th_L = acc_norm[0] - 10000;
}
else
{
th_L = 11000;
}
}
else
{
interval_hh += ( interval_hh != 255 ) ? 1: 0; // 飽和加算って楽に書けたらいいのに
}
// (2) 直近の極小からの時間
if( acc_norm[2] >= acc_norm[1] && acc_norm[1] < acc_norm[0]
&& acc_norm[0] < th_L )
{
// 極小を検出
time_l = 0;
peak_l = acc_norm[0];
}
else
{
time_l += ( time_l != 255 ) ? 1: 0;
}
{
/*
static u8 i = 0;
vreg_ctr[VREG_C_FREE_0] = i++;
vreg_ctr[VREG_C_FREE_1] = (u8)( acc_norm[0] / 256 & 0x00FF );
vreg_ctr[VREG_C_FREE_2] = (u8)( acc_norm[0] & 0x00FF );
vreg_ctr[VREG_C_FREE_3] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] / 256 & 0xFF );
vreg_ctr[VREG_C_FREE_4] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] & 0xFF );
vreg_ctr[VREG_C_FREE_5] = interval_hh;
vreg_ctr[VREG_C_FREE_6] = time_l;
vreg_ctr[VREG_C_FREE_7] = vreg_ctr[ VREG_C_ACC_HOSU_L ];
vreg_ctr[VREG_C_FREE_8] = (u8)( peak_l / 256 & 0x00FF );
vreg_ctr[VREG_C_FREE_9] = (u8)( peak_l & 0x00FF );
// vreg_ctr[VREG_C_FREE_A] = (u8)( norm_avg[0] / 256 & 0x00FF );
// vreg_ctr[VREG_C_FREE_B] = (u8)( norm_avg[0] & 0x00FF );
*/
}
}
/*=========================================================
　歩数＋１
　　累積をインクリメント
　履歴を更新
========================================================*/
u8 p_record;
u8 last_hour = 0x23; // 履歴の最新は何時？
u8 last_day = 0x30;
u8 last_month = 0x12;
u8 last_year = 0x99;
#define HOSU_NODATA 0xFFFF
#define HOSU_MAX 0xFFFE
static void hosu_increment()
{
static u16 last_hour_fny;
// 累積の更新 //
// いろいろ失敗した...
if( ++vreg_ctr[ VREG_C_ACC_HOSU_L ] == 0 )
{
if( ++vreg_ctr[ VREG_C_ACC_HOSU_M ] == 0 )
{
if( ++vreg_ctr[ VREG_C_ACC_HOSU_H ] == 0 ){
vreg_ctr[ VREG_C_ACC_HOSU_L ] = 255; // カンスト orz
vreg_ctr[ VREG_C_ACC_HOSU_M ] = 255;
vreg_ctr[ VREG_C_ACC_HOSU_H ] = 255;
}
}
}
{
// 毎時履歴の更新 /////////////////////////////
// 空白の時間を考慮する。１時間以上放置されたなど。
u16 now_hour;
u8 now_year;
// 時計を止める必要が有るので↓は一気に行って下さい
DI();
RWAIT = 1;
while( !RWST ){;}
now_year = bcdtob( YEAR );
last_hour = HOUR; // 履歴読み出し時に使用。BCDのままでよい
last_day = DAY;
last_month = MONTH;
now_hour = get_long_hour();
// RWAIT = 0; ↑で行っています
// EI(); 〃
// 歩数計が止まっていた時間を考慮して必要なら進める //
if( last_year == now_year )
{
if( now_hour > last_hour_fny )
{
fill_hosu_hist_hours( now_hour - last_hour_fny );
}
}
else if( last_year == ( now_year -1 ) )
{
// 年をまたいでいるとき
u16 temp_hours = ( ( 365 + (( now_year & 0x03 ) == 1 ? 1: 0 )) * 24 ) - last_hour_fny + now_hour;
fill_hosu_hist_hours( temp_hours );
}
else if( last_year < now_year )
{
// 数年放置
fill_hosu_hist_hours( 0 );
}
else
{
// カレンダーが巻き戻るなど
// ノーケアでよい
}
last_year = now_year;
last_hour_fny = now_hour;
// 実際にインクリメント
if( pool[ p_record ] == HOSU_NODATA )
{
pool[ p_record ] = 1;
}
else if( pool[ p_record ] != HOSU_MAX )
{
pool[ p_record ] += 1;
}
}
}
/* ========================================================
空白の時間を適切に0にして、
今を含む１時間のデータを書く位置にポインタ？を進める
======================================================== */
void fill_hosu_hist_hours( u16 hours )
{
if( hours > 255 )
{
hours = 0;
}
// 空白の数時間の設定
do
{
p_record += 1;
pool[ p_record ] = 0;
hours -= 1;
}
while( hours != 0 );
}
/* ========================================================
空白の時間を適切に0にして、
今を含む１時間のデータを書く位置にポインタ？を進める
======================================================== */
void clear_hosu_hist()
{
u8 hours = 0;
// 空白の数時間の設定
do
{
pool[ hours ] = 0xFFFF;
hours -= 1;
}
while( hours != 0 );
vreg_ctr[ VREG_C_ACC_HOSU_L ] = 0;
vreg_ctr[ VREG_C_ACC_HOSU_M ] = 0;
vreg_ctr[ VREG_C_ACC_HOSU_H ] = 0;
}
extern u8 iic_burst_state;
bit record_read_msb_lsb;
/* ========================================================
歩数計ヒストリ読み出しの後処理(初期化)
読み出しポインタのクリア
======================================================== */
void hosu_read_end( )
{
record_read_msb_lsb = 0;
}
/* ========================================================
歩数計のヒストリを返す。
１回呼ぶ度に、ヒストリの下位、上位、一時間遡って下位上位...
======================================================== */
u8 hosu_read( )
{
u8 dat;
u16 temp;
static u8 p_record_buffer;
switch( iic_burst_state ){
case( 0 ):
p_record_buffer = p_record;
iic_burst_state += 1;
return( last_hour );
case( 1 ):
iic_burst_state += 1;
return( last_day );
case( 2 ):
iic_burst_state += 1;
return( last_month );
case( 3 ):
iic_burst_state += 1;
return( btobcd(last_year) );
case( 4 ):
iic_burst_state += 1;
return( 0x55 ); // reserved dummy
case( 5 ):
iic_burst_state += 1;
return( 0xAA ); // reserved. dummy
default:
temp = pool[ p_record_buffer ];
if( record_read_msb_lsb == 0 )
{
dat = (u8)( temp & 0x00FF );
}
else
{
dat = (u8)(( temp >> 8 ) & 0x00FF );
p_record_buffer -= 1;
}
record_read_msb_lsb += 1;
return( dat );
}
}
/* ========================================================
今年の元旦からの経過時間(hour)を返す。
引数 無し
返値 u16 long_hour
======================================================== */
static const u16 DAYS_FROM_HNY[] = {
0,
0, 31, 31+28, 59+31, 90+30,
120+31, 151+30, 181+31, 212+31, 243+30,
273+31, 304+30 };
u16 get_long_hour()
{
u16 long_hour;
u8 year_hex;
u8 month_hex;
u8 day_hex;
u8 hour_hex;
year_hex = bcdtob( YEAR );
month_hex = bcdtob( MONTH );
day_hex = bcdtob( DAY );
hour_hex = bcdtob( HOUR );
RWAIT = 0;
EI();
// まず日数の部分
long_hour = DAYS_FROM_HNY[ month_hex ];
if(( ( year_hex & 0x03 ) == 0 ) && ( ( 3 <= month_hex )
|| (( 2 == month_hex ) && ( day_hex == 29 )) ))
{
// 閏年で、閏日より後
long_hour += 1;
}
long_hour += day_hex - 1;
long_hour *= 24; // 日数→時間
long_hour += hour_hex;
return( long_hour );
}
Reference in New Issue View Git Blame Copy Permalink