Пример #1
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;
}
Пример #2
0
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;
}
Пример #3
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;
}