inline void MacroAssembler::trap_range_check_ge(Register a, int si16) {
twi(traptoEqual | traptoGreaterThanUnsigned, a/*reg a*/, si16);
}
inline void MacroAssembler::trap_range_check_l(Register a, int si16) {
twi(traptoLessThanUnsigned, a/*reg a*/, si16);
}
ISR(TWI_vect, ISR_BLOCK)
{
uint8_t i2c_status = TWSR & 0xF8;
switch (i2c_status)
{
case 0x08: //Start
{
if (i2c_state == 0)
{
TWDR = CMS50L_ADDR | I2C_WRITE;
twi(TWI_TRANSMIT);
i2c_state = 1;
}
break;
}
case 0x18:
{
if (i2c_state == 1)
{
TWDR = 0x00;
twi(TWI_TRANSMIT);
i2c_state = 2;
}
break;
}
case 0x28:
{
if (i2c_state == 2)
{
twi(TWI_RESTART);
i2c_state = 3;
}
break;
}
case 0x10:
{
if (i2c_state == 3)
{
TWDR = CMS50L_ADDR | I2C_READ;
twi(TWI_TRANSMIT);
i2c_state = 4;
}
else if (i2c_state == 0)
{
TWDR = CMS50L_ADDR | I2C_WRITE;
twi(TWI_TRANSMIT);
i2c_state = 1;
}
break;
}
case 0x40:
{
if (i2c_state == 4)
{
twi(TWI_RECEIVE_ACK);
i2c_state = 5;
}
break;
}
case 0x50:
{
if (i2c_state == 5)
{
Measurements[HR].value = TWDR;
twi(TWI_RECEIVE_NACK);
i2c_state = 6;
}
break;
}
case 0x58:
{
if (i2c_state == 6)
{
Measurements[SPO2].value = TWDR;
twi(TWI_RESTART);
i2c_state = 0;
}
break;
}
default:
{
twi(TWI_RESTART);
i2c_state = 0;
}
}
}
uint8_t ADXL345_Init()
{
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x31; //DATA_FORMAT
twi(TWI_TRANSMIT);
TWDR=0b00001011;
twi(TWI_TRANSMIT);
twi(TWI_STOP);
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x2C; //BW_RATE
twi(TWI_TRANSMIT);
TWDR=0b00000111; //0x09; //0b00001111 rate=50hz, bw=20hz
twi(TWI_TRANSMIT);
twi(TWI_STOP);
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x2D; //POWER_CTL
twi(TWI_TRANSMIT);
TWDR=0x08;
twi(TWI_TRANSMIT);
twi(TWI_STOP);
/*
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x2E; //INT_ENABLE
twi(TWI_TRANSMIT);
TWDR=0x80;
twi(TWI_TRANSMIT);
twi(TWI_STOP);
*/
return 0;
}
uint8_t ADXL345_Poll()
{
uint8_t LSB, MSB;
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x32;
twi(TWI_TRANSMIT);
twi(TWI_RESTART);
TWDR=ADXL345_ADDR|I2C_READ;
twi(TWI_TRANSMIT);
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_y.value = k_y.value;
k_y.value = (-0.0032) * (double)(MSB << 8 | LSB);
//k_y.value = LowPassFilter((-0.0032) * (double)(MSB << 8 | LSB), p_k_y.value - coeffs[ADXL345_OFFY].value, coeffs[LP_AX_Y].value);
k_y.value += coeffs[ADXL345_OFFY].value;
k_y.value *= coeffs[ADXL345_SCY].value;
//Measure_Push((-0.0032) * (double)(MSB << 8 | LSB) + coeffs[ADXL345_OFFY].value, KY);
//Measure_Push((MSB << 8 | LSB), KY);
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_x.value = k_x.value;
k_x.value = (-0.0032) * (double)(MSB << 8 | LSB);
//k_x.value = LowPassFilter((-0.0032) * (double)(MSB << 8 | LSB), p_k_x.value - coeffs[ADXL345_OFFX].value, coeffs[LP_AX_X].value);
k_x.value += coeffs[ADXL345_OFFX].value;
k_x.value *= coeffs[ADXL345_SCX].value;
//Measure_Push((MSB << 8 | LSB), KX);
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_NACK);
MSB=TWDR;
twi(TWI_STOP);
p_k_z.value = k_z.value;
k_z.value = (0.0032) * (double)(MSB << 8 | LSB);
//k_z.value = LowPassFilter((0.0032) * (double)(MSB << 8 | LSB), p_k_z.value - coeffs[ADXL345_OFFZ].value, coeffs[LP_AX_Z].value);
k_z.value += coeffs[ADXL345_OFFZ].value;
k_z.value *= coeffs[ADXL345_SCZ].value;
//Measure_Push((MSB << 8 | LSB), KZ);
return 0;
}
uint8_t ADXL345_Poll()
{
uint8_t LSB, MSB;
twi(TWI_START);
TWDR=ADXL345_ADDR|I2C_WRITE;
twi(TWI_TRANSMIT);
TWDR=0x32;
twi(TWI_TRANSMIT);
twi(TWI_RESTART);
TWDR=ADXL345_ADDR|I2C_READ;
twi(TWI_TRANSMIT);
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_y.value = k_y.value;
k_y.value = LowPassFilter((-0.0032) * (float)(MSB << 8 | LSB), 1);
k_y.value += coeffs[ADXL345_OFFY].value;
k_y.value *= coeffs[ADXL345_SCY].value;
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_ACK);
MSB=TWDR;
p_k_x.value = k_x.value;
k_x.value = LowPassFilter((-0.0032) * (float)(MSB << 8 | LSB), 2);
k_x.value += coeffs[ADXL345_OFFX].value;
k_x.value *= coeffs[ADXL345_SCX].value;
twi(TWI_RECEIVE_ACK);
LSB=TWDR;
twi(TWI_RECEIVE_NACK);
MSB=TWDR;
twi(TWI_STOP);
p_k_z.value = k_z.value;
k_z.value = LowPassFilter((0.0032) * (float)(MSB << 8 | LSB), 3);
k_z.value += coeffs[ADXL345_OFFZ].value;
k_z.value *= coeffs[ADXL345_SCZ].value;
return 0;
}
