int drotek_marg2_read(marg_data_t *data, drotek_marg2_t *marg2)
{
THROW_BEGIN();
THROW_ON_ERR(mpu6050_read(&marg2->mpu, &data->gyro, &data->acc, NULL));
THROW_ON_ERR(hmc5883_read_mag(&data->mag, &marg2->hmc));
THROW_END();
}
int drotek_marg2_init(drotek_marg2_t *marg2, i2c_bus_t *bus)
{
THROW_BEGIN();
THROW_ON_ERR(mpu6050_init(&marg2->mpu, bus, MPU6050_DLPF_CFG_94_98Hz, MPU6050_FS_SEL_500, MPU6050_AFS_SEL_4G));
THROW_ON_ERR(hmc5883_init(&marg2->hmc, bus));
THROW_END();
}
static int ms5611_read_adc(uint32_t *val, ms5611_t *ms5611)
{
THROW_BEGIN();
uint8_t raw[3]; /* 24-bit adc data */
THROW_ON_ERR(i2c_read_block_reg(&ms5611->i2c_dev, MS5611_ADC, raw, sizeof(raw)));
*val = raw[2] | (raw[1] << 8) | (raw[0] << 16);
THROW_END();
}
/* reads prom register into val and returns 0
* if the read failed, val remains untouched
* and a negative error code is returned */
static int ms5611_read_prom(ms5611_t *ms5611, uint8_t reg)
{
THROW_BEGIN();
uint8_t raw[2];
THROW_ON_ERR(i2c_read_block_reg(&ms5611->i2c_dev, MS5611_PROM_READ(reg), raw, sizeof(raw)));
ms5611->prom[reg] = raw[1] | (raw[0] << 8);
THROW_END();
}
int drotek_marg_read(marg_data_t *data, drotek_marg_t *marg)
{
THROW_BEGIN();
THROW_ON_ERR(itg3200_read_gyro(data->gyro.ve, &marg->itg));
THROW_ON_ERR(bma180_read_acc(data->acc.ve, &marg->bma));
THROW_ON_ERR(hmc5883_read_mag(&data->mag, &marg->hmc));
THROW_END();
}
int drotek_marg_init(drotek_marg_t *marg, i2c_bus_t *bus)
{
THROW_BEGIN();
THROW_ON_ERR(itg3200_init(&marg->itg, bus, ITG3200_DLPF_98HZ));
THROW_ON_ERR(bma180_init(&marg->bma, bus, BMA180_RANGE_4G, BMA180_BW_40HZ));
THROW_ON_ERR(hmc5883_init(&marg->hmc, bus));
THROW_END();
}
SIMPLE_THREAD_END
int scl_elevmap_init(void)
{
THROW_BEGIN();
scl_socket = scl_get_socket("elev");
THROW_IF(scl_socket == NULL, -ENODEV);
tsfloat_init(&elevation, 0.0f);
simple_thread_start(&thread, thread_func, "elevmap_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
SIMPLE_THREAD_END
int scl_power_init(void)
{
THROW_BEGIN();
scl_socket = scl_get_socket("power");
THROW_IF(scl_socket == NULL, -ENODEV);
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &mutexattr);
simple_thread_start(&thread, thread_func, "power_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
SIMPLE_THREAD_END
int scl_gps_init(void)
{
ASSERT_ONCE();
THROW_BEGIN();
scl_socket = scl_get_socket("gps");
THROW_IF(scl_socket == NULL, -ENODEV);
pthread_mutexattr_init(&mutexattr);
pthread_mutexattr_setprotocol(&mutexattr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &mutexattr);
interval_init(&interval);
simple_thread_start(&thread, thread_func, "gps_reader", THREAD_PRIORITY, NULL);
THROW_END();
}
int generic_platform_init(platform_t *plat)
{
ASSERT_ONCE();
THROW_BEGIN();
LOG(LL_INFO, "initializing power reader");
THROW_ON_ERR(scl_power_init());
plat->read_power = scl_power_read;
LOG(LL_INFO, "initializing remote control reader");
THROW_ON_ERR(scl_rc_init());
plat->read_rc = scl_rc_read;
LOG(LL_INFO, "initializing GPS reader");
THROW_ON_ERR(scl_gps_init());
plat->read_gps = scl_gps_read;
THROW_END();
}
