mirror of
https://github.com/rvtr/ctr_mcu.git
synced 2025-10-31 13:51:10 -04:00
cb4c756204
歩数計が年をまたぐとき（で、境界が0分0秒以外の時）、ログの時刻が期待通りでなかった git-svn-id: file:///Volumes/Transfer/gigaleak_20231201/2020-05-23%20-%20ctr.7z%20+%20svn_v1.068.zip/ctr/svn/ctr_mcu@217 013db118-44a6-b54f-8bf7-843cb86687b1
497 lines
13 KiB
C
497 lines
13 KiB
C
/* ********************************************************
|
||
歩数計
|
||
3軸加速度のリアルタイムデータから、ベクトルのノルムを出し、
|
||
閾値を超える時間、間隔、ノルムの大きさで閾値を切り替えるなど
|
||
********************************************************* */
|
||
#pragma mul
|
||
#pragma div
|
||
#pragma bcd
|
||
|
||
#include "incs.h"
|
||
#include <math.h>
|
||
|
||
#include "accero.h"
|
||
#include "pedometer.h"
|
||
|
||
#include "pedo_lpf_coeff.h"
|
||
#include "pool.h"
|
||
|
||
// ========================================================
|
||
static void hosu_increment();
|
||
|
||
|
||
|
||
// ========================================================
|
||
u16 get_long_hour();
|
||
extern uni_pool pool;
|
||
|
||
|
||
|
||
// ========================================================
|
||
#define _use_my_sqrt_
|
||
#ifdef _use_my_sqrt_
|
||
unsigned long my_sqrt();
|
||
#endif
|
||
|
||
|
||
|
||
/*=========================================================
|
||
歩数計
|
||
========================================================*/
|
||
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 norm_hist[TAP];
|
||
|
||
static u8 hist_indx;
|
||
|
||
signed long filterd;
|
||
|
||
u8 i;
|
||
|
||
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_
|
||
# ifndef _use_my_sqrt_
|
||
norm_hist[ hist_indx & TAP-1 ] = sqrt( (long)sx16 * ( sx16 / 2 ) +
|
||
(long)sy16 * ( sy16 / 2 ) +
|
||
(long)sz16 * ( sz16 / 2 )
|
||
);
|
||
# else
|
||
norm_hist[ hist_indx & TAP-1 ] = my_sqrt( (long)sx16 * ( sx16 / 2 ) +
|
||
(long)sy16 * ( sy16 / 2 ) +
|
||
(long)sz16 * ( sz16 / 2 )
|
||
);
|
||
# endif
|
||
#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;
|
||
}
|
||
|
||
#ifdef _DBG_PEDO_AUTO_ENABLE_
|
||
{
|
||
static u8 i = 0;
|
||
|
||
vreg_ctr[ 0x50 ] = i++;
|
||
|
||
vreg_ctr[ 0x51 ] = (u8)( acc_norm[0] / 256 & 0x00FF );
|
||
vreg_ctr[ 0x52 ] = (u8)( acc_norm[0] & 0x00FF );
|
||
|
||
vreg_ctr[ 0x53 ] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] / 256 & 0xFF );
|
||
vreg_ctr[ 0x54 ] = (u8)( norm_hist[ hist_indx -1 & TAP-1 ] & 0xFF );
|
||
|
||
vreg_ctr[ 0x55 ] = interval_hh;
|
||
vreg_ctr[ 0x56 ] = time_l;
|
||
|
||
vreg_ctr[ 0x57 ] = vreg_ctr[ VREG_C_ACC_HOSU_L ];
|
||
vreg_ctr[ 0x58 ] = (u8)( peak_l / 256 & 0x00FF );
|
||
vreg_ctr[ 0x59 ] = (u8)( peak_l & 0x00FF );
|
||
// vreg_ctr[ 0x5A ] = (u8)( norm_avg[0] / 256 & 0x00FF );
|
||
// vreg_ctr[ 0x5B ] = (u8)( norm_avg[0] & 0x00FF );
|
||
}
|
||
#endif
|
||
}
|
||
|
||
|
||
|
||
/*=========================================================
|
||
歩数＋１
|
||
累積をインクリメント
|
||
履歴を更新
|
||
========================================================*/
|
||
u8 p_record;
|
||
u8 last_hour = 0x23; // 履歴の最新は何時？
|
||
u8 last_day = 0x30;
|
||
u8 last_month = 0x12;
|
||
u8 last_year = 0x99;
|
||
u8 now_min;
|
||
u8 now_sec;
|
||
|
||
#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_longhour;
|
||
u8 now_year;
|
||
|
||
// 時計を止める必要が有るので↓は一気に行って下さい
|
||
DI();
|
||
RWAIT = 1;
|
||
while( !RWST ){;}
|
||
now_year = bcdtob( YEAR );
|
||
|
||
// 履歴読み出し時に使用。BCDのままでよい
|
||
last_hour = HOUR;
|
||
last_day = DAY;
|
||
last_month = MONTH;
|
||
now_min = MIN;
|
||
now_sec = SEC;
|
||
now_longhour = get_long_hour();
|
||
|
||
// RWAIT = 0; ↑で行っています
|
||
// EI(); 〃
|
||
|
||
// 元旦零時台で昨日扱いになった場合の帳尻合わせ
|
||
if( now_longhour == 65535 )
|
||
now_longhour = ( ( 365 + (( now_year & 0x03 ) == 1 ? 1: 0 )) * 24 ) -1;
|
||
}
|
||
|
||
// 歩数計が止まっていた時間を考慮して必要なら進める //
|
||
if( last_year == now_year )
|
||
{
|
||
if( now_longhour > last_hour_fny )
|
||
{
|
||
fill_hosu_hist_hours( now_longhour - last_hour_fny );
|
||
}
|
||
}
|
||
else if( last_year == ( now_year -1 ) )
|
||
{
|
||
// 年をまたいでいるとき
|
||
fill_hosu_hist_hours( ( ( 365 + (( now_year & 0x03 ) == 1 ? 1: 0 )) * 24 ) - last_hour_fny + now_longhour );
|
||
}
|
||
else if( last_year < now_year )
|
||
{
|
||
// 数年放置
|
||
fill_hosu_hist_hours( 0 );
|
||
}
|
||
else
|
||
{
|
||
// カレンダーが巻き戻るなど
|
||
// ノーケアでよい
|
||
}
|
||
last_year = now_year;
|
||
last_hour_fny = now_longhour;
|
||
|
||
// 実際にインクリメント
|
||
{
|
||
u16* p_pedo_data = &pool.vreg_c_ext.pedo_log[ p_record ];
|
||
|
||
if( *p_pedo_data == HOSU_NODATA )
|
||
{
|
||
*p_pedo_data = 1;
|
||
}
|
||
else if( *p_pedo_data != HOSU_MAX )
|
||
{
|
||
*p_pedo_data += 1;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
空白の時間を適切に0にして、
|
||
今を含む１時間のデータを書く位置にポインタ？を進める
|
||
======================================================== */
|
||
void fill_hosu_hist_hours( u16 hours )
|
||
{
|
||
if( hours > PEDOMETER_LOG_SIZE )
|
||
{
|
||
hours = PEDOMETER_LOG_SIZE;
|
||
}
|
||
|
||
// 空白の数時間の設定
|
||
do
|
||
{
|
||
p_record += 1;
|
||
if( p_record >= PEDOMETER_LOG_SIZE )
|
||
{
|
||
p_record = 0;
|
||
}
|
||
pool.vreg_c_ext.pedo_log[ p_record ] = 0;
|
||
hours -= 1;
|
||
}
|
||
while( hours != 0 );
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
空白の時間を適切に0にして、
|
||
今を含む１時間のデータを書く位置にポインタ？を進める
|
||
======================================================== */
|
||
void clear_hosu_hist()
|
||
{
|
||
u8 hours = PEDOMETER_LOG_SIZE;
|
||
do
|
||
{
|
||
hours -= 1;
|
||
pool.vreg_c_ext.pedo_log[ hours ] = 0xFFFF;
|
||
}
|
||
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;
|
||
p_record = 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( now_min ); // reserved dummy
|
||
|
||
case( 5 ):
|
||
iic_burst_state += 1;
|
||
return( now_sec ); // reserved. dummy
|
||
|
||
default:
|
||
temp = pool.vreg_c_ext.pedo_log[ p_record_buffer ];
|
||
if( record_read_msb_lsb == 0 )
|
||
{
|
||
dat = (u8)( temp & 0x00FF );
|
||
}
|
||
else
|
||
{
|
||
dat = (u8)(( temp >> 8 ) & 0x00FF );
|
||
if( p_record_buffer == 0 )
|
||
{
|
||
p_record_buffer = PEDOMETER_LOG_SIZE-1;
|
||
}
|
||
else
|
||
{
|
||
p_record_buffer -= 1;
|
||
}
|
||
}
|
||
record_read_msb_lsb += 1;
|
||
return( dat );
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
今年の元旦からの経過時間(hour)を返す。
|
||
引数 無し
|
||
返値 u16 long_hour
|
||
======================================================== */
|
||
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;
|
||
|
||
u8 min;
|
||
u8 sec;
|
||
|
||
// RWAIT = 1 を確認してから↓に進んで下さい
|
||
year_hex = YEAR;
|
||
month_hex = MONTH;
|
||
day_hex = DAY;
|
||
hour_hex = HOUR;
|
||
min = MIN;
|
||
sec = SEC;
|
||
|
||
RWAIT = 0;
|
||
EI();
|
||
|
||
year_hex = bcdtob( year_hex );
|
||
month_hex = bcdtob( month_hex );
|
||
day_hex = bcdtob( day_hex );
|
||
hour_hex = bcdtob( hour_hex );
|
||
|
||
// まず日数の部分
|
||
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;
|
||
|
||
if( ( min > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
|
||
|| ( ( min >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
|
||
&& ( sec > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY_SEC ] ))
|
||
)
|
||
{
|
||
return( long_hour );
|
||
}
|
||
else
|
||
{
|
||
return( long_hour -1 ); // 1時間前に含める 注意：元旦の0時
|
||
}
|
||
}
|
||
|
||
|
||
// 拝借もと
|
||
// ttp://www001.upp.so-net.ne.jp/y_yutaka/labo/math_algo/math_algo.html
|
||
unsigned long my_sqrt(unsigned long x)
|
||
{
|
||
unsigned long s, t;
|
||
|
||
if (x <= 0) return 0;
|
||
|
||
s = 1;
|
||
t = x;
|
||
while (s < t)
|
||
{
|
||
s <<= 1;
|
||
t >>= 1;
|
||
}
|
||
do
|
||
{
|
||
t = s;
|
||
s = (x / s + s) >> 1;
|
||
} while (s < t);
|
||
|
||
return t;
|
||
}
|