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 bool set_slave(struct accelerometer_lsm303_data *mdata, bool (*cb)(void *data)) { int r; r = sol_i2c_set_slave_address(mdata->i2c, mdata->slave); if (r < 0) { if (r == -EBUSY) lsm303_timer_resched(mdata, ACCEL_STEP_TIME, cb); else { const char errmsg[] = "Failed to set slave at address 0x%02x"; SOL_WRN(errmsg, mdata->slave); sol_flow_send_error_packet(mdata->node, r, errmsg, mdata->slave); } return false; } return true; }
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; }