add magnetometer
This commit is contained in:
@ -91,7 +91,7 @@ int main(void)
|
||||
uint8_t ppt;
|
||||
uint8_t font_width;
|
||||
uint8_t font_height;
|
||||
struct sensor_value voltage, current, pressure, temperature, accel_x, accel_y, accel_z, accel_z_ref, gyro_x, gyro_y, gyro_z, humidity;
|
||||
struct sensor_value voltage, current, pressure, temperature, accel_x, accel_y, accel_z, accel_z_ref, gyro_x, gyro_y, gyro_z, mag_x, mag_y, mag_z, humidity;
|
||||
|
||||
char str_v[15] = {0};
|
||||
char str_i[15] = {0};
|
||||
@ -104,6 +104,9 @@ int main(void)
|
||||
char str_gx[16] = {0};
|
||||
char str_gy[16] = {0};
|
||||
char str_gz[16] = {0};
|
||||
char str_mx[16] = {0};
|
||||
char str_my[16] = {0};
|
||||
char str_mz[16] = {0};
|
||||
char str_h[16] = {0};
|
||||
|
||||
LOG_INF("Starting Mellifera version %s...", APP_VERSION_STRING);
|
||||
@ -267,6 +270,18 @@ int main(void)
|
||||
{
|
||||
LOG_ERR("Could not get gyro");
|
||||
}
|
||||
if (sensor_channel_get(imu, SENSOR_CHAN_MAGN_X, &mag_x) < 0)
|
||||
{
|
||||
LOG_ERR("Could not get mag");
|
||||
}
|
||||
if (sensor_channel_get(imu, SENSOR_CHAN_MAGN_Y, &mag_y) < 0)
|
||||
{
|
||||
LOG_ERR("Could not get mag");
|
||||
}
|
||||
if (sensor_channel_get(imu, SENSOR_CHAN_MAGN_Z, &mag_z) < 0)
|
||||
{
|
||||
LOG_ERR("Could not get mag");
|
||||
}
|
||||
|
||||
if (sensor_sample_fetch(lis) < 0)
|
||||
{
|
||||
@ -300,21 +315,24 @@ int main(void)
|
||||
sprintf(str_gx, "GX:%+7.3f", gyro_x.val1 + gyro_x.val2 * 1e-6);
|
||||
sprintf(str_gy, "GY:%+7.3f", gyro_y.val1 + gyro_y.val2 * 1e-6);
|
||||
sprintf(str_gz, "GZ:%+7.3f", gyro_z.val1 + gyro_z.val2 * 1e-6);
|
||||
sprintf(str_mx, "MX:%+7.3f", mag_x.val1 + mag_x.val2 * 1e-6);
|
||||
sprintf(str_my, "MY:%+7.3f", mag_y.val1 + mag_y.val2 * 1e-6);
|
||||
sprintf(str_mz, "MZ:%+7.3f", mag_z.val1 + mag_z.val2 * 1e-6);
|
||||
sprintf(str_az_ref, "Zr:%+7.3f", accel_z_ref.val1 + accel_z_ref.val2 * 1e-6);
|
||||
sprintf(str_h, "H :%7.3f", humidity.val1 + humidity.val2 * 1e-6);
|
||||
|
||||
// LOG_INF("%s\t%s\t%s\t%s", str_v, str_i, str_p, str_t);
|
||||
|
||||
cfb_framebuffer_clear(display, false);
|
||||
if (cfb_print(display, str_gx, 0, 0))
|
||||
if (cfb_print(display, str_mx, 0, 0))
|
||||
{
|
||||
LOG_ERR("Failed to print a string");
|
||||
}
|
||||
if (cfb_print(display, str_gy, 0, 16))
|
||||
if (cfb_print(display, str_my, 0, 16))
|
||||
{
|
||||
LOG_ERR("Failed to print a string");
|
||||
}
|
||||
if (cfb_print(display, str_gz, 0, 16 * 2))
|
||||
if (cfb_print(display, str_mz, 0, 16 * 2))
|
||||
{
|
||||
LOG_ERR("Failed to print a string");
|
||||
}
|
||||
|
@ -149,7 +149,18 @@ const uint8_t MAG_REG_RHALL_LSB = 0x48;
|
||||
const uint8_t MAG_REG_RHALL_MSB = 0x49;
|
||||
const uint8_t MAG_REG_INT_STATUS = 0x4A;
|
||||
const uint8_t MAG_REG_POWER = 0x4B;
|
||||
typedef enum mag_power_t
|
||||
{
|
||||
MAG_POWER_POWER_ON = 0x01,
|
||||
MAG_POWER_RESET = 0x82,
|
||||
} mag_power_t;
|
||||
const uint8_t MAG_REG_MODE = 0x4C;
|
||||
typedef enum mag_mode_t
|
||||
{
|
||||
MAG_MODE_OPMODE_NORMAL = 0x00 << 1,
|
||||
MAG_MODE_OPMODE_FORCED = 0x01 << 1,
|
||||
MAG_MODE_OPMODE_SLEEP_MODE = 0x11 << 1,
|
||||
} mag_mode_t;
|
||||
const uint8_t MAG_REG_INT_ENABLE = 0x4D;
|
||||
const uint8_t MAG_REG_INT_SETTINGS = 0x4E;
|
||||
const uint8_t MAG_REG_LOW_THRESH = 0x4F;
|
||||
@ -213,6 +224,19 @@ static int bmx055_sample_fetch(const struct device *dev,
|
||||
data->gyro_y = ((int16_t)sys_get_le16(&gyro[2]));
|
||||
data->gyro_z = ((int16_t)sys_get_le16(&gyro[4]));
|
||||
|
||||
uint8_t mag[6];
|
||||
struct i2c_dt_spec mag_bus = config->bus;
|
||||
mag_bus.addr = 0x10;
|
||||
ret = i2c_burst_read_dt(&mag_bus, MAG_REG_DATA_X_LSB, mag, sizeof(mag));
|
||||
if (ret < 0)
|
||||
{
|
||||
LOG_ERR("Failed to read gyro registers!");
|
||||
return ret;
|
||||
}
|
||||
data->mag_x = ((int16_t)sys_get_le16(&mag[0])) >> 1;
|
||||
data->mag_y = ((int16_t)sys_get_le16(&mag[2])) >> 1;
|
||||
data->mag_z = ((int16_t)sys_get_le16(&mag[4])) >> 1;
|
||||
|
||||
ret = i2c_burst_read_dt(&config->bus, ACCEL_REG_ACCD_TEMP, &data->temperature, sizeof(data->temperature));
|
||||
if (ret < 0)
|
||||
{
|
||||
@ -287,10 +311,28 @@ static int bmx055_channel_get(const struct device *dev,
|
||||
val->val2 = (rate - val->val1) * 1000000;
|
||||
break;
|
||||
}
|
||||
// Gauss
|
||||
case SENSOR_CHAN_MAGN_X:
|
||||
{
|
||||
float rate = data->mag_x * 1300.0 / (2 << 15) / 100; // to uT, to gauss
|
||||
val->val1 = rate;
|
||||
val->val2 = (rate - val->val1) * 1000000;
|
||||
break;
|
||||
}
|
||||
case SENSOR_CHAN_MAGN_Y:
|
||||
{
|
||||
float rate = data->mag_y * 1300.0 / (2 << 15) / 100; // to uT, to gauss
|
||||
val->val1 = rate;
|
||||
val->val2 = (rate - val->val1) * 1000000;
|
||||
break;
|
||||
}
|
||||
case SENSOR_CHAN_MAGN_Z:
|
||||
return -ENOTSUP;
|
||||
{
|
||||
float rate = data->mag_z * 2500.0 / (2 << 15) / 100; // to uT, to gauss
|
||||
val->val1 = rate;
|
||||
val->val2 = (rate - val->val1) * 1000000;
|
||||
break;
|
||||
}
|
||||
default:
|
||||
return -ENOTSUP;
|
||||
}
|
||||
@ -354,7 +396,6 @@ static int bmx055_init(const struct device *dev)
|
||||
|
||||
struct i2c_dt_spec gyro_bus = config->bus;
|
||||
gyro_bus.addr = 0x68;
|
||||
|
||||
uint8_t gyro_chip_id;
|
||||
ret = i2c_burst_read_dt(&gyro_bus, GYRO_REG_CHIP_ID, &gyro_chip_id, sizeof(gyro_chip_id));
|
||||
if (ret < 0)
|
||||
@ -368,6 +409,37 @@ static int bmx055_init(const struct device *dev)
|
||||
LOG_ERR("Gyro chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, gyro_chip_id, GYRO_CHIP_ID);
|
||||
}
|
||||
|
||||
struct i2c_dt_spec mag_bus = config->bus;
|
||||
mag_bus.addr = 0x10;
|
||||
|
||||
uint8_t mag_power = MAG_POWER_POWER_ON;
|
||||
ret = i2c_burst_write_dt(&mag_bus, MAG_REG_POWER, &mag_power, sizeof(mag_power));
|
||||
if (ret < 0)
|
||||
{
|
||||
LOG_ERR("Failed to power up magnetometer");
|
||||
return ret;
|
||||
}
|
||||
uint8_t mag_mode = MAG_MODE_OPMODE_NORMAL;
|
||||
ret = i2c_burst_write_dt(&mag_bus, MAG_REG_MODE, &mag_mode, sizeof(mag_mode));
|
||||
if (ret < 0)
|
||||
{
|
||||
LOG_ERR("Failed to start magnetometer");
|
||||
return ret;
|
||||
}
|
||||
|
||||
uint8_t mag_chip_id;
|
||||
ret = i2c_burst_read_dt(&mag_bus, MAG_REG_CHIP_ID, &mag_chip_id, sizeof(mag_chip_id));
|
||||
if (ret < 0)
|
||||
{
|
||||
LOG_ERR("Failed to read magnetometer chip ID register!");
|
||||
return ret;
|
||||
}
|
||||
const uint8_t MAG_CHIP_ID = 0x32;
|
||||
if (mag_chip_id != MAG_CHIP_ID)
|
||||
{
|
||||
LOG_ERR("Mag chip ID read from %s incorrect. Read 0x%02X, expected 0x%02X", dev->name, mag_chip_id, MAG_CHIP_ID);
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
||||
|
Reference in New Issue
Block a user