Пример #1
0
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);
}
Пример #2
0
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;
}
Пример #3
0
/* 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;
}
Пример #4
0
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;
}
Пример #5
0
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;
}
Пример #6
0
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;
}
Пример #7
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;
}
Пример #8
0
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;
}
Пример #9
0
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");
}
Пример #10
0
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;
}
Пример #11
0
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;
}
Пример #12
0
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;
        }
    }
}
Пример #13
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
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;
}
Пример #14
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;
}
Пример #15
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;
}
Пример #16
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;
}