struct compass_packet compass_get() { set_slave(compass_i2c_fd, COMPASS_PIN); uint8_t mode[] = {0x02, 0x01}; if (write(compass_i2c_fd, mode, 2) != 2) { perror("compass set mode"); } sleep_ms(10 * 1000); uint8_t read_data[] = {0x03}; if (write(compass_i2c_fd, read_data, 1) != 1) { perror("compass set read"); } uint8_t input[6]; if (read(compass_i2c_fd, input, 6) != 6) { perror("compass read"); } // printf("compass %d %d %d %d %d %d\n", input[0], input[1], input[2], input[3], input[4], input[5]); double x = (double) ((short) ((input[0] << 8) | input[1])); double y = (double) ((short) ((input[4] << 8) | input[5])); double z = (double) ((short) ((input[2] << 8) | input[3])); // printf("compass x=%f y=%f z=%f\n", x, y, z); double angle = atan2(y, x) * 180.0 / M_PI; double mag = sqrt(x * x + y * y + z * z); if (angle < 0) { angle += 180.0; } // printf("angle=%0.1f, mag=%0.1f\n", angle, mag); struct compass_packet compass = { .packet.id = PACKET_ID_COMPASS, .x = (float) x, .y = (float) y, .z = (float) z, .angle = (float) angle, .magnitude = (float) mag }; return compass; }
static bool gyro_init(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init)) 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 bool gyro_init_fifo(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_fifo)) 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 bool gyro_tick_do(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_tick_do)) 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_init_range(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_range)) 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_stream(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_stream)) 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 bool lsm303_accel_init(void *data) { struct accelerometer_lsm303_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, lsm303_accel_init)) 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; }
/* meant to run 3 times */ static bool gyro_init_sampling(void *data) { struct gyroscope_l3g4200d_data *mdata = data; mdata->timer = NULL; if (!set_slave(mdata, gyro_init_sampling)) 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 bool stts751_init(void *data) { struct stts751_data *mdata = data; static const uint8_t resolutions[] = { 0x2, 0x0, 0x1, 0x3 }; mdata->timer = NULL; if (!set_slave(mdata, stts751_init)) return false; /*TODO: It might be a good idea to use the stand-by mode and one-shot reading, * as it saves energy */ /*Resolution bits are form 9 to 12, their setup values are on resolutions * from 0 to 3, thus the '- 9' below */ mdata->i2c_buffer = resolutions[mdata->resolution - 9] << 2; mdata->i2c_pending = sol_i2c_write_register(mdata->i2c, CONFIGURATION_REGISTER, &mdata->i2c_buffer, 1, i2c_write_configuration_cb, mdata); if (!mdata->i2c_pending) SOL_WRN("Could not set STTS751 temperature reading resolution"); return false; }
static void send_temperature(struct stts751_data *mdata) { double temp; static const double steps[] = { 0.5, 0.25, 0.125, 0.0625 }; struct sol_drange val = { .min = -64.0, .max = 127.9375, .step = steps[mdata->resolution - 9] }; SOL_DBG("Temperature registers H:0x%x, L:0x%x", mdata->temp_h, mdata->temp_l); temp = mdata->temp_h; /* XXX Check if negative conversion is right */ temp += ((double)(mdata->temp_l) / (1 << 8)); /* To Kelvin */ temp += 273.16; val.val = temp; sol_flow_send_drange_packet(mdata->node, SOL_FLOW_NODE_TYPE_STTS751__OUT__KELVIN, &val); } static void read_cb(void *cb_data, struct sol_i2c *i2c, uint8_t reg, uint8_t *data, ssize_t status) { struct stts751_data *mdata = cb_data; mdata->i2c_pending = NULL; if (status < 0) { const char errmsg[] = "Failed to read STTS751 temperature status"; SOL_WRN(errmsg); sol_flow_send_error_packet(mdata->node, EIO, errmsg); mdata->reading_step = READING_NONE; return; } /* If reading status, let's check it */ if (mdata->reading_step == READING_STATUS && mdata->status) { const char errmsg[] = "Invalid temperature status: 0x%x"; SOL_WRN(errmsg, mdata->status); mdata->reading_step = READING_NONE; return; } /* Last step, send temperature */ if (mdata->reading_step == READING_TEMP_L) { send_temperature(mdata); mdata->reading_step = READING_NONE; return; } mdata->reading_step++; stts751_read(mdata); } static bool stts751_read(void *data) { struct stts751_data *mdata = data; uint8_t reg, *dst; mdata->timer = NULL; if (!set_slave(mdata, stts751_read)) return false; switch (mdata->reading_step) { case READING_STATUS: reg = STATUS_REGISTER; dst = &mdata->status; break; case READING_TEMP_H: reg = TEMPERATURE_REGISTER_H; dst = (uint8_t *)&mdata->temp_h; break; case READING_TEMP_L: reg = TEMPERATURE_REGISTER_L; dst = &mdata->temp_l; break; default: SOL_WRN("Invalid reading step"); return false; } mdata->i2c_pending = sol_i2c_read_register(mdata->i2c, reg, dst, sizeof(*dst), read_cb, mdata); if (!mdata->i2c_pending) { const char errmsg[] = "Failed to read STTS751 temperature"; SOL_WRN(errmsg); sol_flow_send_error_packet(mdata->node, EIO, errmsg); mdata->reading_step = READING_NONE; } return false; } static int temperature_stts751_tick(struct sol_flow_node *node, void *data, uint16_t port, uint16_t conn_id, const struct sol_flow_packet *packet) { struct stts751_data *mdata = data; if (mdata->reading_step != READING_NONE) { SOL_WRN("Reading operation in progress, discading TICK"); return 0; } /* First, read the status, if it's ok, then we read temp high and low */ mdata->reading_step = READING_STATUS; stts751_read(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 (!set_slave(mdata, lsm303_read_data)) 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; }