Merge pull request #19 from HeronMkII/imu

IMU updates - lib-common conversions, packet printing

Author: brunofalmeida

lib-common

@@ -2,6 +2,8 @@
 NOTE: SET COOLTERM BAUD RATE TO 115,200
 */
 
+#include <conversions/conversions.h>
+
 #include "../../src/imu.h"
 
 void receive_and_print_packets(uint16_t count) {
@@ -45,7 +47,7 @@ void test_prod_id(void) {
 void test_accel_once(void) {
     print("\nGetting acceleration...\n\n");
 
-    int16_t x = 0, y = 0, z = 0;
+    uint16_t x = 0, y = 0, z = 0;
     if (!get_imu_accel(&x, &y, &z)) {
         print("\nGet acceleration: FAIL\n\n");
         return;
@@ -69,7 +71,7 @@ void test_accel_inf(void) {
 }
 
 void test_accel_fast_once(void) {
-    int16_t x = 0, y = 0, z = 0;
+    uint16_t x = 0, y = 0, z = 0;
     get_imu_accel(&x, &y, &z);
     print("Acceleration: x = %+.6f, y = %+.6f, z = %+.6f\n",
         imu_raw_data_to_double(x, IMU_ACCEL_Q),
@@ -80,8 +82,8 @@ void test_accel_fast_once(void) {
 void test_uncal_gyro_once(void) {
     print("\nGetting uncalibrated gyroscope...\n\n");
 
-    int16_t x = 0, y = 0, z = 0;
-    int16_t bias_x = 0, bias_y = 0, bias_z = 0;
+    uint16_t x = 0, y = 0, z = 0;
+    uint16_t bias_x = 0, bias_y = 0, bias_z = 0;
     if (!get_imu_uncal_gyro(&x, &y, &z, &bias_x, &bias_y, &bias_z)) {
         print("\nGet uncalibrated gyroscope: FAIL\n\n");
         return;
@@ -91,14 +93,14 @@ void test_uncal_gyro_once(void) {
     print("Get uncalibrated gyroscope: SUCCESS\n");
     print("Raw: x = %d, y = %d, z = %d\n", x, y, z);
     print("Converted: x = %.6f, y = %.6f, z = %.6f\n",
-        imu_raw_data_to_double(x, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(y, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(z, IMU_UNCAL_GYRO_Q));
+        imu_raw_data_to_gyro(x),
+        imu_raw_data_to_gyro(y),
+        imu_raw_data_to_gyro(z));
     print("Raw: bias_x = %d, bias_y = %d, bias_z = %d\n", bias_x, bias_y, bias_z);
     print("Converted: bias_x = %.6f, bias_y = %.6f, bias_z = %.6f\n",
-        imu_raw_data_to_double(bias_x, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(bias_y, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(bias_z, IMU_UNCAL_GYRO_Q));
+        imu_raw_data_to_gyro(bias_x),
+        imu_raw_data_to_gyro(bias_y),
+        imu_raw_data_to_gyro(bias_z));
     print("\n");
 }
 
@@ -110,18 +112,18 @@ void test_uncal_gyro_inf(void) {
 }
 
 void test_uncal_gyro_fast_once(void) {
-    int16_t x = 0, y = 0, z = 0;
+    uint16_t x = 0, y = 0, z = 0;
     get_imu_uncal_gyro(&x, &y, &z, NULL, NULL, NULL);
     print("Uncalibrated Gyroscope: x = %+.6f, y = %+.6f, z = %+.6f\n",
-        imu_raw_data_to_double(x, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(y, IMU_UNCAL_GYRO_Q),
-        imu_raw_data_to_double(z, IMU_UNCAL_GYRO_Q));
+        imu_raw_data_to_gyro(x),
+        imu_raw_data_to_gyro(y),
+        imu_raw_data_to_gyro(z));
 }
 
 void test_cal_gyro_once(void) {
     print("\nGetting calibrated gyroscope...\n\n");
 
-    int16_t x = 0, y = 0, z = 0;
+    uint16_t x = 0, y = 0, z = 0;
     if (!get_imu_cal_gyro(&x, &y, &z)) {
         print("\nGet calibrated gyroscope.: FAIL\n\n");
         return;
@@ -131,9 +133,9 @@ void test_cal_gyro_once(void) {
     print("Get calibrated gyroscope.: SUCCESS\n");
     print("Raw: x = %d, y = %d, z = %d\n", x, y, z);
     print("Converted: x = %.6f, y = %.6f, z = %.6f\n",
-        imu_raw_data_to_double(x, IMU_CAL_GYRO_Q),
-        imu_raw_data_to_double(y, IMU_CAL_GYRO_Q),
-        imu_raw_data_to_double(z, IMU_CAL_GYRO_Q));
+        imu_raw_data_to_gyro(x),
+        imu_raw_data_to_gyro(y),
+        imu_raw_data_to_gyro(z));
     print("\n");
 }
 
@@ -146,38 +148,38 @@ void test_cal_gyro_inf(void) {
 
 // Without success/fail print statements
 void test_cal_gyro_fast_once(void) {
-    int16_t x = 0, y = 0, z = 0;
+    uint16_t x = 0, y = 0, z = 0;
     get_imu_cal_gyro(&x, &y, &z);
     print("Calibrated Gyroscope: x = %+.6f, y = %+.6f, z = %+.6f\n",
-        imu_raw_data_to_double(x, IMU_CAL_GYRO_Q),
-        imu_raw_data_to_double(y, IMU_CAL_GYRO_Q),
-        imu_raw_data_to_double(z, IMU_CAL_GYRO_Q));
+        imu_raw_data_to_gyro(x),
+        imu_raw_data_to_gyro(y),
+        imu_raw_data_to_gyro(z));
 }
 
 // Compare uncalibrated with calibrated
 void test_gyro_comp_inf(void) {
     print("\nGetting gyroscope...\n\n");
 
     while (1) {
-        int16_t uncal_x = 0, uncal_y = 0, uncal_z = 0;
-        int16_t bias_x = 0, bias_y = 0, bias_z = 0;
+        uint16_t uncal_x = 0, uncal_y = 0, uncal_z = 0;
+        uint16_t bias_x = 0, bias_y = 0, bias_z = 0;
         get_imu_uncal_gyro(&uncal_x, &uncal_y, &uncal_z, &bias_x, &bias_y, &bias_z);
-        int16_t cal_x = 0, cal_y = 0, cal_z = 0;
+        uint16_t cal_x = 0, cal_y = 0, cal_z = 0;
         get_imu_cal_gyro(&cal_x, &cal_y, &cal_z);
 
         print("\n");
         print("Uncalibrated: x = %+.6f, y = %+.6f, z = %+.6f\n",
-            imu_raw_data_to_double(uncal_x, IMU_UNCAL_GYRO_Q),
-            imu_raw_data_to_double(uncal_y, IMU_UNCAL_GYRO_Q),
-            imu_raw_data_to_double(uncal_z, IMU_UNCAL_GYRO_Q));
+            imu_raw_data_to_gyro(uncal_x),
+            imu_raw_data_to_gyro(uncal_y),
+            imu_raw_data_to_gyro(uncal_z));
         print("Calibrated:   x = %+.6f, y = %+.6f, z = %+.6f\n",
-            imu_raw_data_to_double(cal_x, IMU_CAL_GYRO_Q),
-            imu_raw_data_to_double(cal_y, IMU_CAL_GYRO_Q),
-            imu_raw_data_to_double(cal_z, IMU_CAL_GYRO_Q));
+            imu_raw_data_to_gyro(cal_x),
+            imu_raw_data_to_gyro(cal_y),
+            imu_raw_data_to_gyro(cal_z));
         print("Bias:         x = %+.6f, y = %+.6f, z = %+.6f\n",
-            imu_raw_data_to_double(bias_x, IMU_UNCAL_GYRO_Q),
-            imu_raw_data_to_double(bias_y, IMU_UNCAL_GYRO_Q),
-            imu_raw_data_to_double(bias_z, IMU_UNCAL_GYRO_Q));
+            imu_raw_data_to_gyro(bias_x),
+            imu_raw_data_to_gyro(bias_y),
+            imu_raw_data_to_gyro(bias_z));
     }
 }
 

manual_tests/imu_test/imu_test.c

@@ -159,20 +159,6 @@ uint8_t imu_data[IMU_DATA_MAX_LEN] = { 0x00 };
 uint16_t imu_data_len = 0;
 
 
-
-
-// TODO - modify lib-common print_bytes, fix uint16_t bug for len/i
-void print_hex(uint8_t* data, uint16_t len) {
-    if (len == 0) {
-        return;
-    }
-    print("%.2x", data[0]);
-    for (uint16_t i = 1; i < len; i++) {
-        print(":%.2x", data[i]);
-    }
-    print("\n");
-}
-
 /*
 Initializes the IMU (#0 p. 43).
 */
@@ -361,9 +347,9 @@ uint8_t receive_imu_packet(void) {
 #ifdef IMU_DEBUG
     print("data_len = %u\n", data_len);
     print("Header: ");
-    print_hex(imu_header, IMU_HEADER_LEN);
+    print_bytes(imu_header, IMU_HEADER_LEN);
     print("Data: ");
-    print_hex(imu_data, imu_data_len);
+    print_bytes(imu_data, imu_data_len);
 
     // Print data as string
     // for (uint16_t i = 0; i < imu_data_len; i++) {
@@ -407,9 +393,9 @@ uint8_t send_imu_packet(uint8_t channel) {
 #ifdef IMU_DEBUG
     print("\nSending IMU SPI:\n");
     print("Header: ");
-    print_hex(imu_header, IMU_HEADER_LEN);
+    print_bytes(imu_header, IMU_HEADER_LEN);
     print("Data: ");
-    print_hex(imu_data, imu_data_len);
+    print_bytes(imu_data, imu_data_len);
 #endif
 
     // Set feature command (#1 p.55-56)
@@ -509,25 +495,13 @@ uint8_t disable_imu_feat(uint8_t feat_report_id) {
     return send_imu_set_feat_cmd(feat_report_id, 0);
 }
 
-/*
-Converts the raw 16-bit signed fixed-point value from the input report to the actual floating-point measurement using the Q point.
-Q point - number of fractional digits after (to the right of) the decimal point, i.e. higher Q point means smaller/more precise number (#1 p.22)
-https://en.wikipedia.org/wiki/Q_(number_format)
-Similar to reference library qToFloat()
-raw_data - 16 bit raw value
-q_point - number of binary digits to shift
-*/
-double imu_raw_data_to_double(int16_t raw_data, uint8_t q_point) {
-    // Implement power of 2 with a bitshift instead of pow(), which links to the
-    // math library and increases the binary size by ~1.3kB
-    return ((double) raw_data) / ((double) (1 << q_point));
-}
+
 
 /*
 Enables a feature, gets one input report, and disables the feature.
 This only works for features that provide an input report of 5 bytes (timebase reference) + 10 bytes (sensor data, last 6 bytes are x/y/z)
 */
-uint8_t get_imu_data(uint8_t feat_report_id, int16_t* x, int16_t* y, int16_t* z) {
+uint8_t get_imu_data(uint8_t feat_report_id, uint16_t* x, uint16_t* y, uint16_t* z) {
     // Send set feature command, receive get feature response
     if (!enable_imu_feat(feat_report_id)) {
         return 0;
@@ -576,7 +550,7 @@ uint8_t get_imu_data(uint8_t feat_report_id, int16_t* x, int16_t* y, int16_t* z)
 x, y, z are signed fixed-point
 "The units are m/s^2. The Q point is 8." (#1 p.58)
 */
-uint8_t get_imu_accel(int16_t* x, int16_t* y, int16_t* z) {
+uint8_t get_imu_accel(uint16_t* x, uint16_t* y, uint16_t* z) {
     return get_imu_data(IMU_ACCEL, x, y, z);
 }
 
@@ -587,7 +561,7 @@ Calibrated gyroscope - drift-compensated rotational velocity
 
 x, y, z are signed fixed-point
 */
-uint8_t get_imu_cal_gyro(int16_t* x, int16_t* y, int16_t* z) {
+uint8_t get_imu_cal_gyro(uint16_t* x, uint16_t* y, uint16_t* z) {
     return get_imu_data(IMU_CAL_GYRO, x, y, z);
 }
 
@@ -600,8 +574,8 @@ x, y, z are signed fixed-point
 
 This does not use `get_imu_data()` because its input report format is different.
 */
-uint8_t get_imu_uncal_gyro(int16_t* x, int16_t* y, int16_t* z, int16_t* bias_x, 
-    int16_t* bias_y, int16_t* bias_z) {
+uint8_t get_imu_uncal_gyro(uint16_t* x, uint16_t* y, uint16_t* z, uint16_t* bias_x, 
+    uint16_t* bias_y, uint16_t* bias_z) {
 
     // Send set feature command, receive get feature response
     if (!enable_imu_feat(IMU_UNCAL_GYRO)) {

src/imu.c

@@ -38,11 +38,6 @@
 #define IMU_UNCAL_GYRO  0x07
 #define IMU_RAW_GYRO    0x15
 
-// Q points
-#define IMU_ACCEL_Q         8
-#define IMU_UNCAL_GYRO_Q    9
-#define IMU_CAL_GYRO_Q      9
-
 // Default report inteval (60ms, in microseconds)
 #define IMU_DEF_REPORT_INTERVAL 0x0000EA60
 
@@ -72,12 +67,10 @@ uint8_t send_imu_set_feat_cmd(uint8_t feat_report_id, uint32_t report_interval);
 uint8_t enable_imu_feat(uint8_t feat_report_id);
 uint8_t disable_imu_feat(uint8_t feat_report_id);
 
-double imu_raw_data_to_double(int16_t raw_data, uint8_t q_point);
-
-uint8_t get_imu_data(uint8_t feat_report_id, int16_t* x, int16_t* y, int16_t* z);
-uint8_t get_imu_accel(int16_t* x, int16_t* y, int16_t* z);
-uint8_t get_imu_uncal_gyro(int16_t* x, int16_t* y, int16_t* z, int16_t* bias_x, 
-    int16_t* bias_y, int16_t* bias_z);
-uint8_t get_imu_cal_gyro(int16_t* x, int16_t* y, int16_t* z);
+uint8_t get_imu_data(uint8_t feat_report_id, uint16_t* x, uint16_t* y, uint16_t* z);
+uint8_t get_imu_accel(uint16_t* x, uint16_t* y, uint16_t* z);
+uint8_t get_imu_uncal_gyro(uint16_t* x, uint16_t* y, uint16_t* z, uint16_t* bias_x, 
+    uint16_t* bias_y, uint16_t* bias_z);
+uint8_t get_imu_cal_gyro(uint16_t* x, uint16_t* y, uint16_t* z);
 
 #endif