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