/* ========================================================
　加速度センサ関係
・データ更新完了でデータを吸い上げ手レジスタを更新、CPUに割り込み
・フラグが立っていれば歩数カウント
・加速度センサ割り込みからタスクを登録して下さい。（I2Cの競合回避などがあるので）

 $Id: accero.c 418 2011-09-22 01:35:37Z n2232 $
 ======================================================== */
#ifndef _WIN32

#pragma SFR
#pragma NOP
#pragma HALT
#pragma STOP
#pragma ROT
// rorb, rolb, rorw, rolw
#pragma MUL
#pragma BCD

#endif

#ifndef _WIN32

#pragma interrupt INTP23	intp23_ACC_ready RB3       // 加速度センサ、データ準備完了

#endif

#include "incs.h"
#include "accero.h"
#include "i2c_mcu.h"


#ifndef _WIN32
#include <math.h>
#endif
// ========================================================
// レジスタ名
#define ACC_REG_WHOAMI 0x0F
#define ACC_REG_CTRL1  0x20
#define ACC_REG_CTRL5  0x24
#define ACC_REG_X      0x28

#define ACC_REG_FLG_BURST_ACCESS	0x80

// ビット位置
#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_ACQ  ( 1 << 0 )
#define VREG_BITMASK_ACC_CONF_HOSU ( 1 << 1 )



// ========================================================
#define ACC_RAW_DATA_SIZE	6


// ========================================================
static u8 acc_retry_count;	// 加速度センサ読み出し時、エラーだったときリトライする。回数数える



// ========================================================
task_status tsk_soft_int(  );



/********************************************//**
　・割り込みを確認してデータを吸い上げ、レジスタに書き出します

  ・本当であればコールバック関数を登録しておけばいいじゃんとなるのですが、
  I2Cが使用中だったら？とか考えると私ではそこまでできないのです。

  ・自動歩数計とかでも結局
 ***********************************************/
task_status_immed tski_cbk_accero(  )
{                               // （疑似）isrから登録されます
	u8 acc_dat_buff[ ACC_RAW_DATA_SIZE ];

    // 加速度センサデータレジスタへの反映
    if( iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | ACC_REG_FLG_BURST_ACCESS ), ACC_RAW_DATA_SIZE, acc_dat_buff )
        != ERR_SUCCESS )
    {
		// SLA NAK //
        if( acc_retry_count < 4 )
        {
	        acc_retry_count ++;
            // リトライ
            return( TSKI_CONTINUE );
        }
        else
        {
            // 加速度センサが異常になったので止める
            vreg_ctr[ VREG_C_ACC_CONFIG ] &= ~( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ );
            tski_acc_setup();
            vreg_ctr[ VREG_C_STATUS_1 ] |= REG_BIT_ACCERO_ERR;
            return ( TSKI_FINISHED ); // タスクの削除は必要
        }
    }
    else
    {
        // 正常時パス //
		DI_wt_chk();
		memcpy( &vreg_ctr[VREG_C_ACC_XL], acc_dat_buff, ACC_RAW_DATA_SIZE );
		EI();

        acc_retry_count = 0;
        // 加速度更新＆割り込み
        if( (( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_ACQ ) != 0 ) &&
            ( system_status.pwr_state == ON )
            )
            {
                set_irq( VREG_C_IRQ1, REG_BIT_ACC_DAT_RDY );
            // ゴミデータのカラ読み
            if( ACC_VALID )
            {
                u8 temp[ACC_RAW_DATA_SIZE];
                iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | ACC_REG_FLG_BURST_ACCESS ), ACC_RAW_DATA_SIZE, temp );
            }
        }
        if( (( system_status.pwr_state == ON )
			 ||( system_status.pwr_state == SLEEP ))
            &&( ( vreg_ctr[VREG_C_ACC_CONFIG] & VREG_BITMASK_ACC_CONF_HOSU ) != 0 )
            )
        {
            pedometer();    // 歩数計
        }
    }
    return ( TSKI_FINISHED );
}





/********************************************//**
　加速度センサ透過アクセス　リード
 ***********************************************/
task_status_immed tski_acc_read(  )
{
    vreg_ctr[VREG_C_ACC_W_BUF] = iic_mcu_read_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_R_ADRS] );
	set_irq( VREG_C_IRQ1, REG_BIT_ACC_ACK );
    return ( TSKI_FINISHED );
}



