Mirrors_Framework/RBF-PID-Balbot-firmware
1
0
Fork 0
mirror of https://github.com/Dictor/RBF-PID-Balbot-firmware.git synced 2026-09-24 23:03:49 +08:00
Code Issues Packages Projects Releases Wiki Activity

Implement magnetic sensor reading

Browse Source
This commit is contained in:
Jeonghyun Kim
2023-08-15 22:12:45 +09:00
parent b254ff4f30
commit 36d74adf3d
3 changed files with 12 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
inc/hardware.h
+1 -1
View File
@@ -17,7 +17,7 @@ extern const struct device *imu;
int CheckHardware();
int InitHardware();
int ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro);
int ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro, std::array<double, 3> &magn);
}; // namespace hardware
}; // namespace RbfpidBalbot
src/app_main.cpp
+5 -7
View File
@@ -28,15 +28,12 @@ void AppMain(void) {
LOG_INF("application started");
LOG_INF("RBF-PID Balbot");
const int dt_ms = 10;
std::array<double, 3> d_accel, d_gyro, euler;
std::array<float, 3> f_accel, f_gyro;
posture::MahonyAHRS mahony(dt_ms);
std::array<double, 3> d_accel, d_gyro, d_magn, euler;
std::array<float, 3> f_accel, f_gyro, f_magn;
for (;;) {
k_sleep(K_MSEC(dt_ms));
if (int ret = hardware::ReadIMU(d_accel, d_gyro) < 0) {
if (int ret = hardware::ReadIMU(d_accel, d_gyro, d_magn) < 0) {
LOG_ERR("fail to read IMU, ret=%d", ret);
continue;
}
@@ -44,9 +41,10 @@ void AppMain(void) {
for (int i = 0; i < 3; i++) {
f_accel[i] = (float)d_accel[i];
f_gyro[i] = (float)d_accel[i];
f_magn[i] = (float)d_magn[i];
}
mahony.Update(f_gyro[0], f_gyro[1], f_gyro[2], f_accel[0], f_accel[1], f_accel[2]);
mahony.Update(f_gyro[0], f_gyro[1], f_gyro[2], f_accel[0], f_accel[1], f_accel[2], f_magn[0], f_magn[1], f_magn[2]);
euler = mahony.GetEuler();
LOG_INF("r %f p %f y %f", euler[0], euler[1], euler[2]);
}
src/hardware.cpp
+6 -2
View File
@@ -37,8 +37,8 @@ int hardware::InitHardware() {
return 0;
}
int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro) {
struct sensor_value tmp_a[3], tmp_g[3];
int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro, std::array<double, 3> &magn) {
struct sensor_value tmp_a[3], tmp_g[3], tmp_m[3];
int rc = sensor_sample_fetch(hardware::imu);
if (rc == 0) {
@@ -47,10 +47,14 @@ int hardware::ReadIMU(std::array<double, 3> &accel, std::array<double, 3> &gyro)
if (rc == 0) {
rc = sensor_channel_get(hardware::imu, SENSOR_CHAN_GYRO_XYZ, tmp_g);
}
if (rc == 0) {
rc = sensor_channel_get(hardware::imu, SENSOR_CHAN_MAGN_XYZ, tmp_m);
}
if (rc == 0) {
for (int i = 0; i < 3; i++) {
accel[i] = sensor_value_to_double(&tmp_a[i]);
gyro[i] = sensor_value_to_double(&tmp_g[i]);
magn[i] = sensor_value_to_double(&tmp_m[i]);
}
} else {
LOG_ERR("sample fetch/get failed: %d\n", rc);