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