/* meant to run 3 times */ static bool gyro_init_sampling(void *data) { bool r; struct gyroscope_l3g4200d_data *mdata = data; static const uint8_t value = GYRO_REG_CTRL_REG1_DRBW_800_110 | GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE; if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 800Hz sampling with 110Hz filter */ r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, &value, 1); if (!r) { SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return false; } mdata->init_sampling_cnt--; if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, mdata->init_sampling_cnt ? gyro_init_sampling : gyro_init_range, mdata) < 0) { SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } return false; }
static bool lsm303_init(void *data) { struct magnetometer_lsm303_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { timer_sched(mdata, MAG_STEP_TIME, lsm303_init); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, mdata->slave)) { SOL_WRN("Failed to set slave at address 0x%02x\n", mdata->slave); return false; } mdata->i2c_buffer[0] = LSM303_MAG_DEFAULT_MODE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, LSM303_MAG_REG_MR_REG_M, mdata->i2c_buffer, 1, i2c_write_mode_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not enable LSM303 magnetometer"); return false; }
static bool gyro_init_range(void *data) { bool r; struct gyroscope_l3g4200d_data *mdata = data; static const uint8_t value = GYRO_REG_CTRL_REG4_FS_2000; if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 2000 degrees/sec */ r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, &value, 1); if (!r) { SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return false; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo, mdata) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); return false; }
static bool lsm303_accel_init(void *data) { struct accelerometer_lsm303_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { lsm303_timer_resched(mdata, ACCEL_STEP_TIME, lsm303_accel_init); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, mdata->slave)) { SOL_WRN("Failed to set slave at address 0x%02x\n", mdata->slave); return false; } mdata->i2c_buffer[0] = LSM303_ACCEL_DEFAULT_MODE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, LSM303_ACCEL_REG_CTRL_REG1_A, mdata->i2c_buffer, 1, lsm303_i2c_write_mode_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not enable LSM303 accelerometer"); return false; }
static void i2c_write_mode_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct magnetometer_lsm303_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Could not enable LSM303 magnetometer"); return; } if (!_get_range_bits_and_gain(mdata->scale, mdata->i2c_buffer, &mdata->gain_xy, &mdata->gain_z)) { SOL_WRN("Invalid gain. Expected one of 1.3, 1.9, 2.5, 4.0, 4.5, 5.6 or 8.1"); return; } mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, LSM303_MAG_REG_CRB_REG_M, mdata->i2c_buffer, 1, i2c_write_range_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not set LSM303 magnetometer range"); return; }
static int magnetometer_lsm303_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct magnetometer_lsm303_data *mdata = data; const struct sol_flow_node_type_magnetometer_lsm303_options *opts; static const uint8_t mode = LSM303_MAG_DEFAULT_MODE; uint8_t range_bit; SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_MAGNETOMETER_LSM303_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_magnetometer_lsm303_options *)options; if (!_get_range_bits_and_gain(opts->scale.val, &range_bit, &mdata->gain_xy, &mdata->gain_z)) { SOL_WRN("Invalid gain. Expected one of 1.3, 1.9, 2.5, 4.0, 4.5, 5.6 or 8.1"); return -EINVAL; } mdata->i2c = sol_i2c_open(opts->i2c_bus.val, SOL_I2C_SPEED_10KBIT); if (!mdata->i2c) { SOL_WRN("Failed to open i2c bus"); return -EINVAL; } if (!sol_i2c_set_slave_address(mdata->i2c, opts->i2c_slave.val)) { SOL_WRN("Failed to set slave at address 0x%02x\n", opts->i2c_slave.val); goto fail; } mdata->slave = opts->i2c_slave.val; if (!sol_i2c_write_register(mdata->i2c, LSM303_MAG_REG_MR_REG_M, &mode, 1)) { SOL_WRN("Could not enable LSM303 magnetometer"); goto fail; } if (!sol_i2c_write_register(mdata->i2c, LSM303_MAG_REG_CRB_REG_M, &range_bit, 1)) { SOL_WRN("Could not set LSM303 magnetometer range"); goto fail; } return 0; fail: sol_i2c_close(mdata->i2c); return -EINVAL; }
static bool gyro_init_fifo(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_fifo)) return false; mdata->common.buffer[0] = GYRO_REG_CTRL_REG5_FIFO_EN; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, mdata->common.buffer, 1, i2c_write_ctrl_reg5_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return false; }
static bool gyro_init_range(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_range)) return false; /* setup for 2000 degrees/sec */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG4_FS_2000; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, mdata->common.buffer, 1, i2c_write_ctrl_reg4_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return false; }
static bool gyro_init_stream(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_stream)) return false; /* enable FIFO in stream mode */ mdata->common.buffer[0] = GYRO_REG_FIFO_CTL_STREAM; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, mdata->common.buffer, 1, i2c_write_fifo_ctl_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return false; }
static bool lsm303_accel_init(void *data) { struct accelerometer_lsm303_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, lsm303_accel_init)) return false; mdata->i2c_buffer[0] = LSM303_ACCEL_DEFAULT_MODE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, LSM303_ACCEL_REG_CTRL_REG1_A, mdata->i2c_buffer, 1, lsm303_i2c_write_mode_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not enable LSM303 accelerometer"); return false; }
/* meant to run 3 times */ static bool gyro_init_sampling(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_sampling)) return false; /* setup for 800Hz sampling with 110Hz filter */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG1_DRBW_800_110 | GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, mdata->common.buffer, 1, i2c_write_ctrl_reg1_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return false; }
static void lsm303_scale_bit_set(struct accelerometer_lsm303_data *mdata) { int r; r = sol_i2c_set_slave_address(mdata->i2c, mdata->slave); if (r < 0) { SOL_WRN("Failed to set slave at address 0x%02x\n", mdata->slave); return; } switch (mdata->scale) { case 2: mdata->i2c_buffer[0] = 0x00; mdata->sensitivity = 1.0 / 1000; break; case 4: mdata->i2c_buffer[0] = 0x01; mdata->sensitivity = 2.0 / 1000; break; case 8: mdata->i2c_buffer[0] = 0x02; mdata->sensitivity = 4.0 / 1000; break; case 16: mdata->i2c_buffer[0] = 0x03; mdata->sensitivity = 12.0 / 1000; break; default: SOL_WRN("Invalid scale. Expected one of 2, 4, 8 or 16"); return; } mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, LSM303_ACCEL_REG_CTRL_REG4_A, mdata->i2c_buffer, 1, lsm303_i2c_write_scale_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not set scale to LSM303 accelerometer"); }
static bool gyro_init_fifo(void *data) { bool r; struct gyroscope_l3g4200d_data *mdata = data; static const uint8_t value = GYRO_REG_CTRL_REG5_FIFO_EN; if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, &value, 1); if (!r) { SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return false; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream, mdata) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); return false; }
static bool stts751_init(void *data) { struct stts751_data *mdata = data; static const uint8_t resolutions[] = { 0x2, 0x0, 0x1, 0x3 }; mdata->timer = NULL; if (!set_slave(mdata, stts751_init)) return false; /*TODO: It might be a good idea to use the stand-by mode and one-shot reading, * as it saves energy */ /*Resolution bits are form 9 to 12, their setup values are on resolutions * from 0 to 3, thus the '- 9' below */ mdata->i2c_buffer = resolutions[mdata->resolution - 9] << 2; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, CONFIGURATION_REGISTER, &mdata->i2c_buffer, 1, i2c_write_configuration_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not set STTS751 temperature reading resolution"); return false; }
static int gyro_tick_do(struct gyroscope_l3g4200d_data *mdata) { struct sol_direction_vector val = { .min = -GYRO_RANGE, .max = GYRO_RANGE, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; int r; gyro_read(mdata); r = sol_flow_send_direction_vector_packet (mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val); return r; } static bool gyro_ready(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; mdata->ready = true; while (mdata->pending_ticks) { gyro_tick_do(mdata); mdata->pending_ticks--; } SOL_DBG("gyro is ready for reading"); return false; } static bool gyro_init_stream(void *data) { bool r; struct gyroscope_l3g4200d_data *mdata = data; static const uint8_t value = GYRO_REG_FIFO_CTL_STREAM; if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* enable FIFO in stream mode */ r = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, &value, 1); if (!r) { SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return false; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready, mdata) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); return false; }
static void i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD : GYRO_SCALE_R_S; uint8_t num_samples_available; struct sol_direction_vector val = { .min = -GYRO_RANGE, .max = GYRO_RANGE, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } num_samples_available = status / (sizeof(int16_t) * 3); /* raw readings, with only the sensor-provided filtering */ for (uint8_t i = 0; i < num_samples_available; i++) { mdata->reading[0] = mdata->gyro_data.buffer[i][0] * scale; mdata->reading[1] = -mdata->gyro_data.buffer[i][1] * scale; mdata->reading[2] = -mdata->gyro_data.buffer[i][2] * scale; } sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_GYROSCOPE_L3G4200D__OUT__OUT, &val); mdata->pending_ticks--; if (mdata->pending_ticks) gyro_tick_do(mdata); } static void i2c_read_fifo_status_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; uint8_t num_samples_available; uint8_t fifo_status = mdata->common.buffer[0]; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read L3G4200D gyro fifo status"); return; } if (fifo_status & GYRO_REG_FIFO_SRC_OVERRUN) { num_samples_available = 32; } else if (fifo_status & GYRO_REG_FIFO_SRC_EMPTY) { num_samples_available = 0; } else { num_samples_available = fifo_status & GYRO_REG_FIFO_SRC_ENTRIES_MASK; } if (!num_samples_available) { SOL_INF("No samples available"); return; } SOL_DBG("%d samples available", num_samples_available); /* Read *all* the entries in one go, using AUTO_INCREMENT */ mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT, (uint8_t *)&mdata->gyro_data.buffer[0][0], sizeof(mdata->gyro_data.buffer), i2c_read_data_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro samples"); } static bool gyro_tick_do(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_tick_do); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = 0; mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_FIFO_SRC, mdata->common.buffer, 1, i2c_read_fifo_status_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read L3G4200D gyro fifo status"); return false; } static bool gyro_ready(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->ready = true; SOL_DBG("gyro is ready for reading"); if (mdata->pending_ticks) gyro_tick_do(mdata); return false; } static void i2c_write_fifo_ctl_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_ready) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_stream(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* enable FIFO in stream mode */ mdata->common.buffer[0] = GYRO_REG_FIFO_CTL_STREAM; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_FIFO_CTL, mdata->common.buffer, 1, i2c_write_fifo_ctl_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's stream mode"); return false; } static void i2c_write_ctrl_reg5_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_stream) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_fifo(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->common.buffer[0] = GYRO_REG_CTRL_REG5_FIFO_EN; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, mdata->common.buffer, 1, i2c_write_ctrl_reg5_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's fifo mode"); return false; } static void i2c_write_ctrl_reg4_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return; } if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_fifo) < 0) SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } static bool gyro_init_range(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_range); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 2000 degrees/sec */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG4_FS_2000; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, mdata->common.buffer, 1, i2c_write_ctrl_reg4_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's resolution"); return false; } static bool gyro_init_sampling(void *data); static void i2c_write_ctrl_reg1_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return; } mdata->init_sampling_cnt--; if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, mdata->init_sampling_cnt ? gyro_init_sampling : gyro_init_range) < 0) { SOL_WRN("error in scheduling a L3G4200D gyro's init command"); } } /* meant to run 3 times */ static bool gyro_init_sampling(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } /* setup for 800Hz sampling with 110Hz filter */ mdata->common.buffer[0] = GYRO_REG_CTRL_REG1_DRBW_800_110 | GYRO_REG_CTRL_REG1_PD | GYRO_REG_CTRL_REG1_XYZ_ENABLE; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, mdata->common.buffer, 1, i2c_write_ctrl_reg1_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("could not set L3G4200D gyro sensor's sampling rate"); return false; } static void i2c_read_who_am_i_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct gyroscope_l3g4200d_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Failed to read i2c register"); return; } if (mdata->common.buffer[0] != GYRO_REG_WHO_AM_I_VALUE) { SOL_WRN("could not find L3G4200D gyro sensor"); return; } gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init_sampling); } static bool gyro_init(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, gyro_init); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) { SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS); return false; } mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, GYRO_REG_WHO_AM_I, mdata->common.buffer, 1, i2c_read_who_am_i_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read i2c register"); return false; } static int gyroscope_l3g4200d_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct gyroscope_l3g4200d_data *mdata = data; const struct sol_flow_node_type_gyroscope_l3g4200d_options *opts = (const struct sol_flow_node_type_gyroscope_l3g4200d_options *)options; SOL_NULL_CHECK(options, -EINVAL); mdata->i2c = sol_i2c_open(opts->i2c_bus, I2C_SPEED); SOL_NULL_CHECK_MSG(mdata->i2c, -EIO, "Failed to open i2c bus"); mdata->use_rad = opts->output_radians; mdata->init_sampling_cnt = 3; mdata->node = node; gyro_init(mdata); return 0; } static void gyroscope_l3g4200d_close(struct sol_flow_node *node, void *data) { struct gyroscope_l3g4200d_data *mdata = data; if (mdata->i2c_pending) sol_i2c_pending_cancel(mdata->i2c, mdata->i2c_pending); if (mdata->i2c) sol_i2c_close(mdata->i2c); if (mdata->timer) sol_timeout_del(mdata->timer); } static int gyroscope_l3g4200d_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct gyroscope_l3g4200d_data *mdata = data; if (!mdata->ready || mdata->pending_ticks) { mdata->pending_ticks++; return 0; } gyro_tick_do(mdata); return 0; }