static void direction_vector_receiver(void *data, uint32_t id, struct sol_blob *message) { struct sol_flow_node *node = data; sol_flow_send_direction_vector_packet(node, SOL_FLOW_NODE_TYPE_IPM_DIRECTION_VECTOR_READER__OUT__OUT, message->mem); sol_blob_unref(message); }
/* *----------------------- direction_vector -------------------------- */ static void direction_vector_read_data(void *data, int fd) { struct unix_socket_data *mdata = data; struct sol_direction_vector direction_vector; if (FILL_BUFFER(fd, direction_vector) < 0) return; sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_UNIX_SOCKET_DIRECTION_VECTOR_READER__OUT__OUT, &direction_vector); }
static void _send_result(struct sol_flow_node *node, struct compass_data *mdata) { _calculate_result(mdata); sol_flow_send_direction_vector_packet(node, SOL_FLOW_NODE_TYPE_COMPASS_ACCELEROMETER_MAGNETOMETER__OUT__VECTOR, &mdata->result); sol_flow_send_drange_value_packet(node, SOL_FLOW_NODE_TYPE_COMPASS_ACCELEROMETER_MAGNETOMETER__OUT__HEADING, mdata->heading); mdata->has_accel = false; mdata->has_mag = false; }
static int constant_direction_vector_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { const struct sol_flow_node_type_constant_direction_vector_options *opts; SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_CONSTANT_DIRECTION_VECTOR_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_constant_direction_vector_options *)options; return sol_flow_send_direction_vector_packet(node, SOL_FLOW_NODE_TYPE_CONSTANT_DIRECTION_VECTOR__OUT__OUT, &opts->value); return 0; }
static void _lsm303_send_output_packets(struct sol_flow_node *node, struct magnetometer_lsm303_data *mdata) { struct sol_direction_vector val = { .min = -mdata->scale, .max = -mdata->scale, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; sol_flow_send_direction_vector_packet(node, SOL_FLOW_NODE_TYPE_MAGNETOMETER_LSM303__OUT__OUT, &val); } static int magnetometer_lsm303_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct magnetometer_lsm303_data *mdata = data; int8_t buffer[LSM303_MAG_BYTES_NUMBER]; int r; if (!sol_i2c_set_slave_address(mdata->i2c, mdata->slave)) { const char errmsg[] = "Failed to set slave at address 0x%02x"; SOL_WRN(errmsg, mdata->slave); sol_flow_send_error_packet(node, EIO, errmsg, mdata->slave); return -EIO; } r = sol_i2c_read_register(mdata->i2c, LSM303_ACCEL_REG_OUT_X_H_M, (uint8_t *)buffer, sizeof(buffer)); if (r <= 0) { const char errmsg[] = "Failed to read LSM303 magnetometer samples"; SOL_WRN(errmsg); sol_flow_send_error_packet(node, EIO, errmsg); return -EIO; } /* Get X, Z and Y. That's why reading[] is indexed 0, 2 and 1. */ mdata->reading[0] = ((buffer[0] << 8) | buffer[1]) / mdata->gain_xy; mdata->reading[2] = ((buffer[2] << 8) | buffer[3]) / mdata->gain_z; mdata->reading[1] = ((buffer[4] << 8) | buffer[5]) / mdata->gain_xy; _lsm303_send_output_packets(node, mdata); return 0; }
static void iio_direction_vector_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_direction_vector_data *mdata = sol_flow_node_get_private_data(node); struct sol_direction_vector out = { .min = mdata->iio_base.out_range.min, .max = mdata->iio_base.out_range.max }; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_x, &out.x); SOL_INT_CHECK_GOTO(r, < 0, error); r = sol_iio_read_channel_value(mdata->channel_y, &out.y); SOL_INT_CHECK_GOTO(r, < 0, error); r = sol_iio_read_channel_value(mdata->channel_z, &out.z); SOL_INT_CHECK_GOTO(r, < 0, error); sol_flow_send_direction_vector_packet(node, type->out_port, &out); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static void iio_double_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_double_data *mdata = sol_flow_node_get_private_data(node); struct sol_drange out = { .min = mdata->iio_base.out_range.min, .max = mdata->iio_base.out_range.max, .step = mdata->iio_base.out_range.step }; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_val, &out.val); SOL_INT_CHECK_GOTO(r, < 0, error); sol_flow_send_drange_value_packet(node, type->out_port, out.val); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static void iio_color_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_color_data *mdata = sol_flow_node_get_private_data(node); struct sol_rgb out = { .red_max = mdata->iio_base.out_range.max, .green_max = mdata->iio_base.out_range.max, .blue_max = mdata->iio_base.out_range.max }; double tmp; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_red, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.red = tmp; r = sol_iio_read_channel_value(mdata->channel_green, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.green = tmp; r = sol_iio_read_channel_value(mdata->channel_blue, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.blue = tmp; sol_flow_send_rgb_packet(node, type->out_port, &out); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static bool gyroscope_create_channels(struct iio_direction_vector_data *mdata, int device_id) { struct sol_iio_channel_config channel_config = SOL_IIO_CHANNEL_CONFIG_INIT; mdata->iio_base.device = sol_iio_open(device_id, &mdata->iio_base.config); SOL_NULL_CHECK(mdata->iio_base.device, false); #define ADD_CHANNEL(_axis) \ if (!mdata->iio_base.use_device_default_scale) \ channel_config.scale = mdata->scale._axis; \ if (!mdata->iio_base.use_device_default_offset) \ channel_config.offset = mdata->offset._axis; \ mdata->channel_ ## _axis = sol_iio_add_channel(mdata->iio_base.device, "in_anglvel_" # _axis, &channel_config); \ SOL_NULL_CHECK_GOTO(mdata->channel_ ## _axis, error); ADD_CHANNEL(x); ADD_CHANNEL(y); ADD_CHANNEL(z); #undef ADD_CHANNEL sol_iio_device_start_buffer(mdata->iio_base.device); return true; error: SOL_WRN("Could not create iio/gyroscope node. Failed to open IIO device %d", device_id); sol_iio_close(mdata->iio_base.device); return false; } static int gyroscope_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct iio_direction_vector_data *mdata = data; const struct sol_flow_node_type_iio_gyroscope_options *opts; int device_id; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_IIO_GYROSCOPE_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_iio_gyroscope_options *)options; mdata->iio_base.buffer_enabled = opts->buffer_size > -1; SOL_SET_API_VERSION(mdata->iio_base.config.api_version = SOL_IIO_CONFIG_API_VERSION; ) if (opts->iio_trigger_name) { mdata->iio_base.config.trigger_name = strdup(opts->iio_trigger_name); SOL_NULL_CHECK(mdata->iio_base.config.trigger_name, -ENOMEM); } mdata->iio_base.config.buffer_size = opts->buffer_size; mdata->iio_base.config.sampling_frequency = opts->sampling_frequency; if (mdata->iio_base.buffer_enabled) { mdata->iio_base.config.sol_iio_reader_cb = type->reader_cb; mdata->iio_base.config.data = node; } mdata->iio_base.use_device_default_scale = opts->use_device_default_scale; mdata->iio_base.use_device_default_offset = opts->use_device_default_offset; mdata->scale = opts->scale; mdata->offset = opts->offset; mdata->iio_base.out_range = opts->out_range; device_id = sol_iio_address_device(opts->iio_device); if (device_id < 0) { SOL_WRN("Could not create iio/gyroscope node. Failed to open IIO device %s", opts->iio_device); goto err; } if (!gyroscope_create_channels(mdata, device_id)) goto err; return 0; err: free((char *)mdata->iio_base.config.trigger_name); return -EINVAL; } static bool magnet_create_channels(struct iio_direction_vector_data *mdata, int device_id) { struct sol_iio_channel_config channel_config = SOL_IIO_CHANNEL_CONFIG_INIT; mdata->iio_base.device = sol_iio_open(device_id, &mdata->iio_base.config); SOL_NULL_CHECK(mdata->iio_base.device, false); #define ADD_CHANNEL(_axis) \ if (!mdata->iio_base.use_device_default_scale) \ channel_config.scale = mdata->scale._axis; \ if (!mdata->iio_base.use_device_default_offset) \ channel_config.offset = mdata->offset._axis; \ mdata->channel_ ## _axis = sol_iio_add_channel(mdata->iio_base.device, "in_magn_" # _axis, &channel_config); \ SOL_NULL_CHECK_GOTO(mdata->channel_ ## _axis, error); ADD_CHANNEL(x); ADD_CHANNEL(y); ADD_CHANNEL(z); #undef ADD_CHANNEL sol_iio_device_start_buffer(mdata->iio_base.device); return true; error: SOL_WRN("Could not create iio/magnet node. Failed to open IIO device %d", device_id); sol_iio_close(mdata->iio_base.device); return false; } static int magnet_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct iio_direction_vector_data *mdata = data; const struct sol_flow_node_type_iio_magnetometer_options *opts; int device_id; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_IIO_MAGNETOMETER_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_iio_magnetometer_options *)options; mdata->iio_base.buffer_enabled = opts->buffer_size > -1; SOL_SET_API_VERSION(mdata->iio_base.config.api_version = SOL_IIO_CONFIG_API_VERSION; ) if (opts->iio_trigger_name) { mdata->iio_base.config.trigger_name = strdup(opts->iio_trigger_name); SOL_NULL_CHECK(mdata->iio_base.config.trigger_name, -ENOMEM); } mdata->iio_base.config.buffer_size = opts->buffer_size; mdata->iio_base.config.sampling_frequency = opts->sampling_frequency; if (mdata->iio_base.buffer_enabled) { mdata->iio_base.config.sol_iio_reader_cb = type->reader_cb; mdata->iio_base.config.data = node; } mdata->iio_base.use_device_default_scale = opts->use_device_default_scale; mdata->iio_base.use_device_default_offset = opts->use_device_default_offset; mdata->scale = opts->scale; mdata->offset = opts->offset; mdata->iio_base.out_range = opts->out_range; device_id = sol_iio_address_device(opts->iio_device); if (device_id < 0) { SOL_WRN("Could not create iio/magnet node. Failed to open IIO device %s", opts->iio_device); goto err; } if (!magnet_create_channels(mdata, device_id)) goto err; return 0; err: free((char *)mdata->iio_base.config.trigger_name); return -EINVAL; } static bool temp_create_channels(struct iio_double_data *mdata, int device_id) { struct sol_iio_channel_config channel_config = SOL_IIO_CHANNEL_CONFIG_INIT; mdata->iio_base.device = sol_iio_open(device_id, &mdata->iio_base.config); SOL_NULL_CHECK(mdata->iio_base.device, false); #define ADD_CHANNEL(_val) \ if (!mdata->iio_base.use_device_default_scale) \ channel_config.scale = mdata->scale; \ if (!mdata->iio_base.use_device_default_offset) \ channel_config.offset = mdata->offset; \ mdata->channel_ ## _val = sol_iio_add_channel(mdata->iio_base.device, "in_temp", &channel_config); \ SOL_NULL_CHECK_GOTO(mdata->channel_ ## _val, error); ADD_CHANNEL(val); #undef ADD_CHANNEL sol_iio_device_start_buffer(mdata->iio_base.device); return true; error: SOL_WRN("Could not create iio/thermometer node. Failed to open" " IIO device %d", device_id); sol_iio_close(mdata->iio_base.device); return false; } static int temperature_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct iio_double_data *mdata = data; const struct sol_flow_node_type_iio_thermometer_options *opts; int device_id; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_IIO_THERMOMETER_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_iio_thermometer_options *)options; mdata->iio_base.buffer_enabled = opts->buffer_size > -1; SOL_SET_API_VERSION(mdata->iio_base.config.api_version = SOL_IIO_CONFIG_API_VERSION; ) if (opts->iio_trigger_name) { mdata->iio_base.config.trigger_name = strdup(opts->iio_trigger_name); SOL_NULL_CHECK(mdata->iio_base.config.trigger_name, -ENOMEM); } mdata->iio_base.config.buffer_size = opts->buffer_size; mdata->iio_base.config.sampling_frequency = opts->sampling_frequency; if (mdata->iio_base.buffer_enabled) { mdata->iio_base.config.sol_iio_reader_cb = type->reader_cb; mdata->iio_base.config.data = node; } mdata->iio_base.use_device_default_scale = opts->use_device_default_scale; mdata->iio_base.use_device_default_offset = opts->use_device_default_offset; mdata->scale = opts->scale; mdata->offset = opts->offset; mdata->iio_base.out_range = opts->out_range; device_id = sol_iio_address_device(opts->iio_device); if (device_id < 0) { SOL_WRN("Could not create iio/thermometer node. Failed to" " open IIO device %s", opts->iio_device); goto err; } if (!temp_create_channels(mdata, device_id)) goto err; return 0; err: free((char *)mdata->iio_base.config.trigger_name); return -EINVAL; }
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; }
static void _lsm303_send_output_packets(struct accelerometer_lsm303_data *mdata) { struct sol_direction_vector val = { .min = -mdata->scale, .max = mdata->scale, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_ACCELEROMETER_LSM303__OUT__RAW, &val); val.x = val.x * GRAVITY_MSS; val.y = val.y * GRAVITY_MSS; val.z = val.z * GRAVITY_MSS; sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_ACCELEROMETER_LSM303__OUT__OUT, &val); mdata->pending_ticks--; if (mdata->pending_ticks) lsm303_read_data(mdata); } static void i2c_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *buffer, ssize_t status) { struct accelerometer_lsm303_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { SOL_WRN("Could not enable LSM303 accelerometer"); return; } /* http://stackoverflow.com/a/19164062 says that it's necessary to >> 4 buffer result. * https://github.com/adafruit/Adafruit_LSM303/blob/master/Adafruit_LSM303.cpp does the shift * Doing it here, but it's interesting to check it. Datasheet says nothing about it, though. */ mdata->reading[0] = ((buffer[0] | (buffer[1] << 8)) >> 4) * mdata->sensitivity; mdata->reading[1] = ((buffer[2] | (buffer[3] << 8)) >> 4) * mdata->sensitivity; mdata->reading[2] = ((buffer[4] | (buffer[5] << 8)) >> 4) * mdata->sensitivity; _lsm303_send_output_packets(mdata); } static bool lsm303_read_data(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_read_data); 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; } /* ORing with 0x80 to read all bytes in a row */ mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, LSM303_ACCEL_REG_OUT_X_H_A | 0x80, mdata->i2c_buffer, sizeof(mdata->i2c_buffer), i2c_read_data_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Failed to read LSM303 accel samples"); return false; } static int accelerometer_lsm303_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct accelerometer_lsm303_data *mdata = data; if (!mdata->ready || mdata->pending_ticks) { mdata->pending_ticks++; return 0; } lsm303_read_data(mdata); return 0; }
static void iio_direction_vector_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_direction_vector_data *mdata = sol_flow_node_get_private_data(node); struct sol_direction_vector out = { .min = mdata->iio_base.out_range.min, .max = mdata->iio_base.out_range.max }; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_x, &out.x); SOL_INT_CHECK_GOTO(r, < 0, error); r = sol_iio_read_channel_value(mdata->channel_y, &out.y); SOL_INT_CHECK_GOTO(r, < 0, error); r = sol_iio_read_channel_value(mdata->channel_z, &out.z); SOL_INT_CHECK_GOTO(r, < 0, error); SOL_DBG("Before mount_calibration: %f-%f-%f", out.x, out.y, out.z); // mount correction sol_iio_mount_calibration(device, &out); sol_flow_send_direction_vector_packet(node, type->out_port, &out); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static void iio_double_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_double_data *mdata = sol_flow_node_get_private_data(node); struct sol_drange out = { .min = mdata->iio_base.out_range.min, .max = mdata->iio_base.out_range.max, .step = mdata->iio_base.out_range.step }; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_val, &out.val); SOL_INT_CHECK_GOTO(r, < 0, error); sol_flow_send_drange_value_packet(node, type->out_port, out.val); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static void iio_color_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct iio_color_data *mdata = sol_flow_node_get_private_data(node); struct sol_rgb out = { .red_max = mdata->iio_base.out_range.max, .green_max = mdata->iio_base.out_range.max, .blue_max = mdata->iio_base.out_range.max }; double tmp; int r; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); r = sol_iio_read_channel_value(mdata->channel_red, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.red = tmp; r = sol_iio_read_channel_value(mdata->channel_green, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.green = tmp; r = sol_iio_read_channel_value(mdata->channel_blue, &tmp); if (r < 0 || tmp < 0 || tmp > UINT32_MAX) goto error; out.blue = tmp; sol_flow_send_rgb_packet(node, type->out_port, &out); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static bool gyroscope_create_channels(struct iio_direction_vector_data *mdata, int device_id) { mdata->iio_base.device = sol_iio_open(device_id, &mdata->iio_base.config); SOL_NULL_CHECK(mdata->iio_base.device, false); mdata->channel_x = iio_add_channel(mdata->scale.x, mdata->offset.x, "in_anglvel_x", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_x, error); mdata->channel_y = iio_add_channel(mdata->scale.y, mdata->offset.y, "in_anglvel_y", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_y, error); mdata->channel_z = iio_add_channel(mdata->scale.z, mdata->offset.z, "in_anglvel_z", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_z, error); sol_iio_device_start_buffer(mdata->iio_base.device); return true; error: SOL_WRN("Could not create iio/gyroscope node. Failed to open IIO device %d", device_id); sol_iio_close(mdata->iio_base.device); return false; } static int gyroscope_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct iio_direction_vector_data *mdata = data; const struct sol_flow_node_type_iio_gyroscope_options *opts; int device_id, ret; struct iio_node_type *type; type = (struct iio_node_type *)sol_flow_node_get_type(node); SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_IIO_GYROSCOPE_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_iio_gyroscope_options *)options; mdata->iio_base.buffer_enabled = opts->buffer_size > -1; SOL_SET_API_VERSION(mdata->iio_base.config.api_version = SOL_IIO_CONFIG_API_VERSION; ) if (opts->iio_trigger_name) { mdata->iio_base.config.trigger_name = strdup(opts->iio_trigger_name); SOL_NULL_CHECK(mdata->iio_base.config.trigger_name, -ENOMEM); } mdata->iio_base.config.buffer_size = opts->buffer_size; mdata->iio_base.config.sampling_frequency = opts->sampling_frequency; ret = snprintf(mdata->iio_base.config.sampling_frequency_name, sizeof(mdata->iio_base.config.sampling_frequency_name), "%s", "in_anglvel_"); SOL_INT_CHECK_GOTO(ret, >= (int)sizeof(mdata->iio_base.config.sampling_frequency_name), err); SOL_INT_CHECK_GOTO(ret, < 0, err); if (mdata->iio_base.buffer_enabled) { mdata->iio_base.config.sol_iio_reader_cb = type->reader_cb; mdata->iio_base.config.data = node; } mdata->iio_base.use_device_default_scale = opts->use_device_default_scale; mdata->iio_base.use_device_default_offset = opts->use_device_default_offset; mdata->scale = opts->scale; mdata->offset = opts->offset; mdata->iio_base.out_range = opts->out_range; device_id = sol_iio_address_device(opts->iio_device); if (device_id < 0) { SOL_WRN("Could not create iio/gyroscope node. Failed to open IIO device %s", opts->iio_device); goto err; } if (!gyroscope_create_channels(mdata, device_id)) goto err; return 0; err: free((char *)mdata->iio_base.config.trigger_name); return -EINVAL; } static bool magnet_create_channels(struct iio_direction_vector_data *mdata, int device_id) { mdata->iio_base.device = sol_iio_open(device_id, &mdata->iio_base.config); SOL_NULL_CHECK(mdata->iio_base.device, false); mdata->channel_x = iio_add_channel(mdata->scale.x, mdata->offset.x, "in_magn_x", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_x, error); mdata->channel_y = iio_add_channel(mdata->scale.y, mdata->offset.y, "in_magn_y", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_y, error); mdata->channel_z = iio_add_channel(mdata->scale.z, mdata->offset.z, "in_magn_z", &mdata->iio_base); SOL_NULL_CHECK_GOTO(mdata->channel_z, error); sol_iio_device_start_buffer(mdata->iio_base.device); return true; error: SOL_WRN("Could not create iio/magnet node. Failed to open IIO device %d", device_id); sol_iio_close(mdata->iio_base.device); return false; }
static void _lsm303_send_output_packets(struct magnetometer_lsm303_data *mdata) { struct sol_direction_vector val = { .min = -mdata->scale, .max = mdata->scale, .x = mdata->reading[0], .y = mdata->reading[1], .z = mdata->reading[2] }; sol_flow_send_direction_vector_packet(mdata->node, SOL_FLOW_NODE_TYPE_MAGNETOMETER_LSM303__OUT__OUT, &val); } static void i2c_lsm303_read_data_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct magnetometer_lsm303_data *mdata = cb_data; uint8_t *buffer = mdata->i2c_buffer; mdata->i2c_pending = NULL; if (status < 0) { const char errmsg[] = "Failed to read LSM303 magnetometer samples"; SOL_WRN(errmsg); sol_flow_send_error_packet(mdata->node, EIO, errmsg); return; } /* Get X, Z and Y. That's why reading[] is indexed 0, 2 and 1. */ mdata->reading[0] = ((buffer[0] << 8) | buffer[1]) / mdata->gain_xy; mdata->reading[2] = ((buffer[2] << 8) | buffer[3]) / mdata->gain_z; mdata->reading[1] = ((buffer[4] << 8) | buffer[5]) / mdata->gain_xy; _lsm303_send_output_packets(mdata); mdata->pending_ticks--; if (mdata->pending_ticks) magnetometer_lsm303_tick_do(mdata); } static bool magnetometer_lsm303_tick_do(void *data) { struct magnetometer_lsm303_data *mdata = data; mdata->timer = NULL; if (sol_i2c_busy(mdata->i2c)) { timer_sched(mdata, MAG_STEP_TIME, magnetometer_lsm303_tick_do); return false; } if (!sol_i2c_set_slave_address(mdata->i2c, mdata->slave)) { const char errmsg[] = "Failed to set slave at address 0x%02x"; SOL_WRN(errmsg, mdata->slave); sol_flow_send_error_packet(mdata->node, EIO, errmsg, mdata->slave); return false; } mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, LSM303_ACCEL_REG_OUT_X_H_M, mdata->i2c_buffer, sizeof(mdata->i2c_buffer), i2c_lsm303_read_data_cb, mdata); if (!mdata->i2c_pending) { const char errmsg[] = "Failed to read LSM303 magnetometer samples"; SOL_WRN(errmsg); sol_flow_send_error_packet(mdata->node, EIO, errmsg); } return false; } static int magnetometer_lsm303_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct magnetometer_lsm303_data *mdata = data; if (!mdata->ready || mdata->pending_ticks) { mdata->pending_ticks++; return 0; } magnetometer_lsm303_tick_do(mdata); return 0; }
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 set_slave(struct gyroscope_l3g4200d_data *mdata, bool (*cb)(void *data)) { int r; r = sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS); if (r < 0) { if (r == -EBUSY) gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME, cb); else { const char errmsg[] = "Failed to set slave at address 0x%02x"; SOL_WRN(errmsg, GYRO_ADDRESS); sol_flow_send_error_packet(mdata->node, r, errmsg, GYRO_ADDRESS); } return false; } return true; }
static void accelerate_reader_cb(void *data, struct sol_iio_device *device) { static const char *errmsg = "Could not read channel buffer values"; struct sol_flow_node *node = data; struct accelerate_data *mdata = sol_flow_node_get_private_data(node); struct sol_direction_vector out = { .min = mdata->out_range.min, .max = mdata->out_range.max }; bool b; b = sol_iio_read_channel_value(mdata->channel_x, &out.x); if (!b) goto error; b = sol_iio_read_channel_value(mdata->channel_y, &out.y); if (!b) goto error; b = sol_iio_read_channel_value(mdata->channel_z, &out.z); if (!b) goto error; sol_flow_send_direction_vector_packet(node, SOL_FLOW_NODE_TYPE_IIO_ACCELEROMETER__OUT__OUT, &out); return; error: sol_flow_send_error_packet_str(node, EIO, errmsg); SOL_WRN("%s", errmsg); } static bool accelerate_create_channels(struct accelerate_data *mdata, int device_id) { struct sol_iio_channel_config channel_config = SOL_IIO_CHANNEL_CONFIG_INIT; mdata->device = sol_iio_open(device_id, &mdata->config); SOL_NULL_CHECK(mdata->device, false); #define ADD_CHANNEL(_axis) \ if (!mdata->use_device_default_scale) \ channel_config.scale = mdata->scale._axis; \ if (!mdata->use_device_default_offset) \ channel_config.offset = mdata->offset._axis; \ mdata->channel_ ## _axis = sol_iio_add_channel(mdata->device, "in_accel_" # _axis, &channel_config); \ SOL_NULL_CHECK_GOTO(mdata->channel_ ## _axis, error); ADD_CHANNEL(x); ADD_CHANNEL(y); ADD_CHANNEL(z); #undef ADD_CHANNEL sol_iio_device_start_buffer(mdata->device); return true; error: SOL_WRN("Could not create iio/accelerate node. Failed to open IIO device %d", device_id); sol_iio_close(mdata->device); return false; } static int accelerate_open(struct sol_flow_node *node, void *data, const struct sol_flow_node_options *options) { struct accelerate_data *mdata = data; const struct sol_flow_node_type_iio_accelerometer_options *opts; int device_id; SOL_FLOW_NODE_OPTIONS_SUB_API_CHECK(options, SOL_FLOW_NODE_TYPE_IIO_ACCELEROMETER_OPTIONS_API_VERSION, -EINVAL); opts = (const struct sol_flow_node_type_iio_accelerometer_options *)options; mdata->buffer_enabled = opts->buffer_size > -1; SOL_SET_API_VERSION(mdata->config.api_version = SOL_IIO_CONFIG_API_VERSION; ) if (opts->iio_trigger_name) { mdata->config.trigger_name = strdup(opts->iio_trigger_name); SOL_NULL_CHECK(mdata->config.trigger_name, -ENOMEM); } mdata->config.buffer_size = opts->buffer_size; mdata->config.sampling_frequency = opts->sampling_frequency; if (mdata->buffer_enabled) { mdata->config.sol_iio_reader_cb = accelerate_reader_cb; mdata->config.data = node; } mdata->use_device_default_scale = opts->use_device_default_scale; mdata->use_device_default_offset = opts->use_device_default_offset; mdata->scale = opts->scale; mdata->offset = opts->offset; mdata->out_range = opts->out_range; device_id = sol_iio_address_device(opts->iio_device); if (device_id < 0) { SOL_WRN("Could not create iio/accelerate node. Failed to open IIO device %s", opts->iio_device); goto err; } if (!accelerate_create_channels(mdata, device_id)) goto err; return 0; err: free((char *)mdata->config.trigger_name); return -EINVAL; }