mirror of
https://github.com/rvtr/ctr_mcu.git
synced 2025-06-18 16:45:33 -04:00
c7141fe743
・加速度センサセカンドベンダ対応に落とし穴、加速度センサが壊れている/外れているでNAKを返し続けるようなとき、WDTリセットしていた。(accero.c) ・加速度センサエラーリトライ失敗時、復帰条件が、「何度も加速度センサonにしてカウンタがオーバーフローする」を修正(accero.c) ・DI_wt_chkを#ifdefでdisableにしたとき、ただのDI()になるように実装を変更。 ・#ifdef _irq_debug_ の実装がよくなかった（無駄にROMを消費） のを修正。まるまる取り除かれるようにした(ini_VECT.c) ・雑多関数をutil_funcs.cに切り出し。ROM配置時に便利というのもある ・WDIリセットしたときの追跡用にWDIリセットベクタを使えるように（#ifdef切り替え) git-svn-id: file:///Volumes/Transfer/gigaleak_20231201/2020-05-23%20-%20ctr.7z%20+%20svn_v1.068.zip/ctr/svn/ctr_mcu@407 013db118-44a6-b54f-8bf7-843cb86687b1
568 lines
14 KiB
C
568 lines
14 KiB
C
/* ********************************************************
|
||
歩数計
|
||
|
||
3軸加速度のリアルタイムデータから、ベクトルのノルムを出し、
|
||
閾値を超える時間、間隔、ノルムの大きさで閾値を切り替えるなど
|
||
|
||
$Id$
|
||
********************************************************* */
|
||
#ifndef _WIN32
|
||
|
||
#pragma mul
|
||
#pragma div
|
||
#pragma bcd
|
||
|
||
#endif
|
||
|
||
#include "incs.h"
|
||
|
||
#ifndef _WIN32
|
||
#include <math.h>
|
||
#endif
|
||
|
||
#include "accero.h"
|
||
#include "pedometer.h"
|
||
|
||
#include "pool.h"
|
||
|
||
// ========================================================
|
||
// 履歴の最終記録時刻
|
||
// この順番はログ読み出しの順番でもあるのでいじらないでね
|
||
// 順番にアドレスの若いのから確保されるのを期待してます...
|
||
typedef struct{
|
||
u8 hour_bcd;
|
||
u8 day_bcd;
|
||
u8 month_bcd;
|
||
u8 year_bcd;
|
||
u8 min_bcd;
|
||
u8 sec_bcd;
|
||
}st_calender;
|
||
|
||
|
||
|
||
// ========================================================
|
||
static u16 get_long_hour();
|
||
static u16 calc_hours_spend( u8 );
|
||
|
||
|
||
|
||
// ========================================================
|
||
bit pedolog_overflow; // 192時間記録済みフラグ(i2cで読める)
|
||
extern uni_pool pool; // 歩数ログはこの構造体の中
|
||
static u8 p_record; // ログの書き込み位置
|
||
static st_calender cal_log_latest; // 最後に歩数を更新した時刻
|
||
static u16 last_hour_fny; // fny:from new year
|
||
static st_calender cal_temp;
|
||
static u16 now_longhour;
|
||
|
||
|
||
|
||
|
||
|
||
|
||
// ========================================================
|
||
#define _use_my_sqrt_
|
||
#ifdef _use_my_sqrt_
|
||
static unsigned long my_sqrt( unsigned long );
|
||
#endif
|
||
|
||
// 今年は閏年？
|
||
#define is_leapyear( y ) (( y & 0x03 ) == 0 )
|
||
// 「去年」は閏年？
|
||
#define is_firstyear( y ) (( y & 0x03 ) == 1 )
|
||
|
||
|
||
// 加速度センサ値をFIR-LPFに通す。それの係数
|
||
extern const s8 lpf_coeff[];
|
||
#define TAP 64
|
||
#define FIL_COEFF_QUANT 10
|
||
|
||
|
||
/********************************************//**
|
||
歩数計
|
||
***********************************************/
|
||
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 = 0xFF; // 山-山間の時間。短過ぎたらはじく。
|
||
static u8 time_l; // 前回の極小からの経過時間
|
||
static u16 peak_l; // 谷の深さ
|
||
static u16 norm_hist[TAP];
|
||
|
||
static u8 hist_indx;
|
||
|
||
signed long filterd;
|
||
|
||
u8 i;
|
||
|
||
u16 sx16,sy16,sz16;
|
||
|
||
DI_wt_chk();
|
||
sx16 = abs( (u16)vreg_ctr[VREG_C_ACC_XH] * 256 + vreg_ctr[VREG_C_ACC_XL] );
|
||
sy16 = abs( (u16)vreg_ctr[VREG_C_ACC_YH] * 256 + vreg_ctr[VREG_C_ACC_YL] );
|
||
sz16 = abs( (u16)vreg_ctr[VREG_C_ACC_ZH] * 256 + vreg_ctr[VREG_C_ACC_ZL] );
|
||
EI();
|
||
|
||
// ベクトルのノルム
|
||
#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 ++;
|
||
|
||
// ヒストリにフィルタ(fir)を掛けて、今回の値を求める //
|
||
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; // DC分加算...だったと思う
|
||
acc_norm_temp = (s16)( filterd /1024 & 0xFFFF ); // ←FIL_COEFF_QUANTから正規化
|
||
/*
|
||
if( acc_norm[0] < acc_norm_temp )
|
||
{
|
||
t_rise ++;
|
||
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_if_necessary();
|
||
}
|
||
}
|
||
interval_hh = 0;
|
||
}
|
||
// なんちゃって閾値の動的変更
|
||
if( acc_norm[0] > 18000 )
|
||
{
|
||
th_L = acc_norm[0] - 10000;
|
||
}
|
||
else
|
||
{
|
||
th_L = 11000;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
if( interval_hh != 255 ) // 飽和加算って楽に書けたらいいのに
|
||
{
|
||
interval_hh ++;
|
||
}
|
||
}
|
||
|
||
// (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
|
||
{
|
||
if( time_l != 255 )
|
||
{
|
||
time_l ++;
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
/********************************************//**
|
||
歩数＋１
|
||
|
||
- 累積をインクリメント
|
||
- 履歴を更新
|
||
|
||
*2011/01/20
|
||
|
||
仕様変更 ログがいっぱいになったらそこで止める
|
||
***********************************************/
|
||
#define HOSU_NODATA 0xFFFF
|
||
#define HOSU_MAX 0xFFFE
|
||
|
||
void hosu_increment_if_necessary()
|
||
{
|
||
u8 year_compd; // hour境界補正済み現在年。comp(ensation -ed)
|
||
|
||
// 現在時刻取得
|
||
DI_wt_chk();
|
||
RWAIT = 1;
|
||
while( !RWST ){;}
|
||
|
||
cal_temp.hour_bcd = HOUR;
|
||
cal_temp.day_bcd = DAY;
|
||
cal_temp.month_bcd = MONTH;
|
||
cal_temp.year_bcd = YEAR;
|
||
cal_temp.min_bcd = MIN;
|
||
cal_temp.sec_bcd = SEC;
|
||
|
||
RWAIT = 0;
|
||
EI();
|
||
|
||
year_compd = bcdtob( cal_temp.year_bcd );
|
||
|
||
now_longhour = get_long_hour();
|
||
|
||
// 書き込みポインタの更新
|
||
if( ! ( vreg_ctr[ VREG_C_ACC_HOSU_L ] == 0 && // 歩数計on後、最初の一歩までは前回からの経過時間を計算しない
|
||
vreg_ctr[ VREG_C_ACC_HOSU_M ] == 0 &&
|
||
vreg_ctr[ VREG_C_ACC_HOSU_H ] == 0 )) //. 全ビットORでゼロ判定するのはデジタル回路屋の方言みたい
|
||
{
|
||
// 歩数計が止まっていた時間を考慮して必要なら進める
|
||
// 補正計算 元旦零時台で昨日扱いになった場合、大晦日の23時台に上書き
|
||
if( now_longhour == (u16)-1 ) // 昨年大晦日の23時台扱いのとき、計算結果が -1 になってる
|
||
{
|
||
now_longhour = 365 * 24 -1;
|
||
if( is_firstyear(year_compd) )
|
||
{
|
||
now_longhour += 24;
|
||
}
|
||
year_compd --;
|
||
}
|
||
fill_hosu_hist_hours( calc_hours_spend( year_compd ) ); // ■書き込みポインタの更新も行う
|
||
|
||
// ログあふれで記録停止？
|
||
if( pedolog_overflow )
|
||
{
|
||
return;
|
||
// おしまい。ログの更新もなし。
|
||
}
|
||
}
|
||
|
||
// インクリメントして良い
|
||
cal_log_latest = cal_temp; // ■ログ時刻更新
|
||
last_hour_fny = now_longhour;
|
||
|
||
// 毎時ログ インクリメント
|
||
if( pool.vreg_c_ext.pedo_log[ p_record ] == HOSU_MAX )
|
||
{
|
||
// 何もしないでおしまい
|
||
//. 小計の合計と累計があわなくなるのを避けるためだろうけど、どうなの？
|
||
return;
|
||
}
|
||
else if( pool.vreg_c_ext.pedo_log[ p_record ] == HOSU_NODATA ) // その時間帯最初のカウントの時
|
||
{ // これしないと1歩足りない
|
||
pool.vreg_c_ext.pedo_log[ p_record ] = 1;
|
||
}
|
||
else
|
||
{
|
||
// 通常パス
|
||
pool.vreg_c_ext.pedo_log[ p_record ] ++;
|
||
}
|
||
|
||
// 累積の更新 //
|
||
DI_wt_chk();
|
||
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; //. いろいろ失敗だったね...
|
||
vreg_ctr[ VREG_C_ACC_HOSU_M ] = 255;
|
||
vreg_ctr[ VREG_C_ACC_HOSU_H ] = 255;
|
||
}
|
||
}
|
||
}
|
||
EI();
|
||
}
|
||
|
||
|
||
/********************************************//**
|
||
空白の時間を適切に0にして、
|
||
今を含む１時間のデータを書く位置にポインタ？を進める
|
||
***********************************************/
|
||
static void fill_hosu_hist_hours( u16 hours )
|
||
{
|
||
// ログあふれ？
|
||
if( (u16)p_record + hours >= PEDOMETER_LOG_SIZE )
|
||
{
|
||
pedolog_overflow = true;
|
||
return;
|
||
}
|
||
|
||
// 空白の数時間の設定
|
||
while( hours != 0 )
|
||
{
|
||
// 新仕様 いっぱいで停止
|
||
p_record ++;
|
||
#if 1 // debug
|
||
if( p_record >= PEDOMETER_LOG_SIZE )
|
||
{
|
||
pedolog_overflow = true;
|
||
// dbg_nop(); // ここに来るようだとバグ
|
||
break;
|
||
}
|
||
else
|
||
#endif
|
||
{
|
||
pool.vreg_c_ext.pedo_log[ p_record ] = 0;
|
||
}
|
||
hours --;
|
||
}
|
||
|
||
return;
|
||
}
|
||
|
||
|
||
|
||
/********************************************//**
|
||
歩数をクリア、履歴を「データ無し」に初期化
|
||
***********************************************/
|
||
void clear_hosu_hist()
|
||
{
|
||
u8 hours = PEDOMETER_LOG_SIZE;
|
||
do
|
||
{
|
||
hours --;
|
||
pool.vreg_c_ext.pedo_log[ hours ] = 0xFFFF;
|
||
}
|
||
while( hours != 0 );
|
||
|
||
DI_wt_chk();
|
||
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;
|
||
pedolog_overflow = false;
|
||
EI();
|
||
}
|
||
|
||
|
||
extern u8 iic_burst_state;
|
||
bit pedolog_read_msb;
|
||
/********************************************//**
|
||
歩数計のヒストリを返す。
|
||
|
||
１回呼ぶ度に、ヒストリの下位、上位、一時間遡って下位上位...
|
||
***********************************************/
|
||
u8 hosu_read( )
|
||
{
|
||
u8 rv;
|
||
static u8 p_record_buffer;
|
||
static st_calender cal_buff; // 一応、アトミック処理に
|
||
|
||
if( iic_burst_state == 0 )
|
||
{
|
||
p_record_buffer = p_record;
|
||
DI_wt_chk();
|
||
cal_buff = cal_log_latest;
|
||
EI();
|
||
}
|
||
|
||
if( iic_burst_state <= 5 )
|
||
{
|
||
rv = *( (u8*)&cal_buff + iic_burst_state ); // あうあう
|
||
iic_burst_state ++;
|
||
return( rv );
|
||
}
|
||
else
|
||
{
|
||
u16 temp;
|
||
// 16ビットで記録してあるのでばらして送る /// もっと楽する方法があるんじゃ
|
||
temp = pool.vreg_c_ext.pedo_log[ p_record_buffer ];
|
||
if( !pedolog_read_msb )
|
||
{
|
||
rv = (u8)( temp & 0x00FF );
|
||
}
|
||
else
|
||
{
|
||
rv = (u8)(( temp >> 8 ) & 0x00FF );
|
||
if( p_record_buffer == 0 )
|
||
{
|
||
p_record_buffer = PEDOMETER_LOG_SIZE-1;
|
||
}
|
||
else
|
||
{
|
||
p_record_buffer --;
|
||
}
|
||
}
|
||
pedolog_read_msb ^= 1;
|
||
return( rv );
|
||
}
|
||
|
||
}
|
||
|
||
|
||
|
||
/********************************************//**
|
||
今年の元旦からの経過時間(hour)を返す。
|
||
|
||
引数 無し
|
||
|
||
返値 u16 long_hour
|
||
***********************************************/
|
||
const u16 DAYS_FROM_HNY[] = {
|
||
0,
|
||
31,
|
||
31+28, // =59。 …３月０日は１月５９日
|
||
31+28+31,
|
||
31+28+31+30,
|
||
31+28+31+30+31,
|
||
31+28+31+30+31+30,
|
||
31+28+31+30+31+30+31,
|
||
31+28+31+30+31+30+31+31,
|
||
31+28+31+30+31+30+31+31+30,
|
||
31+28+31+30+31+30+31+31+30+31,
|
||
31+28+31+30+31+30+31+31+30+31+30
|
||
};
|
||
|
||
static u16 get_long_hour()
|
||
{
|
||
u8 year = bcdtob( cal_temp.year_bcd );
|
||
u8 month = bcdtob( cal_temp.month_bcd );
|
||
u8 day = bcdtob( cal_temp.day_bcd );
|
||
u8 hour = bcdtob( cal_temp.hour_bcd );
|
||
u8 min_bcd = cal_temp.min_bcd; // 大小比較しかしないのでbcdのままでよい
|
||
u8 sec_bcd = cal_temp.sec_bcd;
|
||
u16 long_hour;
|
||
|
||
// まず日数の部分
|
||
long_hour = DAYS_FROM_HNY[ month -1 ]; // -1はインデックス合わせ
|
||
if( is_leapyear(year) && ( 3 <= month ))
|
||
{
|
||
// 閏年で、閏日より後
|
||
long_hour ++;
|
||
}
|
||
long_hour += day - 1;
|
||
long_hour *= 24; // 日数→時間
|
||
|
||
long_hour += hour;
|
||
|
||
// 時・分境界の前？後？
|
||
if( ( min_bcd > vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
|
||
|| ( ( min_bcd >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY ] )
|
||
&& ( sec_bcd >= vreg_ctr[ VREG_C_ACC_HOSU_HOUR_BOUNDARY_SEC ] ))
|
||
)
|
||
{
|
||
return( long_hour );
|
||
}
|
||
else
|
||
{
|
||
return( long_hour -1 ); // 1時間前に含める 注意：元旦で昨年扱いにするとき。-1 になる
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/********************************************//**
|
||
軽量平方根。
|
||
|
||
必要十分な精度で打ち切る
|
||
|
||
pc上でシミュレーションして大丈夫そう
|
||
***********************************************/
|
||
static 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;
|
||
}
|
||
|
||
|
||
|
||
|
||
/********************************************//**
|
||
二つの 前回呼ばれた時刻と、現在時刻の差分を求める。返るのはfill_hosu_hist_hours にそのまま渡せる
|
||
//. 引数がyear_bcd なのがちょっといやだけど...
|
||
***********************************************/
|
||
static u16 calc_hours_spend( u8 year )
|
||
{
|
||
u8 cal_log_latest_year = bcdtob( cal_log_latest.year_bcd );
|
||
|
||
// 同じ年の内
|
||
if( cal_log_latest_year == year )
|
||
{
|
||
if( now_longhour > last_hour_fny )
|
||
{
|
||
return( now_longhour - last_hour_fny );
|
||
}
|
||
else if( now_longhour == 0 && last_hour_fny != 0 )
|
||
{
|
||
// 年明けたばかりで、境界を越えた一回目。これやらないと昨年最後に加算してしまう。
|
||
return( 1 );
|
||
}
|
||
else
|
||
{
|
||
return( 0 ); // 同じ時間帯(と、巻き戻り。 どうなっても知らない)
|
||
}
|
||
}
|
||
else if( cal_log_latest_year == ( year -1 ) )
|
||
{
|
||
// 年をまたいでいるとき
|
||
u16 temp = 365 * 24 - last_hour_fny + now_longhour;
|
||
if( is_firstyear(year) )
|
||
{
|
||
temp += 24;
|
||
}
|
||
return( temp );
|
||
}
|
||
else if( cal_log_latest_year < year )
|
||
{
|
||
// 数年放置
|
||
return( PEDOMETER_LOG_SIZE +1 );
|
||
}
|
||
else
|
||
{
|
||
// カレンダーが巻き戻るなど
|
||
// ノーケアでよい…が、不定値というわけにもいかない
|
||
return( 0 );
|
||
}
|
||
}
|