75 lines
2.2 KiB
C
75 lines
2.2 KiB
C
#include "mgos.h"
|
|
#include "tests_autogen.h"
|
|
#include "mgos_imu.h"
|
|
|
|
uint32_t test_imu_period_ms = 100; // 10Hz
|
|
bool test_imu_enabled = false;
|
|
static struct mgos_imu *s_imu = NULL;
|
|
|
|
#define BOARD 3
|
|
#include "boards.h"
|
|
|
|
#ifndef BOARD
|
|
#define ACC_I2CADDR -1
|
|
#define ACC_TYPE -1
|
|
#define GYRO_I2CADDR -1
|
|
#define GYRO_TYPE -1
|
|
#define MAG_I2CADDR -1
|
|
#define MAG_TYPE -1
|
|
#endif
|
|
|
|
bool test_imu_create(void) {
|
|
struct mgos_imu_acc_opts acc_opts;
|
|
struct mgos_imu_gyro_opts gyro_opts;
|
|
struct mgos_imu_mag_opts mag_opts;
|
|
if (!(s_imu = mgos_imu_create())) {
|
|
LOG(LL_ERROR, ("Cannot create IMU"));
|
|
return false;
|
|
}
|
|
|
|
acc_opts.type = ACC_TYPE;
|
|
acc_opts.scale = 16.0; // G
|
|
acc_opts.odr = 100; // Hz
|
|
if (ACC_I2CADDR > 0 && !mgos_imu_accelerometer_create_i2c(s_imu, mgos_i2c_get_global(), ACC_I2CADDR, &acc_opts))
|
|
LOG(LL_ERROR, ("Cannot create accelerometer on IMU"));
|
|
|
|
gyro_opts.type = ACC_TYPE;
|
|
gyro_opts.scale = 2000; // deg/sec
|
|
gyro_opts.odr = 100; // Hz
|
|
if (GYRO_I2CADDR > 0 && !mgos_imu_gyroscope_create_i2c(s_imu, mgos_i2c_get_global(), GYRO_I2CADDR, &gyro_opts))
|
|
LOG(LL_ERROR, ("Cannot create gyroscope on IMU"));
|
|
|
|
mag_opts.type = MAG_TYPE;
|
|
mag_opts.scale = 12.0; // Gauss
|
|
mag_opts.odr = 10; // Hz
|
|
if (MAG_I2CADDR > 0 && !mgos_imu_magnetometer_create_i2c(s_imu, mgos_i2c_get_global(), MAG_I2CADDR, &mag_opts))
|
|
LOG(LL_ERROR, ("Cannot create magnetometer on IMU"));
|
|
|
|
return true;
|
|
}
|
|
|
|
bool test_imu_run(void) {
|
|
float ax, ay, az;
|
|
float gx, gy, gz;
|
|
float mx, my, mz;
|
|
|
|
if (mgos_imu_accelerometer_get(s_imu, &ax, &ay, &az)) {
|
|
LOG(LL_INFO, ("type=%-10s Accel X=%+.2f Y=%+.2f Z=%+.2f (m/s/s)",
|
|
mgos_imu_accelerometer_get_name(s_imu), ax, ay, az));
|
|
}
|
|
if (mgos_imu_gyroscope_get(s_imu, &gx, &gy, &gz)) {
|
|
LOG(LL_INFO, ("type=%-10s Gyro X=%+.2f Y=%+.2f Z=%+.2f (Rad/sec)",
|
|
mgos_imu_gyroscope_get_name(s_imu), gx, gy, gz));
|
|
}
|
|
if (mgos_imu_magnetometer_get(s_imu, &mx, &my, &mz)) {
|
|
LOG(LL_INFO, ("type=%-10s Mag X=%+.2f Y=%+.2f Z=%+.2f (uTesla)",
|
|
mgos_imu_magnetometer_get_name(s_imu), mx, my, mz));
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool test_imu_destroy(void) {
|
|
mgos_imu_destroy(&s_imu);
|
|
return true;
|
|
}
|