Mandatory assert

ECU_V2/README.md

@@ -0,0 +1,6 @@
+## Generate hash number to file name mapping
+(Note: untested) You should be able to generate a reverse mapping table to
+recover file names from the reported hash by running
+```bash
+$ pio test -e native --filter="test_generate_file_hash_mapping" -v
+```

ECU_V2/lib/common/assert.hpp

@@ -37,6 +37,7 @@ enum class AssertCode: uint8_t {
     TorqueLessThanZero = 4,
     SmoothTorqueLessThanZero = 5,
     ThrottleOverflow = 6,
+    BadMessage = 7,
 };
 
 enum class AssertLevel {
@@ -60,8 +61,5 @@ void assert_failed(AssertLevel level, const char* file, int line, AssertCode err
         } \
     } while (0)
 
-#define SOFT_ASSERT(condition)   GENERIC_ASSERT(AssertLevel::Soft, (condition), AssertCode::Unknown)
-#define SAFETY_ASSERT(condition) GENERIC_ASSERT(AssertLevel::Safety, (condition), AssertCode::Unknown)
-
-#define SOFT_ASSERT_CODE(condition, code)   GENERIC_ASSERT(AssertLevel::Soft, (condition), (code))
-#define SAFETY_ASSERT_CODE(condition, code) GENERIC_ASSERT(AssertLevel::Safety, (condition), (code))
+#define SOFT_ASSERT(condition, code)   GENERIC_ASSERT(AssertLevel::Soft, (condition), (code))
+#define SAFETY_ASSERT(condition, code) GENERIC_ASSERT(AssertLevel::Safety, (condition), (code))

ECU_V2/lib/common/ecu.cpp

@@ -6,9 +6,10 @@
 #include "can_serde.hpp"
 #include "constants.hpp"
 
