mirror of
https://github.com/rvtr/ctr_mcu.git
synced 2025-06-19 09:05:48 -04:00
c30f9cad91
git-svn-id: file:///Volumes/Transfer/gigaleak_20231201/2020-05-23%20-%20ctr.7z%20+%20svn_v1.068.zip/ctr/svn/ctr_mcu@68 013db118-44a6-b54f-8bf7-843cb86687b1
503 lines
14 KiB
C
503 lines
14 KiB
C
/* ========================================================
|
||
加速度センサ関係
|
||
・データ更新完了でデータを吸い上げ手レジスタを更新、CPUに割り込み
|
||
・フラグが立っていれば歩数カウント
|
||
・加速度センサ割り込みからタスクを登録して下さい。（I2Cの競合回避などがあるので）
|
||
|
||
======================================================== */
|
||
#pragma SFR
|
||
#pragma NOP
|
||
#pragma HALT
|
||
#pragma STOP
|
||
#pragma ROT
|
||
// rorb, rolb, rorw, rolw
|
||
#pragma MUL
|
||
#pragma BCD
|
||
|
||
|
||
#include "incs.h"
|
||
#include <math.h>
|
||
|
||
// ========================================================
|
||
// レジスタ名
|
||
#define ACC_REG_WHOAMI 0x0F
|
||
#define ACC_REG_CTRL1 0x20
|
||
#define ACC_REG_CTRL5 0x24
|
||
#define ACC_REG_X 0x28
|
||
|
||
// ビット位置
|
||
#define ACC_bP_PM0 5
|
||
#define ACC_bP_DR0 3
|
||
|
||
// ビット設定値
|
||
#define ACC_BITS_PM_PDN 0
|
||
#define ACC_BITS_PM_NORM 1
|
||
#define ACC_BITS_PM_LP0R5 2
|
||
#define ACC_BITS_PM_LP1 3
|
||
#define ACC_BITS_PM_LP2 4
|
||
#define ACC_BITS_PM_LP5 5
|
||
#define ACC_BITS_PM_LP10 6
|
||
|
||
#define ACC_BITS_DR_50Hz 0
|
||
#define ACC_BITS_DR_100Hz 1
|
||
#define ACC_BITS_DR_400Hz 2
|
||
#define ACC_BITS_DR_1000Hz 3
|
||
|
||
#define ACC_BITS_ALL_AXIS_ON 7
|
||
|
||
|
||
#define VREG_BITMASK_ACC_CONF_HOSU ( 1 << 1 )
|
||
#define VREG_BITMASK_ACC_CONF_ACQ ( 1 << 0 )
|
||
|
||
// ========================================================
|
||
extern u8 pool[];
|
||
u16* hyst_pedometer;
|
||
|
||
|
||
|
||
// ========================================================
|
||
task_status tsk_soft_int( );
|
||
|
||
static void hosu_increment();
|
||
u16 get_long_hour();
|
||
|
||
|
||
|
||
|
||
/* ========================================================
|
||
・割り込みを確認してデータを吸い上げ、レジスタに書き出します
|
||
・本当であればコールバック関数を登録しておけばいいじゃんとなるのですが、
|
||
I2Cが使用中だったら？とか考えると私ではそこまでできないのです。
|
||
・自動歩数計とかでも結局
|
||
======================================================== */
|
||
task_status_immed tsk_cbk_accero( )
|
||
{ // （疑似）isrから登録されます
|
||
|
||
if( system_status.pwr_state == ON )
|
||
{
|
||
// 加速度センサデータレジスタへの反映
|
||
if( iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, &vreg_ctr[VREG_C_ACC_XL] )
|
||
== ERR_SUCCESS )
|
||
{
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0 )
|
||
{
|
||
set_irq( VREG_C_IRQ1, REG_BIT_ACC_DAT_RDY );
|
||
}
|
||
|
||
// ゴミデータのカラ読み
|
||
if( ACC_VALID == 1 )
|
||
{
|
||
u8 temp[6];
|
||
iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, temp );
|
||
}
|
||
}
|
||
else
|
||
{
|
||
// 加速度センサが異常になったので止める
|
||
vreg_ctr[VREG_C_ACC_CONFIG] &= ~( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ );
|
||
acc_hosu_set();
|
||
return ( ERR_SUCCESS ); // タスクの削除は必要
|
||
}
|
||
}
|
||
|
||
// 歩数計 /////////////////////////////////////////////
|
||
// offでなければ、電源off中でも計測
|
||
// if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_HOSU ) != 0x00 )
|
||
{
|
||
static s16 th_H = 0x3500; // 閾値。暫定。動的変更とかしたい…ので変数
|
||
static s16 th_L = 0x2A00;
|
||
static u16 acc_norm[3]; // 加速度の大きさのヒストリ。数字が大きい方が古い
|
||
static u16 interval_hh; // 山-山間の時間。短過ぎたらはじく。
|
||
static u16 time_l; // 前回の極小からの経過時間
|
||
|
||
|
||
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] );
|
||
|
||
acc_norm[2] = acc_norm[1];
|
||
acc_norm[1] = acc_norm[0];
|
||
// そのうちローコストな方法を考える、かも
|
||
acc_norm[0] = sqrt( (long)sx16 * ( sx16 / 2 ) +
|
||
(long)sy16 * ( sy16 / 2 ) +
|
||
(long)sz16 * ( sz16 / 2 ) );
|
||
|
||
if( acc_norm[2] <= acc_norm[1] && acc_norm[1] > acc_norm[0]
|
||
&& acc_norm[0] > th_H )
|
||
// 極大で、閾値を超えていた
|
||
{
|
||
if(( 25 < interval_hh ) && ( interval_hh < 200 ))
|
||
// 前回の極大からの間隔がほどよい
|
||
{
|
||
if( time_l < interval_hh )
|
||
// 谷を挟んでいる
|
||
{
|
||
// 一歩増えました
|
||
hosu_increment();
|
||
}
|
||
}
|
||
interval_hh = 0;
|
||
}
|
||
else
|
||
{
|
||
interval_hh += ( interval_hh != 65535 ) ? 1: 0; // 飽和加算って楽に書けたらいいのに
|
||
}
|
||
|
||
// (2) 直近の極小からの時間
|
||
if( acc_norm[2] >= acc_norm[1] && acc_norm[1] < acc_norm[0]
|
||
&& acc_norm[0] < th_L )
|
||
{
|
||
time_l = 0;
|
||
}
|
||
else
|
||
{
|
||
time_l += ( time_l != 65535 ) ? 1: 0;
|
||
}
|
||
}
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
/*=========================================================
|
||
歩数＋１
|
||
累積をインクリメント
|
||
履歴を更新
|
||
========================================================*/
|
||
#define MAGIC_PEDO_NODATA 65535
|
||
#define MAGIC_PEDO_MAX 65534
|
||
|
||
u8 p_record;
|
||
|
||
static void hosu_increment()
|
||
{
|
||
static u16 last_hour; // 履歴の最新は何時？
|
||
static u8 last_year;
|
||
|
||
// 累積の更新 //
|
||
// いろいろ失敗した...
|
||
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 );
|
||
now_hour = get_long_hour();
|
||
// RWAIT = 0; ↑で行っています
|
||
// EI(); 〃
|
||
|
||
// 歩数計が止まったいた時間を考慮して必要なら進める //
|
||
if( last_year == now_year )
|
||
{
|
||
if( now_hour > last_hour )
|
||
{
|
||
fill_hosu_hist_hours( now_hour - last_hour );
|
||
}
|
||
}
|
||
else if( last_year == ( now_year -1 ) )
|
||
{
|
||
// 年をまたいでいるとき
|
||
u16 temp_hours = ( ( 365 + (( now_year & 0x03 ) == 1 )? 1: 0 ) * 24 ) - last_hour + now_hour;
|
||
fill_hosu_hist_hours( temp_hours );
|
||
}
|
||
else if( last_year < now_year )
|
||
{
|
||
// 数年放置
|
||
fill_hosu_hist_hours( 255 );
|
||
}
|
||
else
|
||
{
|
||
// 巻き戻るなど
|
||
// 知らない…
|
||
}
|
||
|
||
last_year = now_year;
|
||
last_hour = now_hour;
|
||
|
||
// 実際にインクリメント
|
||
*( hyst_pedometer + p_record ) += 1;
|
||
if( *( hyst_pedometer + p_record ) > MAGIC_PEDO_MAX )
|
||
{
|
||
*( hyst_pedometer + p_record ) = MAGIC_PEDO_MAX;
|
||
}
|
||
|
||
}
|
||
}
|
||
|
||
|
||
|
||
void fill_hosu_hist_hours( u16 hours )
|
||
{
|
||
if( hours > 255 )
|
||
{
|
||
hours = 255;
|
||
}
|
||
|
||
hours -= 1;
|
||
|
||
// 空白の数時間の設定
|
||
while( hours != 0 )
|
||
{
|
||
p_record += 1;
|
||
*( hyst_pedometer + p_record ) = MAGIC_PEDO_NODATA;
|
||
hours -= 1;
|
||
}
|
||
p_record += 1;
|
||
*( hyst_pedometer + p_record ) = 0;
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
|
||
bit hosu_read_inited;
|
||
bit record_read_msb_lsb;
|
||
|
||
/* ========================================================
|
||
歩数計ヒストリ読み出しの後処理(初期化)
|
||
読み出しポインタのクリア
|
||
======================================================== */
|
||
void hosu_read_end( )
|
||
{
|
||
record_read_msb_lsb = 0;
|
||
hosu_read_inited = 0;
|
||
}
|
||
|
||
/* ========================================================
|
||
歩数計のヒストリを返す。
|
||
１回呼ぶ度に、ヒストリの下位、上位、一時間遡って下位上位...
|
||
======================================================== */
|
||
u8 hosu_read( )
|
||
{
|
||
u16 temp;
|
||
u8 rv;
|
||
|
||
static u8 p_record_buffer;
|
||
|
||
if( hosu_read_inited == 0 )
|
||
{
|
||
p_record_buffer = p_record;
|
||
hosu_read_inited = 1;
|
||
}
|
||
|
||
temp = *( hyst_pedometer + p_record_buffer );
|
||
if( record_read_msb_lsb == 0 )
|
||
{
|
||
rv = (u8)( ( temp & 0xFF00 ) >> 8 );
|
||
}
|
||
else
|
||
{
|
||
rv = (u8)( temp & 0x00FF );
|
||
p_record_buffer -= 1;
|
||
}
|
||
record_read_msb_lsb += 1;
|
||
|
||
return( rv );
|
||
}
|
||
|
||
|
||
void hosu_init()
|
||
{
|
||
u8 i = 0;
|
||
|
||
hyst_pedometer = (u16*)pool;
|
||
do
|
||
{
|
||
*( hyst_pedometer + i ) = MAGIC_PEDO_NODATA;
|
||
i += 1;
|
||
}while( i != 0 );
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/*=========================================================
|
||
加速度センサ透過アクセス リード
|
||
========================================================*/
|
||
task_status_immed acc_read( )
|
||
{
|
||
vreg_ctr[VREG_C_ACC_W_BUF] = iic_mcu_read_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_R_ADRS] );
|
||
// vreg_ctr[ VREG_C_ACC_R_BUF ] = iic_mcu_read_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_R_ADRS] );
|
||
vreg_ctr[VREG_C_IRQ1] |= REG_BIT_ACC_ACK;
|
||
if( ( vreg_ctr[VREG_C_IRQ_MASK1] & REG_BIT_ACC_ACK ) == 0 )
|
||
{
|
||
IRQ0_ast;
|
||
}
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
/*=========================================================
|
||
加速度センサ透過アクセス ライト
|
||
========================================================*/
|
||
task_status_immed acc_write( )
|
||
{
|
||
iic_mcu_write_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_W_ADRS], vreg_ctr[VREG_C_ACC_W_BUF] );
|
||
vreg_ctr[VREG_C_IRQ1] |= REG_BIT_ACC_ACK;
|
||
if( ( vreg_ctr[VREG_C_IRQ_MASK1] & REG_BIT_ACC_ACK ) == 0 )
|
||
{
|
||
IRQ0_ast;
|
||
}
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
/*=========================================================
|
||
自動歩数カウントモードにセット
|
||
todo 他のモードだったら止めたり、復帰させたり
|
||
割り込みルーチンなどでカウント判定が必要
|
||
========================================================*/
|
||
task_status_immed acc_hosu_set( )
|
||
{
|
||
u8 str_send_buf[4];
|
||
|
||
iic_mcu_read_a_byte( IIC_SLA_ACCEL, ACC_REG_WHOAMI );
|
||
if( iic_mcu_bus_status == ERR_NOSLAVE )
|
||
{
|
||
vreg_ctr[ VREG_C_STATUS_1 ] |= REG_BIT_ACCERO_ERR;
|
||
#ifdef _MCU_BSR_
|
||
// PMK23 = 1;
|
||
#endif
|
||
return ( ERR_SUCCESS ); // とりあえず、タスクは削除しなくてはならない
|
||
}else{
|
||
vreg_ctr[ VREG_C_STATUS_1 ] &= ~REG_BIT_ACCERO_ERR;
|
||
}
|
||
|
||
|
||
str_send_buf[1] = 0x00; // ctrl2 HPF:normal, filterd, HPF for IRQ : dis/dis, HPF coeff:norm
|
||
#ifdef _MODEL_WM0_
|
||
str_send_buf[2] = 0x10; // 3 IRQ pol :Active HI, Drive:Pushpull,
|
||
/// IRQ2flg latch: auto clear after read, IRQ2 conf: IRQ( fall,shock,...)
|
||
/// 1 : auto clear after read, conf: data ready
|
||
#else
|
||
str_send_buf[2] = 0x02; // 3 IRQ pol :Active HI, Drive:Pushpull,
|
||
/// IRQ2flg latch: auto clear after read, IRQ2 conf: IRQ( fall,shock,...)
|
||
/// 1 : auto clear after read, conf: data ready
|
||
#endif
|
||
|
||
str_send_buf[3] = 0x80; // ctrl3 block update:enable, MSB first, scale: +-2G(default), selftest: dis
|
||
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] &
|
||
( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ ) ) == 0 )
|
||
{
|
||
#ifdef _MCU_BSR_
|
||
PMK23 = 1;
|
||
#endif
|
||
// 完全停止
|
||
str_send_buf[0] =
|
||
( ACC_BITS_PM_PDN << ACC_bP_PM0 | 0 << ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
|
||
}
|
||
else
|
||
{
|
||
#ifdef _MCU_BSR_
|
||
PMK23 = 0;
|
||
#endif
|
||
// 100Hz 自動取り込み
|
||
str_send_buf[0] =
|
||
( ACC_BITS_PM_NORM << ACC_bP_PM0
|
||
| ACC_BITS_DR_100Hz << ACC_bP_DR0
|
||
| ACC_BITS_ALL_AXIS_ON );
|
||
}
|
||
iic_mcu_write( IIC_SLA_ACCEL, ( ACC_REG_CTRL1 | 0x80 ), 4, str_send_buf );
|
||
|
||
// カラ読み
|
||
if( ACC_VALID == 1 )
|
||
{
|
||
if( system_status.pwr_state == ON )
|
||
{
|
||
u8 temp[6];
|
||
iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, temp );
|
||
}
|
||
}
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
加速度センサ割り込み
|
||
I2Cが使用中かもしれないので、読み出しタスクの登録を行うのみ
|
||
======================================================== */
|
||
__interrupt void intp23_ACC_ready( )
|
||
{
|
||
EI();
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] & 0x03 ) != 0x00 )
|
||
{
|
||
if( ( system_status.pwr_state == ON ) || ( system_status.pwr_state == SLEEP ) )
|
||
{
|
||
if( ACC_VALID )
|
||
{
|
||
renge_task_immed_add( tsk_cbk_accero );
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
今年の元旦からの経過時間(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;
|
||
long_hour *= 24; // 日数→時間
|
||
|
||
long_hour += hour_hex;
|
||
|
||
return( long_hour );
|
||
}
|
||
|
||
|
||
|