/********************************************//**
　加速度センサ透過アクセス　ライト
 ***********************************************/
task_status_immed tski_acc_write(  )
{
    iic_mcu_write_a_byte( IIC_SLA_ACCEL, vreg_ctr[VREG_C_ACC_W_ADRS], vreg_ctr[VREG_C_ACC_W_BUF] );
	set_irq( VREG_C_IRQ1, REG_BIT_ACC_ACK );
    return ( TSKI_FINISHED );
}



/********************************************//**
　加速度センサの設定
 ***********************************************/
task_status_immed tski_acc_setup(  )
{

	// 加速度 on/off設定する
	u8      str_send_buf[4];

	str_send_buf[1] = 0x00;     // ctrl2  HPF:normal, filterd, HPF for IRQ : dis/dis, HPF coeff:norm

	// ピン不足のため、TSとそれ以外（回路違い）を区別して設定
	if( system_status.model == MODEL_TS_BOARD )
	{
		str_send_buf[2] = bits8(0,0,0,0, 0,0,1,0);	
	}
	else
	{
		str_send_buf[2] = bits8(0,0,0,1, 0,0,0,0);
	}
	str_send_buf[3] = 0x80;         // ctrl3  block update:enable, MSB first, scale: +-2G(default), selftest: dis

	{
		u8 acc_setting_sent = ( vreg_ctr[VREG_C_ACC_CONFIG] &
			( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ ));

		if( acc_setting_sent == 0 )
		{
			// 完全停止
			PMK23 = 1;
			str_send_buf[0] =
				( ACC_BITS_PM_PDN << ACC_bP_PM0 | 0 << ACC_bP_DR0 | ACC_BITS_ALL_AXIS_ON );
		}
		else
		{
			// on	100Hz 自動取り込み
			PMK23 = 0;
			str_send_buf[0] =
				( ACC_BITS_PM_NORM << ACC_bP_PM0
					| ACC_BITS_DR_100Hz << ACC_bP_DR0
					| ACC_BITS_ALL_AXIS_ON );
		}
		
		// 実書き込み兼、通信できたかフラグ更新
		if( iic_mcu_write( IIC_SLA_ACCEL, ( ACC_REG_CTRL1 | ACC_REG_FLG_BURST_ACCESS ), 4, str_send_buf ) == I2C_ERR_NOSLAVE )
		{
			// センサ反応無し。タスクは削除しなくてはならない。
			vreg_ctr[ VREG_C_STATUS_1 ] |= REG_BIT_ACCERO_ERR;
		    return ( TSKI_FINISHED );
			// おしまい
		}
		// else

		// センサ設定成功 正常パス
		vreg_ctr[ VREG_C_STATUS_1 ] &= ~REG_BIT_ACCERO_ERR;
		acc_retry_count = 0;

		// 前回のゴミが有ればカラ読み
		if( ACC_VALID )
		{
			if( system_status.pwr_state == ON )
			{
				u8 temp[ACC_RAW_DATA_SIZE];
				iic_mcu_read( IIC_SLA_ACCEL, ( ACC_REG_X | ACC_REG_FLG_BURST_ACCESS ), 6, temp );
			}
		}

		// センサに書きにいっている最中にSoCがまた書き換えてしまうかもしれない
		DI_wt_chk(); 
		if( acc_setting_sent != ( vreg_ctr[VREG_C_ACC_CONFIG] &
				( VREG_BITMASK_ACC_CONF_HOSU | VREG_BITMASK_ACC_CONF_ACQ )))
		{
			// DI状態のまま帰る
			return ( TSKI_CONTINUE );	// ちょっと間をおいて再度書きにくる // もっとよい実装？
		}
	}

	// DI状態のまま帰る
    return ( TSKI_FINISHED );
}



/********************************************//**
 加速度センサ割り込み

  I2Cが使用中かもしれないので、読み出しタスクの登録を行うのみ
 ***********************************************/
__interrupt void intp23_ACC_ready(  )
{
    EI();
    if( ( vreg_ctr[VREG_C_ACC_CONFIG] & 0x03 ) != 0x00 )	
    {
		// 加速度センサ on
		if( ( system_status.pwr_state == ON ) || ( system_status.pwr_state == SLEEP ) )
        {
//            if( ACC_VALID )	// 確認不要
            {
                renge_task_immed_add( tski_cbk_accero );
            }
        }
    }
}