-
-int64_t map(int32_t input_32, int32_t old_min_32, int32_t old_max_32, int32_t new_min_32, int32_t new_max_32) {
-    if (old_min_32 == old_max_32 || new_min_32 == new_max_32) {
+int64_t map(int32_t input_32, int32_t old_min_32, int32_t old_max_32, int32_t new_min_32, int32_t new_max_32)
+{
+    if (old_min_32 == old_max_32 || new_min_32 == new_max_32)
+    {
         /* Avoid division by zero. */
         return 0;
     }
@@ -32,51 +33,71 @@ int64_t map(int32_t input_32, int32_t old_min_32, int32_t old_max_32, int32_t ne
     return output_shifted;
 }
 
-int16_t Ecu::smooth_torque(uint16_t current_time_ms, uint16_t torque) {
-    /* smooth out throttle values by averaging out previous four values */
+int16_t Ecu::smoothTorque(uint32_t current_time_ms, int16_t torque)
+{
+    /* Smooth out throttle values by averaging out previous four values. */
+
+    static_assert(sizeof(this->torque_memory) / sizeof(this->torque_memory[0]) == 4);
 
     /* If recieves a value of 0 return 0 and set all values to 0 ;) */
-    if (torque == 0) {
-        for (int i = 0; i < 4; i++)
+    if (torque == 0)
+    {
+        for (size_t i = 0; i < 4; i++)
         {
             this->torque_memory[i] = 0;
         }
+        return 0;
+    }
+    else
+    {
+        int16_t total_torque = 0;
+        /* cycle through last 4 torque values*/
+        this->torque_memory[3] = this->torque_memory[2];
+        this->torque_memory[2] = this->torque_memory[1];
+        this->torque_memory[1] = this->torque_memory[0];
+        this->torque_memory[0] = torque;
+
+        /* sum last 4 torque values */
+        for (int i = 0; i < 4; i++)
+        {
+            total_torque += this->torque_memory[i];
+        }
         this->last_output_torque = 0;
         return 0;
     }
 
     /* Continues to send same torque message until 20ms has passed */
-    if ((uint16_t)(current_time_ms - last_smooth_update_ms) < SMOOTH_PERIOD_MS)
+    if (current_time_ms - last_smooth_update_ms < SMOOTH_PERIOD_MS)
     {
         return this->last_output_torque;
     }
     /* Starts timer for next cycle when 20ms has passed */
     this->last_smooth_update_ms = current_time_ms;
 
-    uint16_t total_torque = 0;
+    int16_t total_torque = 0;
     /* cycle through last 4 torque values*/
     this->torque_memory[3] = this->torque_memory[2];
     this->torque_memory[2] = this->torque_memory[1];
     this->torque_memory[1] = this->torque_memory[0];
     this->torque_memory[0] = torque;
 
     /* sum last 4 torque values */
-    for (int i = 0; i < 4; i++) {
+    for (int i = 0; i < 4; i++)
+    {
         total_torque += this->torque_memory[i];
     }
 
     /* average out torque and check for errors */
     total_torque = total_torque / 4;
-    SAFETY_ASSERT_CODE((total_torque >= 0), AssertCode::SmoothTorqueLessThanZero);
+    SAFETY_ASSERT((total_torque >= 0), AssertCode::SmoothTorqueLessThanZero);
     this->last_output_torque = total_torque;
     return total_torque;
 }
 
-
-int16_t throttle_map(uint16_t throttle1, uint16_t throttle2) {
+int16_t throttle_map(uint16_t throttle1, uint16_t throttle2)
+{
     /* Make sure the throttle values are in range. */
-    SAFETY_ASSERT_CODE((throttle1 >= THROTTLE1_MIN && throttle1 <= THROTTLE1_MAX), AssertCode::ThrottleOutOfRange);
-    SAFETY_ASSERT(throttle2 >= THROTTLE2_MIN && throttle2 <= THROTTLE2_MAX);
+    SAFETY_ASSERT((throttle1 >= THROTTLE1_MIN && throttle1 <= THROTTLE1_MAX), AssertCode::ThrottleOutOfRange);
 
     int64_t throttle1_percent = map(throttle1, THROTTLE1_MIN, THROTTLE1_MAX, 0, 100);
     /* DEBUG ONLY!!!! Throttle 2 set to throttle 1 :) */
@@ -87,22 +108,17 @@ int16_t throttle_map(uint16_t throttle1, uint16_t throttle2) {
 
     // FIXME this is throwing an error
     /* Make sure the two throttle values haven't diverged too far. */
-    SAFETY_ASSERT_CODE((abs(throttle1_percent - throttle2_percent) < THROTTLE_DISAGREE), AssertCode::ThrottleDisagree);
+    SAFETY_ASSERT((abs(throttle1_percent - throttle2_percent) < THROTTLE_DISAGREE), AssertCode::ThrottleDisagree);
 
     int64_t average = (throttle1_percent + throttle2_percent) / 2;
 
-    // FIXME this throwing an error
-    /* Make sure the average can fit into the new size (int16_t). */
-    SAFETY_ASSERT_CODE((average >= MIN_THROTTLE && average <= MAX_THROTTLE), AssertCode::ThrottleOverflow);
+    int64_t torque_mapped = map(average, MIN_THROTTLE, MAX_THROTTLE, 0, MAX_TORQUE);
 
-    int16_t torque_mapped = map(average, MIN_THROTTLE, MAX_THROTTLE, 0, MAX_TORQUE);
-    SAFETY_ASSERT_CODE((torque_mapped >= 0), AssertCode::TorqueLessThanZero);
-    /* TODO look into this */
-    if (torque_mapped < 0) {
-        torque_mapped = 0;
-    }
+    /* Make sure the average can fit into the new size (int16_t). */
+    SAFETY_ASSERT((torque_mapped >= MIN_THROTTLE && torque_mapped <= MAX_THROTTLE), AssertCode::ThrottleOverflow);
+    SAFETY_ASSERT((torque_mapped >= 0), AssertCode::TorqueLessThanZero);
 
-    return torque_mapped;
+    return static_cast<int16_t>(torque_mapped);
 }
 
 void Ecu::processMessage(uint32_t current_time_ms, CAN_message_t msg)
@@ -143,7 +159,7 @@ void Ecu::processMessage(uint32_t current_time_ms, CAN_message_t msg)
         // FIXME this isn't working
         /* Brake and throttle cannot be pressed at the same time. */
         // SAFETY_ASSERT_CODE(!(mapped_torque > 0 && this->brake_pressure >= BRAKE_CONSIDERED_PRESSED), AssertCode::BrakeAndThrottle);
-        int16_t smoothed_torque = smooth_torque(current_time_ms, mapped_torque);
+        int16_t smoothed_torque = this->smoothTorque(current_time_ms, mapped_torque);
         this->calculated_torque = smoothed_torque;
     }
 
@@ -200,15 +216,14 @@ void Ecu::processMessage(uint32_t current_time_ms, CAN_message_t msg)
          * Once these preconditions are met, we can do the startup sequence.
          * We will also wait two seconds before fully starting up, or abort if
          * one of the preconditions stops holding. */
-
-        /* DEBUG ONLY */
-        // TODO this needs to be looked over 
-        // if ((this->brake_pressure >= BRAKE_CONSIDERED_PRESSED) && this->start_switch_on) {
-        if (this->start_switch_on) {
+        if ((this->brake_pressure >= BRAKE_CONSIDERED_PRESSED) && debounced_switch)
+        {
             if (!this->startup_countdown.started())
             {
                 this->startup_countdown.start(current_time_ms, STARTUP_DELAY_MS);
-            } else if (this->startup_countdown.triggerReached(current_time_ms)) {
+            }
+            else if (this->startup_countdown.triggerReached(current_time_ms))
+            {
                 /* Good to go! */
                 this->car_fully_on = true;
             }
@@ -248,16 +263,8 @@ std::optional<CAN_message_t> Ecu::emitMessage(uint32_t current_time_ms)
         torque_to_use = this->calculated_torque;
     }
 
-    /* We need to pace how often motor control messages are passed along the CAN bus.
-     * We do this by using a trigger. To use the trigger, we first start it. Once
-     * enough time has passed since we started it, `triggerReached` will return true,
-     * and mark the trigger as not started. Next time, we'll see that the trigger is
-     * marked as not started, and we'll start it again. This loops forever. */
-    if (!this->motor_control_message_pacing.started())
-    {
-        this->motor_control_message_pacing.start(current_time_ms, 15 /*ms*/);
-    }
-    else if (this->motor_control_message_pacing.triggerReached(current_time_ms))
+    /* Pace how often we send a motor command by pacing it with a timer. */
+    if (this->motor_control_pacing.shouldFire(current_time_ms))
     {
         MotorControlCommand cmd;
         cmd.torque = torque_to_use;
@@ -303,5 +310,4 @@ void Ecu::printState()
     }
 
     PRINTF("startup_countdown: %s\n", this->startup_countdown.started() ? "started" : "not started");
-    PRINTF("motor_control_message_pacing: %s\n", this->motor_control_message_pacing.started() ? "started" : "not started");
 }

ECU_V2/lib/common/ecu.hpp

@@ -17,7 +17,7 @@ class Ecu {
     std::optional<CAN_message_t> emitMessage(uint32_t current_time_ms);
 
     /* smooth torque */
-    int16_t smooth_torque(uint16_t current_time, uint16_t torque);
+    int16_t smoothTorque(uint32_t current_time, int16_t torque);
 
     /* Uses PRINTF for all printing. */
     void printState();
@@ -58,8 +58,8 @@ class Ecu {
      * wait 2 seconds before enabling the motor. This is that startup countdown. */
     Trigger startup_countdown;
 
-    /* Time since we last sent a motor control command. */
-    Trigger motor_control_message_pacing;
+    /* Timer to pace how often we send motor control commands. */
+    Timer motor_control_pacing = Timer(0, 15);
 
     /* throttle mapping memory */
     uint16_t torque_memory[4] = {0, 0, 0, 0};

ECU_V2/lib/common/util.cpp

@@ -31,14 +31,41 @@ void Trigger::cancel() {
     started_at = std::nullopt;
 }
 
+
+/* Timer */
+Timer::Timer(uint32_t current_time, uint32_t duration_ms) {
+    this->last_fired_at = current_time;
+    this->duration_ms = duration_ms;
+}
+
+/* Important: this will reset the timer when this returns true. */
+bool Timer::shouldFire(uint32_t current_time) {
+    if (current_time - this->last_fired_at >= this->duration_ms) {
+        this->last_fired_at = current_time;
+        return true;
+    } else {
+        return false;
+    }
+}
+
+/* Tells us how long until this timer will fire again, or std::nullopt if
+ * it's due to fire. */
+std::optional<uint32_t> Timer::timeUntilNextFiring(uint32_t current_time_ms) {
+    if (current_time_ms - this->last_fired_at < this->duration_ms) {
+        return this->duration_ms - (current_time_ms - this->last_fired_at);
+    } else {
+        return std::nullopt;
+    }
+}
+
 /* Used to condense the file name into something short enough we can send
  * along the CAN bus in the case of a fault. */
 uint32_t str_hash(const char *str) {
     /* djb2 hash. */
     uint32_t hash = 5381;
     uint32_t c;
 
-    while (c = *str++)
+    while ((c = *str++))
         hash = ((hash << 5) + hash) + c; /* hash * 33 + c */
 
     return hash;

ECU_V2/lib/common/util.hpp

@@ -16,6 +16,16 @@ class Trigger {
     uint32_t target_duration = 0;
 };
 
+class Timer {
+public:
+    Timer(uint32_t current_time, uint32_t frequency);
+    bool shouldFire(uint32_t current_time);
+    std::optional<uint32_t> timeUntilNextFiring(uint32_t current_time_ms);
+private:
+    uint32_t last_fired_at = 0;
+    uint32_t duration_ms = 0;
+};
+
 #ifdef BUILD_MODE_TEST
 #include <cstdio>
 #define PRINTF(...) printf(__VA_ARGS__)
@@ -24,4 +34,6 @@ class Trigger {
 #define PRINTF(...) Serial.printf(__VA_ARGS__)
 #endif
 
+#define UNUSED(x) ((void)x)
+
 uint32_t str_hash(const char *str);

ECU_V2/platformio.ini

@@ -8,18 +8,28 @@
 ; Please visit documentation for the other options and examples
 ; https://docs.platformio.org/page/projectconf.html
 
-[env:teensy41]
+[env:teensy41_debug]
 platform = teensy
+build_type = debug
+build_flags = -DLOG_FILE_PATH="logs/log_${date}_${time}.log" -DENABLE_DEBUGGING -Wall -Wunused-parameter
 board = teensy41
 framework = arduino
 monitor_filters = time, log2file
+lib_deps = https://github.com/tonton81/FlexCAN_T4
+
+[env:teensy41_production]
+platform = teensy
+build_type = release
 monitor_speed = 115200
-build_flags = -DLOG_FILE_PATH="logs/log_${date}_${time}.log" -DENABLE_DEBUGGING
+build_flags = -DLOG_FILE_PATH="logs/log_${date}_${time}.log" -Wall -Wunused-parameter
+board = teensy41
+framework = arduino
+monitor_filters = time, log2file
 lib_deps = https://github.com/tonton81/FlexCAN_T4
 
-[env:native]
+[env:native_tests]
 platform = native
 test_framework = unity
 lib_compat_mode = off
 # If we're building it for a native target (e.g. local computer), it means we're building it to test it.
-build_flags = -DBUILD_MODE_TEST -DENABLE_DEBUGGING -g3 -Wall
+build_flags = -DBUILD_MODE_TEST -DENABLE_DEBUGGING -fsanitize=address -fsanitize=undefined -g3 -Wall -Wextra -Wpedantic

ECU_V2/test/test_can_serde/main.cpp

@@ -8,6 +8,7 @@
 
 #include "assert.hpp"
 #include "can_serde.hpp"
+#include "util.hpp"
 
 void setUp(void) {
     // set stuff up here
@@ -17,11 +18,13 @@ void tearDown(void) {
     // clean stuff up here
 }
 
-void safety_assert_failed_handler(AssertLevel level, const char *file, int line, AssertCode error_code) {
+void assert_failed_handler(AssertLevel level, const char *file, int line, AssertCode error_code) {
+    UNUSED(level);
+
     /* If an SAFETY_ASSERT fails somewhere in the code, this will let the testing environment
      * know that we failed the test. */
     std::stringstream fail_msg;
-    fail_msg << "Assert failed at " << file << ":" << line;
+    fail_msg << "Assert failed at " << file << ":" << line << " with code " << static_cast<uint8_t>(error_code);
     TEST_FAIL_MESSAGE(fail_msg.str().c_str());
 }
 
@@ -64,7 +67,7 @@ void test_can_dump_parsing() {
         iss >> msg_num >> time_offset >> type >> id_hex >> direction >> len;
 
         /* Construct the message that we'll be using. */
-        CAN_message_t msg = {0};
+        CAN_message_t msg = {};
         msg.id = std::stoul(id_hex, nullptr, 16); /* Hex -> number. */
         msg.len = len;
 
@@ -189,8 +192,8 @@ void test_inverter_creation_message() {
     TEST_ASSERT(created.buf[7] == 0x00);
 }
 
-int main(int argc, char **argv) {
-    register_assert_failed_handler(safety_assert_failed_handler);
+int main() {
+    register_assert_failed_handler(assert_failed_handler);
 
     UNITY_BEGIN();
     RUN_TEST(test_start_switch);