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