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(); }