void adxl345_driver_init_slow(void)
{
static twim_options_t twi_opt =
{
.pba_hz = 64000000,
.speed = 400000,
.chip = ADXL_ALT_SLAVE_ADDRESS,
.smbus = false
};
twim_master_init(&AVR32_TWIM0, &twi_opt);
twim_write(&AVR32_TWIM0, (uint8_t*)&default_configuration, 2, ADXL_ALT_SLAVE_ADDRESS, false);
twim_write(&AVR32_TWIM0, (uint8_t*)&data_configuration, 2, ADXL_ALT_SLAVE_ADDRESS, false);
}
acc_data_t* adxl345_driver_get_acc_data_slow(void)
{
int32_t i;
uint8_t write_then_read_preamble=SENSOR_REG_ADDRESS;
twim_write(&AVR32_TWIM0, (uint8_t*)&write_then_read_preamble, 1, ADXL_ALT_SLAVE_ADDRESS, false);
twim_read(&AVR32_TWIM0, (uint8_t*)&acc_outputs, 6, ADXL_ALT_SLAVE_ADDRESS, false);
for (i = 0; i < 3; i++)
{
acc_outputs.axes[i] = (int16_t)(acc_outputs.raw_data[2 * i]) + (int16_t)(acc_outputs.raw_data[2 * i + 1] << 8);
}
return (acc_data_t*)&acc_outputs;
}
void hmc5883l_update(magnetometer_t *compass_outputs)
{
uint8_t start_address = DATA_REG_BEGIN;
uint16_t data[3];
twim_write(&AVR32_TWIM0, (uint8_t*) &start_address, 1, HMC5883_SLAVE_ADDRESS, false);
twim_read(&AVR32_TWIM0, (uint8_t*)(data), 6, HMC5883_SLAVE_ADDRESS, false);
compass_outputs->data[0] = (float)((int16_t)data[0]);
compass_outputs->data[1] = (float)((int16_t)data[1]);
compass_outputs->data[2] = (float)((int16_t)data[2]);
}
int twi_master_read(volatile avr32_twim_t *twi, const twi_package_t *package) {
unsigned int addr = Swap32(package->addr);
// Set Register address if needed
if (package->addr_length) {
#if AVR32_TWIM_H_VERSION > 101
while(twim_write(twi, (unsigned char*)&addr, package->addr_length, package->chip,0)!=TWI_SUCCESS);
#else
twim_write(twi, (unsigned char *)&addr, package->addr_length, package->chip,0);
#endif
}
return twim_read(twi, package->buffer, package->length,package->chip, 0);
}
void sonar_i2cxl_get_last_measure(sonar_i2cxl_t* sonar_i2cxl)
{
uint8_t buf[2];
uint16_t distance_cm = 0;
float distance_m = 0.0f;
float velocity = 0.0f;
float dt = 0.0f;
uint32_t time_us = time_keeper_get_micros();
twim_read(&AVR32_TWIM1, buf, 2, sonar_i2cxl->i2c_address, false);
distance_cm = (buf[0] << 8) + buf[1];
distance_m = ((float)distance_cm) / 100.0f;
if ( distance_m > sonar_i2cxl->data.min_distance && distance_m < sonar_i2cxl->data.max_distance )
{
dt = ((float)time_us - sonar_i2cxl->data.last_update)/1000000.0f;
if (sonar_i2cxl->data.healthy)
{
velocity = (distance_m - sonar_i2cxl->data.current_distance) / dt;
//discard sonar velocity estimation if it seams too big
if (maths_f_abs(velocity) > 20.0f)
{
velocity = 0.0f;
}
if (sonar_i2cxl->data.healthy_vel)
{
sonar_i2cxl->data.current_velocity = (1.0f-LPF_SONAR_VARIO)*sonar_i2cxl->data.current_velocity + LPF_SONAR_VARIO*velocity;
}
else
{
sonar_i2cxl->data.current_velocity = velocity;
}
sonar_i2cxl->data.healthy_vel = true;
}
sonar_i2cxl->data.current_distance = distance_m;
sonar_i2cxl->data.last_update = time_us;
sonar_i2cxl->data.healthy = true;
}
else
{
sonar_i2cxl->data.current_velocity = 0.0f;
sonar_i2cxl->data.healthy = false;
sonar_i2cxl->data.healthy_vel = false;
}
}
void itg3200_init_slow(void)
{
static twim_options_t twi_opt=
{
.pba_hz = 64000000,
.speed = 400000,
.chip = ITG3200_SLAVE_ADDRESS,
.smbus = false
};
twim_master_init(&AVR32_TWIM0, &twi_opt);
twim_write(&AVR32_TWIM0, (uint8_t*)&default_configuration, 4, ITG3200_SLAVE_ADDRESS, false);
}
gyroscope_t* itg3200_get_data_slow(void)
{
uint8_t write_then_read_preamble=SENSOR_REG_ADDRESS;
twim_write(&AVR32_TWIM0, (uint8_t*) &write_then_read_preamble, 1, ITG3200_SLAVE_ADDRESS, false);
twim_read(&AVR32_TWIM0, (uint8_t*)&gyro_outputs, 8, ITG3200_SLAVE_ADDRESS, false);
return (gyroscope_t*)&gyro_outputs;
}
static void lsm330dlc_gyro_read_register(uint8_t* address, uint8_t* buffer, uint32_t nbytes)
{
twim_write(&AVR32_TWIM0, address, 1, LSM330_GYRO_SLAVE_ADDRESS, false);
twim_read(&AVR32_TWIM0, buffer, nbytes, LSM330_GYRO_SLAVE_ADDRESS, false);
}
