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