Merge pull request #20 from HeronMkII/commands

Fixed lib-common update errors, added IMU to commands

Author: brunofalmeida

manual_tests/main_test/main_test.c

@@ -86,8 +86,8 @@ void print_therm_temp(uint16_t raw_data) {
     print(" 0x%.3X = %.2f C\n", raw_data, adc_raw_data_to_therm_temp(raw_data));
 }
 
-void print_imu_data(uint16_t raw_data) {
-    print(" 0x%.4X\n", raw_data);
+void print_imu_gyro(uint16_t raw_data) {
+    print(" 0x%.4X = %.3f rad/s\n", raw_data, imu_raw_data_to_gyro(raw_data));
 }
 
 void process_eps_hk_tx_msg(uint8_t* tx_msg) {
@@ -154,41 +154,29 @@ void process_eps_hk_tx_msg(uint8_t* tx_msg) {
             print("Heater Setpoint 2:");
             print_therm_temp(raw_data);
             break;
-        case CAN_EPS_HK_IMU_ACC_X:
-            print("Acc X:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_UNCAL_X:
+            print("Gyro (Uncal) X:");
+            print_imu_gyro(raw_data);
             break;
-        case CAN_EPS_HK_IMU_ACC_Y:
-            print("Acc Y:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_UNCAL_Y:
+            print("Gyro (Uncal) Y:");
+            print_imu_gyro(raw_data);
             break;
-        case CAN_EPS_HK_IMU_ACC_Z:
-            print("Acc Z:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_UNCAL_Z:
+            print("Gyro (Uncal) Z:");
+            print_imu_gyro(raw_data);
             break;
-        case CAN_EPS_HK_IMU_GYR_X:
-            print("Gyr X:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_CAL_X:
+            print("Gyro (Cal) X:");
+            print_imu_gyro(raw_data);
             break;
-        case CAN_EPS_HK_IMU_GYR_Y:
-            print("Gyr Y:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_CAL_Y:
+            print("Gyro (Cal) Y:");
+            print_imu_gyro(raw_data);
             break;
-        case CAN_EPS_HK_IMU_GYR_Z:
-            print("Gyr Z:");
-            print_imu_data(raw_data);
-            break;
-        case CAN_EPS_HK_IMU_MAG_X:
-            print("Mag X:");
-            print_imu_data(raw_data);
-            break;
-        case CAN_EPS_HK_IMU_MAG_Y:
-            print("Mag Y:");
-            print_imu_data(raw_data);
-            break;
-        case CAN_EPS_HK_IMU_MAG_Z:
-            print("Mag Z:");
-            print_imu_data(raw_data);
+        case CAN_EPS_HK_GYR_CAL_Z:
+            print("Gyro (Cal) Z:");
+            print_imu_gyro(raw_data);
             break;
         default:
             return;
@@ -353,12 +341,13 @@ int main(void) {
     WDT_ENABLE_SYS_RESET(WDTO_8S);
 
     init_eps();
+    set_uart_baud_rate(UART_BAUD_115200);
 
     print("\n\n\nStarting commands test\n\n");
 
     // Change these as necessary for testing
     sim_local_actions = false;
-    sim_obc = false;
+    sim_obc = true;
     print_can_msgs = true;
 
     print("sim_local_actions = %u\n", sim_local_actions);

manual_tests/main_test/makefile

@@ -1,4 +1,4 @@
 PROG = main_test
 # SRC should only include necessary files
-SRC = $(addprefix ../../src/,can_commands.c can_interface.c devices.c general.c shunts.c)
+SRC = $(addprefix ../../src/,can_commands.c can_interface.c devices.c general.c imu.c shunts.c)
 include ../makefile

src/can_commands.c

@@ -79,10 +79,52 @@ void handle_rx_hk(uint8_t* rx_msg) {
         }
     }
 
-    else if ((CAN_EPS_HK_IMU_ACC_X <= field_num) &&
-            (field_num <= CAN_EPS_HK_IMU_MAG_Z)) {
-        // TODO - get IMU data
-        data = random() & 0x7FFF;
+    else if ((CAN_EPS_HK_GYR_UNCAL_X <= field_num) &&
+            (field_num <= CAN_EPS_HK_GYR_UNCAL_Z)) {
+        if (sim_local_actions) {
+            data = random() & 0x7FFF;
+        } else {
+            uint16_t uncal_x = 0, uncal_y = 0, uncal_z = 0;
+            get_imu_uncal_gyro(&uncal_x, &uncal_y, &uncal_z, NULL, NULL, NULL);
+
+            switch (field_num) {
+                case CAN_EPS_HK_GYR_UNCAL_X:
+                    data = (uint32_t) uncal_x;
+                    break;
+                case CAN_EPS_HK_GYR_UNCAL_Y:
+                    data = (uint32_t) uncal_y;
+                    break;
+                case CAN_EPS_HK_GYR_UNCAL_Z:
+                    data = (uint32_t) uncal_z;
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+
+    else if ((CAN_EPS_HK_GYR_CAL_X <= field_num) &&
+            (field_num <= CAN_EPS_HK_GYR_CAL_Z)) {
+        if (sim_local_actions) {
+            data = random() & 0x7FFF;
+        } else {
+            uint16_t cal_x = 0, cal_y = 0, cal_z = 0;
+            get_imu_cal_gyro(&cal_x, &cal_y, &cal_z);
+
+            switch (field_num) {
+                case CAN_EPS_HK_GYR_CAL_X:
+                    data = (uint32_t) cal_x;
+                    break;
+                case CAN_EPS_HK_GYR_CAL_Y:
+                    data = (uint32_t) cal_y;
+                    break;
+                case CAN_EPS_HK_GYR_CAL_Z:
+                    data = (uint32_t) cal_z;
+                    break;
+                default:
+                    break;
+            }
+        }
     }
 
     // If the message type is not recognized, return before enqueueing

src/can_commands.h

@@ -11,6 +11,7 @@
 #include "can_interface.h"
 #include "devices.h"
 #include "general.h"
+#include "imu.h"
 
 extern queue_t can_rx_msg_queue;
 extern queue_t can_tx_msg_queue;

src/general.c

@@ -6,12 +6,6 @@ void init_eps(void) {
     // UART
     init_uart();
 
-    // TODO - remove and call init_imu()
-    // Set the IMU CSn (PD0) high (because it doesn't have a pullup resistor)
-    // so it doesn't interfere with the MOSI/MISO lines
-    init_cs(PD0, &DDRD);
-    set_cs_high(PD0, &PORTD);
-
     // SPI
     init_spi();
 
@@ -27,6 +21,9 @@ void init_eps(void) {
     // Shunts
     init_shunts();
 
+    // IMU
+    init_imu();
+
     // Queues
     init_queue(&can_rx_msg_queue);
     init_queue(&can_tx_msg_queue);