mirror of
https://github.com/rvtr/ctr_mcu.git
synced 2025-10-31 13:51:10 -04:00
5e9ec34e61
全ファイルをindentに通した ほか、たくさん修正 git-svn-id: file:///Volumes/Transfer/gigaleak_20231201/2020-05-23%20-%20ctr.7z%20+%20svn_v1.068.zip/ctr/svn/ctr_mcu@12 013db118-44a6-b54f-8bf7-843cb86687b1
320 lines
9.9 KiB
C
320 lines
9.9 KiB
C
/* ========================================================
|
||
加速度センサ関係
|
||
・データ更新完了でデータを吸い上げ手レジスタを更新、CPUに割り込み
|
||
・フラグが立っていれば歩数カウント
|
||
・加速度センサ割り込みからタスクを登録して下さい。（I2Cの競合回避などがあるので）
|
||
|
||
======================================================== */
|
||
#pragma SFR
|
||
#pragma NOP
|
||
#pragma HALT
|
||
#pragma STOP
|
||
#pragma ROT
|
||
// rorb, rolb, rorw, rolw
|
||
#pragma MUL
|
||
|
||
#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 )
|
||
|
||
// ========================================================
|
||
static u8 hyst_pedometer[256];
|
||
|
||
|
||
// ========================================================
|
||
task_interval tsk_soft_int( );
|
||
|
||
|
||
|
||
|
||
/* ========================================================
|
||
・割り込みを確認してデータを吸い上げ、レジスタに書き出します
|
||
・本当であればコールバック関数を登録しておけばいいじゃんとなるのですが、
|
||
I2Cが使用中だったら？とか考えると私ではそこまでできないのです。
|
||
・自動歩数計とかでも結局
|
||
======================================================== */
|
||
task_status_immed tsk_cbk_accero( )
|
||
{ // （疑似）isrから登録されます
|
||
|
||
if( system_status.pwr_state == ON )
|
||
{
|
||
// 加速度センサデータレジスタへの反映
|
||
iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | 0x80 ), 6, &vreg_ctr[VREG_C_ACC_XL] );
|
||
|
||
{
|
||
// 歩数計 //
|
||
unsigned int mean; // 二乗平均はあまりにも処理が重いので(!) 絶対値の合計にします
|
||
static u8 direction;
|
||
|
||
static s16 th_H = 0x2380; // 閾値。暫定。動的変更とかしたい…
|
||
static s16 th_L = 0x2180;
|
||
|
||
static u16 interval; // 山と谷の間の時間。短すぎても長すぎてもはじく。
|
||
|
||
|
||
s16 sx16 = vreg_ctr[VREG_C_ACC_XH] * 256 + vreg_ctr[VREG_C_ACC_XL];
|
||
s16 sy16 = vreg_ctr[VREG_C_ACC_YH] * 256 + vreg_ctr[VREG_C_ACC_YL];
|
||
s16 sz16 = vreg_ctr[VREG_C_ACC_ZH] * 256 + vreg_ctr[VREG_C_ACC_ZL];
|
||
|
||
// そのうちローコストな方法を考え
|
||
mean = sqrt( (long)abs( sx16 ) * abs( sx16 ) /4 +
|
||
(long)abs( sy16 ) * abs( sy16 ) /4 +
|
||
(long)abs( sz16 ) * abs( sz16 ) /4 );
|
||
|
||
{
|
||
static u8 count_H = 0;
|
||
static u8 count_L = 0;
|
||
|
||
if( direction == 0 ) // 前回に下限を下回っていて
|
||
{
|
||
if( mean > th_H ) // 今回、上の閾値を上回った
|
||
{
|
||
if( count_H == 5 ){ // 突発的なノイズは省く...
|
||
direction = 1;
|
||
count_L = 0;
|
||
}
|
||
if( count_H <= 5 ){
|
||
count_H += 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
count_H = 0;
|
||
}
|
||
}
|
||
|
||
if( count_H >= 5 ){
|
||
interval += 1;
|
||
}
|
||
if( 500 < interval ){
|
||
interval = 0;
|
||
direction = 0;
|
||
count_H = 0;
|
||
count_L = 0;
|
||
}
|
||
|
||
// 下の閾値を超えるのを待つ
|
||
if( direction == 1 )
|
||
{
|
||
if( mean < th_L )
|
||
{
|
||
if( count_L == 5 ){
|
||
direction = 0; // 次は上の閾値を待つ
|
||
if( count_H >= 5 ){
|
||
if( 70 < interval )
|
||
{
|
||
vreg_ctr[ VREG_C_ACC_HOSU_L ] += 1; // 一歩加算
|
||
hyst_pedometer[0] += 1;
|
||
}
|
||
}
|
||
count_H = 0;
|
||
}
|
||
if( count_L <= 5 ){
|
||
count_L += 1;
|
||
}
|
||
}
|
||
else
|
||
{
|
||
count_L = 0;
|
||
}
|
||
}
|
||
}
|
||
|
||
// debug
|
||
if( mean > th_H )
|
||
{
|
||
DBG_LED_WIFI_on;
|
||
DBG_LED_WIFI_2_off;
|
||
}
|
||
|
||
if( mean < th_L )
|
||
{
|
||
DBG_LED_WIFI_off;
|
||
DBG_LED_WIFI_2_on;
|
||
}
|
||
|
||
}
|
||
|
||
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] &
|
||
( VREG_BITMASK_ACC_CONF_ACQ | VREG_BITMASK_ACC_CONF_HOSU ) ) != 0 )
|
||
{
|
||
set_irq( VREG_C_IRQ1, REG_BIT_ACC_DAT_RDY );
|
||
}
|
||
}
|
||
|
||
// 歩数計 offでなければ、電源off中でも計測
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_HOSU ) != 0x00 )
|
||
{
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0x00 )
|
||
{
|
||
// 歩数計アルゴリズム 100Hz版
|
||
}
|
||
else
|
||
{
|
||
// 同 省電力版
|
||
}
|
||
|
||
}
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
|
||
|
||
/*=========================================================
|
||
加速度センサ透過アクセス リード
|
||
========================================================*/
|
||
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 )
|
||
{
|
||
return ( ERR_SUCCESS ); // とりあえず、タスクは削除しなくてはならない
|
||
}
|
||
|
||
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
|
||
if( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0x00 )
|
||
{
|
||
// 100Hz 自動取り込み
|
||
str_send_buf[0] =
|
||
( ACC_BITS_PM_NORM << ACC_bP_PM0 | ACC_BITS_DR_100Hz <<
|
||
ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
|
||
}
|
||
else
|
||
{
|
||
// 100Hz 自動取り込み
|
||
str_send_buf[0] =
|
||
( ACC_BITS_PM_NORM << ACC_bP_PM0 | ACC_BITS_DR_100Hz <<
|
||
ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
|
||
// // 10Hz 自動取り込み(歩数計向け省電力モード)
|
||
// str_send_buf[0] = ( ACC_BITS_PM_LP10 << ACC_bP_PM0 | 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 )
|
||
{
|
||
renge_task_immed_add( tsk_cbk_accero );
|
||
}
|
||
}
|
||
|
||
return ( ERR_SUCCESS );
|
||
}
|
||
|
||
|
||
|
||
/* ========================================================
|
||
加速度センサ割り込み
|
||
I2Cが使用中かもしれないので、読み出しタスクの登録を行うのみ
|
||
======================================================== */
|
||
__interrupt void intp23_ACC_ready( )
|
||
{
|
||
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 );
|
||
}
|
||
}
|
||
}
|
||
}
|