Merge branch 'main' into feature-imufilter

Author: JimOAOOO

src/controls/controller_manager.cpp

@@ -53,12 +53,12 @@ void ControllerManager::step(float macro_reference[STATE_LEN][3], float macro_es
         // grab the motor outputs for this controller
         controllers[i]->step(macro_reference, macro_estimate, micro_estimate, outputs);
 
-        Serial.printf("Controller %d\n", i);
+        // Serial.printf("Controller %d\n", i);
 
         // iterate through all the motors this controller sets
         for (size_t j = 0; j < CAN_MAX_MOTORS + 1; j++) {
             if (config_data->controller_info[i][j + 1] < 0) break;
-            Serial.printf("\tMotor %d: %f\n", config_data->controller_info[i][j + 1], outputs[j]);
+            // Serial.printf("\tMotor %d: %f\n", config_data->controller_info[i][j + 1], outputs[j]);
             actuator_write(config_data->controller_info[i][j + 1], outputs[j]);
         }
     }

src/main.cpp

@@ -260,13 +260,13 @@ int main() {
         }
 
         // print dr16
-        Serial.printf("DR16:\n\t");
-        dr16.print();
+        // Serial.printf("DR16:\n\t");
+        // dr16.print();
 
-        Serial.printf("Target state:\n");
-        for (int i = 0; i < 8; i++) {
-            Serial.printf("\t%d: %f %f %f\n", i, target_state[i][0], target_state[i][1], target_state[i][2]);
-        }
+        // Serial.printf("Target state:\n");
+        // for (int i = 0; i < 8; i++) {
+        //     Serial.printf("\t%d: %f %f %f\n", i, target_state[i][0], target_state[i][1], target_state[i][2]);
+        // }
 
         // override temp state if needed
         if (incoming->get_hive_override_request() == 1) {
@@ -285,25 +285,25 @@ int main() {
             count_one++;
         }
 
-        Serial.printf("Estimated state:\n");
-        for (int i = 0; i < 8; i++) {
-            Serial.printf("\t%d: %f %f %f\n", i, temp_state[i][0], temp_state[i][1], temp_state[i][2]);
-        }
+        // Serial.printf("Estimated state:\n");
+        // for (int i = 0; i < 8; i++) {
+        //     Serial.printf("\t%d: %f %f %f\n", i, temp_state[i][0], temp_state[i][1], temp_state[i][2]);
+        // }
 
         // reference govern
         governor.set_estimate(temp_state);
         governor.step_reference(target_state, config->governor_types);
         governor.get_reference(temp_reference);
 
-        Serial.printf("Reference state:\n");
-        for (int i = 0; i < 8; i++) {
-            Serial.printf("\t%d: %f %f %f\n", i, temp_reference[i][0], temp_reference[i][1], temp_reference[i][2]);
-        }
+        // Serial.printf("Reference state:\n");
+        // for (int i = 0; i < 8; i++) {
+        //     Serial.printf("\t%d: %f %f %f\n", i, temp_reference[i][0], temp_reference[i][1], temp_reference[i][2]);
+        // }
 
         // generate motor outputs from controls
         controller_manager.step(temp_reference, temp_state, temp_micro_state);
 
-        can.print_state();
+        // can.print_state();
 
         // construct sensor data packet
         SensorData sensor_data;
@@ -349,12 +349,12 @@ int main() {
         if (dr16.is_connected() && (dr16.get_l_switch() == 2 || dr16.get_l_switch() == 3) && config_layer.is_configured() && !is_slow_loop) {
             // SAFETY OFF
             can.write();
-            Serial.printf("Can write\n");
+            // Serial.printf("Can write\n");
         } else {
             // SAFETY ON
             // TODO: Reset all controller integrators here
             can.issue_safety_mode();
-            Serial.printf("Can zero\n");
+            // Serial.printf("Can zero\n");
         }
 
         // LED heartbeat -- linked to loop count to reveal slowdowns and freezes.

src/sensors/SensorManager.cpp

@@ -73,7 +73,7 @@ void SensorManager::init(const Config* config_data) {
 void SensorManager::read() {
     for (int i = 0; i < buff_sensor_count; i++) {
         buff_encoders[i]->read();
-        buff_encoders[i]->print();
+        // buff_encoders[i]->print();
     }
     for (int i = 0; i < icm_sensor_count; i++) {
         icm_sensors[i]->read();
@@ -83,7 +83,7 @@ void SensorManager::read() {
 
     for (int i = 0; i < rev_sensor_count; i++) {
         rev_sensors[i].read();
-        rev_sensors[i].print();
+        // rev_sensors[i].print();
     }
     if (lidar_sensor_count > 0) {
 
@@ -97,7 +97,7 @@ void SensorManager::read() {
     ref->read();
     for (int i = 0; i < tof_sensor_count; i++) {
         tof_sensors[i]->read();
-        tof_sensors[i]->print();
+        // tof_sensors[i]->print();
     }
 }
 BuffEncoder* SensorManager::get_buff_encoder(int index) {

src/sensors/can/can_manager.hpp

@@ -6,7 +6,7 @@
 #include <map>
 
 /// @brief Debug flag to enable verbose logging. This can get quite noisy so it is off by default. Critical errors are still printed regardless of flag
-#define CAN_MANAGER_DEBUG
+// #define CAN_MANAGER_DEBUG
 
 /// @brief The maximum number of motors that can be connected to a single bus
 constexpr uint32_t CAN_MAX_MOTORS_PER_BUS = 8u;

tools/tcm_blame.py

@@ -0,0 +1,95 @@
+import re
+import os
+import sys
+
+"""
+python script to parse the map file (generated by build process) and find what symbols are using the most ITCM/DTCM
+
+from firmware/:
+    python tools/tcm_blame.py ./build/firmware.map 20
+    python3 tools/tcm_blame.py ./build/firmware.map 20
+
+from firmware/tools:
+    python tcm_blame.py ../build/firmware.map 20 
+    python3 tcm_blame.py ../build/firmware.map 20 
+"""
+
+# origin and end addresses for tcm (Teensy 4.1)
+ITCM_START, ITCM_END = 0x00000000, 0x0007FFFF
+DTCM_START, DTCM_END = 0x20000000, 0x2007FFFF
+
+if len(sys.argv) != 3:
+    print(f"Usage: {sys.argv[0]} <map_file_path> <top_n>")
+    sys.exit(1)
+
+map_file_path = sys.argv[1]
+n = int(sys.argv[2])
+
+symbols = []
+line_num = 0
+is_discarded = False
+is_debug = False
+
+# parse map file
+with open(map_file_path, "r") as map_file:
+    for line in map_file:
+        # skip the discarded input section
+        if (line.strip() == "Discarded input sections"):
+            is_discarded = True
+        # end of the discarded input section
+        if (line.strip() == "Memory Configuration"):
+            is_discarded = False
+
+        line_num += 1
+        if (is_discarded):
+            continue
+
+        # skip the debug sections
+        debug_match = re.match(r'^\s*\.debug_', line)
+        if (debug_match):
+            is_debug = True
+        # end of the debug sections
+        if (line == "Cross Reference Table\n"):
+            is_debug = False
+        
+        if (is_debug):
+            continue
+        
+        # regex from hell
+        match = re.match(r"\s*(0x[0-9a-fA-F]+)\s+(0x[0-9a-fA-F]+)\s+(\S+)", line)
+
+        # if we get a match for this line, parse it
+        if match:
+            address = int(match.group(1), 16)  # <- int base 16 lol
+            size = int(match.group(2), 16)
+            symbol = match.group(3)
+
+            # infer the region by address
+            if ITCM_START <= address <= ITCM_END:
+                region = "ITCM"
+            elif DTCM_START <= address <= DTCM_END:
+                region = "DTCM"
+            else:
+                # skip things that arent TCM
+                continue
+
+            symbols.append((region, address, size, symbol, line_num))
+
+# sort by size and get the top n
+dtcm_symbols = sorted([s for s in symbols if s[0] == "DTCM"], key=lambda x: -x[2])[:n]
+itcm_symbols = sorted([s for s in symbols if s[0] == "ITCM"], key=lambda x: -x[2])[:n]
+
+# thank you chat
+def print_table(title, symbol_list):
+    print(f"\nTop {n} {title} Memory Users:\n")
+    print(f"{'Region':<6} {'Addr':>10} {'Size':>8}  {'Symbol':<30} {'Line'}")
+    print("-" * 80)
+    for region, address, size, symbol, line_num in symbol_list:
+        filename = os.path.basename(symbol)
+        print(
+            f"{region:<6} {address:#010x} {size:8}  {filename:<30} {map_file_path}:{line_num}"
+        )
+
+
+print_table("DTCM", dtcm_symbols)
+print_table("ITCM", itcm_symbols)