From ff81a07755d8fa1db6e7b697df5119f50b1260d1 Mon Sep 17 00:00:00 2001 From: Olivier Matz Date: Thu, 18 Sep 2014 19:53:31 +0200 Subject: [PATCH] imuboard: fix reading and sending of imu position --- common/i2c_commands.h | 6 +++--- imuboard/i2c_protocol.c | 10 ++++++++++ imuboard/imu.c | 41 +++++++++++++++++++++++++++++------------ imuboard/imu.h | 22 ++++++++++++++++++---- 4 files changed, 60 insertions(+), 19 deletions(-) diff --git a/common/i2c_commands.h b/common/i2c_commands.h index 7a9b9c5..1653368 100644 --- a/common/i2c_commands.h +++ b/common/i2c_commands.h @@ -70,9 +70,9 @@ struct i2c_ans_imuboard_status { * positive means east */ uint32_t altitude; /* altitude from elipsoid, in 1/100 meters */ - uint16_t roll; /* XXX unit ? */ - uint16_t pitch; - uint16_t yaw; + uint16_t roll; /* 1/100 degrees */ + uint16_t pitch; /* 1/100 degrees */ + uint16_t yaw; /* 1/100 degrees */ }; #endif /* _I2C_PROTOCOL_H_ */ diff --git a/imuboard/i2c_protocol.c b/imuboard/i2c_protocol.c index 015e49c..3edb9b4 100644 --- a/imuboard/i2c_protocol.c +++ b/imuboard/i2c_protocol.c @@ -34,6 +34,7 @@ #include "../common/i2c_commands.h" #include "gps_venus.h" +#include "imu.h" #include "main.h" void i2c_protocol_init(void) @@ -72,6 +73,7 @@ static void i2c_send_status(void) { struct i2c_ans_imuboard_status ans; struct gps_pos gps_pos; + struct imu_euler_int imu_pos; uint8_t irq_flags; i2c_flush(); @@ -82,6 +84,11 @@ static void i2c_send_status(void) gps_get_pos(&gps_pos); IRQ_UNLOCK(irq_flags); + /* imu */ + IRQ_LOCK(irq_flags); + imu_get_pos_euler_int(&imu_pos); + IRQ_UNLOCK(irq_flags); + ans.flags = 0; if (imuboard.flags & IMUBOARD_F_BOOT_OK) ans.flags |= I2C_IMUBOARD_STATUS_BOOT_OK; @@ -95,6 +102,9 @@ static void i2c_send_status(void) ans.altitude = gps_pos.altitude; } + ans.roll = imu_pos.roll; + ans.pitch = imu_pos.pitch; + ans.yaw = imu_pos.yaw; i2c_send(I2C_ADD_MASTER, (uint8_t *) &ans, sizeof(ans), I2C_CTRL_GENERIC); } diff --git a/imuboard/imu.c b/imuboard/imu.c index 14f8479..00a61dd 100644 --- a/imuboard/imu.c +++ b/imuboard/imu.c @@ -19,10 +19,11 @@ static struct imu_info g_imu; /* structure storing the latest quaternion */ -static struct quaternion g_quat; +static struct quaternion g_quat = { 1.0, 0.0, 0.0, 0.0 }; /* structure storing the latest euler position */ static struct euler g_euler; +static struct imu_euler_int g_euler_int; /* periodical timer structure */ static struct callout imu_timer; @@ -47,6 +48,7 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) struct imu_info imu; struct quaternion quat; struct euler euler; + struct imu_euler_int euler_int; uint8_t irq_flags; (void)arg; @@ -56,9 +58,13 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) memcpy(&quat, &g_quat, sizeof(quat)); IRQ_UNLOCK(irq_flags); - mpu6050_read_all_axes(&g_imu); - MadgwickAHRSupdate(&g_imu, &quat); - quaternion2euler(&quat, &g_euler); + mpu6050_read_all_axes(&imu); + MadgwickAHRSupdate(&imu, &quat); + quaternion2euler(&quat, &euler); + + euler_int.roll = euler.roll * (18000. / M_PI); + euler_int.pitch = euler.pitch * (18000. / M_PI); + euler_int.yaw = euler.yaw * (18000. / M_PI); /* update global variables */ IRQ_LOCK(irq_flags); @@ -70,6 +76,9 @@ static void imu_cb(struct callout_mgr *cm, struct callout *tim, void *arg) IRQ_LOCK(irq_flags); memcpy(&g_euler, &euler, sizeof(g_euler)); IRQ_UNLOCK(irq_flags); + IRQ_LOCK(irq_flags); + memcpy(&g_euler_int, &euler_int, sizeof(g_euler_int)); + IRQ_UNLOCK(irq_flags); /* reschedule event */ callout_schedule(cm, tim, 2); @@ -98,6 +107,10 @@ void imu_get_pos_euler(struct euler *euler) memcpy(euler, &g_euler, sizeof(*euler)); } +void imu_get_pos_euler_int(struct imu_euler_int *euler_int) +{ + memcpy(euler_int, &g_euler_int, sizeof(*euler_int)); +} int imu_log(uint8_t to_stdout) @@ -107,24 +120,28 @@ int imu_log(uint8_t to_stdout) uint32_t ms; uint8_t flags; struct imu_info imu; - - if (sd_log_enabled() == 0) - return 0; + struct euler angles; IRQ_LOCK(flags); ms = global_ms; imu_get_info(&imu); IRQ_UNLOCK(flags); + IRQ_LOCK(flags); + imu_get_pos_euler(&angles); + IRQ_UNLOCK(flags); + len = snprintf(buf, sizeof(buf), - "%"PRIu32"\t" - "gyro %+3.3f\t%+3.3f\t%+3.3f\t\t" - "accel %+3.3f\t%+3.3f\t%+3.3f\t\t" - "magnet %+3.3f\t%+3.3f\t%+3.3f\r\n", + "%6.6"PRIu32" | " + "gyro %+2.2f %+2.2f %+2.2f | " + "accel %+2.2f %+2.2f %+2.2f | " + "magnet %+2.2f %+2.2f %+2.2f | " + "angles %+2.2f %+2.2f %+2.2f\r\n", ms, imu.gx, imu.gy, imu.gz, imu.ax, imu.ay, imu.az, - imu.mx, imu.my, imu.mz); + imu.mx, imu.my, imu.mz, + angles.roll, angles.pitch, angles.yaw); if (!to_stdout && sd_log_enabled()) { if (sd_log_write(buf, len) != len) { printf_P(PSTR("error writing to file\n")); diff --git a/imuboard/imu.h b/imuboard/imu.h index 06755c9..d5455f4 100644 --- a/imuboard/imu.h +++ b/imuboard/imu.h @@ -29,6 +29,9 @@ #ifndef IMU_H_ #define IMU_H_ +#include +#include + struct imu_info { /* gyro */ double gx; @@ -62,6 +65,13 @@ struct euler { double yaw; }; +/* angles in 1/100 degrees */ +struct imu_euler_int { + int16_t roll; + int16_t pitch; + int16_t yaw; +}; + /* initialize the IMU */ void imu_init(void); @@ -72,12 +82,16 @@ int imu_log(uint8_t to_stdout); * the timer callback. Does not lock irq, so it's up to the user to do that. */ void imu_get_info(struct imu_info *imu); -/* return the latest position in a quaternion struct read in the timer - * callback. Does not lock irq, so it's up to the user to do that. */ +/* return the latest position read in the timer callback in a quaternion + * struct. Does not lock irq, so it's up to the user to do that. */ void imu_get_pos_quat(struct quaternion *pos); -/* return the latest position in an euler struct read in the timer - * callback. Does not lock irq, so it's up to the user to do that. */ +/* return the latest position read in the timer callback in an euler + * struct. Does not lock irq, so it's up to the user to do that. */ void imu_get_pos_euler(struct euler *pos); +/* return the latest position read in the timer callback in an imu_euler_int + * struct. Does not lock irq, so it's up to the user to do that. */ +void imu_get_pos_euler_int(struct imu_euler_int *pos); + #endif -- 2.39.5