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.val, I2C_SPEED);
if (!mdata->i2c) {
SOL_WRN("Failed to open i2c bus");
return -EBADR;
}
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n",
GYRO_ADDRESS);
return -EIO;
}
mdata->use_rad = opts->output_radians;
mdata->init_sampling_cnt = 3;
mdata->ready = false;
mdata->node = node;
return gyro_init(mdata);
}
static bool
gyro_init_range(void *data)
{
bool r;
struct gyroscope_l3g4200d_data *mdata = data;
static const uint8_t value = GYRO_REG_CTRL_REG4_FS_2000;
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 */
r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG4, &value, 1);
if (!r) {
SOL_WRN("could not set L3G4200D gyro sensor's resolution");
return false;
}
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
gyro_init_fifo, mdata) < 0)
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
return false;
}
/* meant to run 3 times */
static bool
gyro_init_sampling(void *data)
{
bool r;
struct gyroscope_l3g4200d_data *mdata = data;
static const uint8_t value =
GYRO_REG_CTRL_REG1_DRBW_800_110 |
GYRO_REG_CTRL_REG1_PD |
GYRO_REG_CTRL_REG1_XYZ_ENABLE;
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 */
r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG1, &value, 1);
if (!r) {
SOL_WRN("could not set L3G4200D gyro sensor's sampling rate");
return false;
}
mdata->init_sampling_cnt--;
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
mdata->init_sampling_cnt ?
gyro_init_sampling : gyro_init_range,
mdata) < 0) {
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
}
return false;
}
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
lsm303_init(void *data)
{
struct magnetometer_lsm303_data *mdata = data;
mdata->timer = NULL;
if (sol_i2c_busy(mdata->i2c)) {
timer_sched(mdata, MAG_STEP_TIME, lsm303_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_MAG_DEFAULT_MODE;
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c,
LSM303_MAG_REG_MR_REG_M, mdata->i2c_buffer, 1, i2c_write_mode_cb,
mdata);
if (!mdata->i2c_pending)
SOL_WRN("Could not enable LSM303 magnetometer");
return false;
}
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 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;
}
static bool
set_slave(struct stts751_data *mdata, bool (*cb)(void *data))
{
int r;
r = sol_i2c_set_slave_address(mdata->i2c, mdata->slave);
if (r < 0) {
if (r == -EBUSY)
timer_sched(mdata, 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_scale_bit_set(struct accelerometer_lsm303_data *mdata)
{
int r;
r = sol_i2c_set_slave_address(mdata->i2c, mdata->slave);
if (r < 0) {
SOL_WRN("Failed to set slave at address 0x%02x\n", mdata->slave);
return;
}
switch (mdata->scale) {
case 2:
mdata->i2c_buffer[0] = 0x00;
mdata->sensitivity = 1.0 / 1000;
break;
case 4:
mdata->i2c_buffer[0] = 0x01;
mdata->sensitivity = 2.0 / 1000;
break;
case 8:
mdata->i2c_buffer[0] = 0x02;
mdata->sensitivity = 4.0 / 1000;
break;
case 16:
mdata->i2c_buffer[0] = 0x03;
mdata->sensitivity = 12.0 / 1000;
break;
default:
SOL_WRN("Invalid scale. Expected one of 2, 4, 8 or 16");
return;
}
mdata->i2c_pending = sol_i2c_write_register(mdata->i2c,
LSM303_ACCEL_REG_CTRL_REG4_A, mdata->i2c_buffer, 1,
lsm303_i2c_write_scale_cb, mdata);
if (!mdata->i2c_pending)
SOL_WRN("Could not set scale to LSM303 accelerometer");
}
static bool
gyro_init_fifo(void *data)
{
bool r;
struct gyroscope_l3g4200d_data *mdata = data;
static const uint8_t value = GYRO_REG_CTRL_REG5_FIFO_EN;
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;
}
r = sol_i2c_write_register(mdata->i2c, GYRO_REG_CTRL_REG5, &value, 1);
if (!r) {
SOL_WRN("could not set L3G4200D gyro sensor's fifo mode");
return false;
}
if (gyro_timer_resched(mdata, GYRO_INIT_STEP_TIME,
gyro_init_stream, mdata) < 0)
SOL_WRN("error in scheduling a L3G4200D gyro's init command");
return false;
}
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
gyro_read(struct gyroscope_l3g4200d_data *mdata)
{
uint8_t num_samples_available;
uint8_t fifo_status = 0;
int r;
if (!sol_i2c_set_slave_address(mdata->i2c, GYRO_ADDRESS)) {
SOL_WRN("Failed to set slave at address 0x%02x\n", GYRO_ADDRESS);
return;
}
fifo_status = 0;
r = sol_i2c_read_register(mdata->i2c,
GYRO_REG_FIFO_SRC, &fifo_status, 1);
if (r <= 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.
* int16_t and 3 entries because of x, y and z axis are read,
* each consisting of L + H byte parts
*/
int16_t buffer[num_samples_available][3];
double scale = mdata->use_rad ? GYRO_SCALE_R_S * DEG_TO_RAD
: GYRO_SCALE_R_S;
r = sol_i2c_read_register(mdata->i2c,
GYRO_REG_XL | GYRO_REG_AUTO_INCREMENT,
(uint8_t *)&buffer[0][0], sizeof(buffer));
if (r <= 0) {
SOL_WRN("Failed to read L3G4200D gyro samples");
return;
}
/* raw readings, with only the sensor-provided filtering */
for (uint8_t i = 0; i < num_samples_available; i++) {
mdata->reading[0] = buffer[i][0] * scale;
mdata->reading[1] = -buffer[i][1] * scale;
mdata->reading[2] = -buffer[i][2] * scale;
}
}
}
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
_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;
}