Added IR edge and ultrasonic self-tests to Pico A.

Source Code/pico_ws/libfreertos/Config/FreeRTOSConfig.h

@@ -105,7 +105,7 @@
 #define configUSE_TIMERS                        1
 #define configTIMER_TASK_PRIORITY               (configMAX_PRIORITIES - 1)
 #define configTIMER_QUEUE_LENGTH                10
-#define configTIMER_TASK_STACK_DEPTH            2048
+#define configTIMER_TASK_STACK_DEPTH            1024
 //#define configTIMER_SERVICE_TASK_CORE_AFFINITY  (1 << 1)
 
 

Source Code/pico_ws/src/lib/FreeRTOS_Helpers_lib/src/uROS_Bridge_Agent.c++

@@ -337,7 +337,7 @@ void uRosBridgeAgent::execute()
 
                 if (current_uros_state == AGENT_CONNECTED) 
                 {
-                    check_bool(check_exec_interval(last_exec_time, MAX_EXEC_TIME + EXECUTE_DELAY_MS, "Executor execution time exceeded limits!"), RT_SOFT_CHECK);
+                    check_exec_interval(last_exec_time, MAX_EXEC_TIME + EXECUTE_DELAY_MS, "Executor execution time exceeded limits!", true);
                     check_rc(rclc_executor_spin_some(&rc_executor, RCL_MS_TO_NS(EXECUTOR_TIMEOUT_MS)), RT_LOG_ONLY_CHECK);
                     vTaskDelay(EXECUTE_DELAY_MS / portTICK_PERIOD_MS);
                 }

Source Code/pico_ws/src/lib/Local_Helpers_lib/include/local_helpers_lib/Common_Definitions.h

@@ -52,4 +52,4 @@
 #define uros_agent_find_attempts    10
 
 // ---- Misc. typedefs ----
-typedef std::vector<diagnostic_msgs__msg__KeyValue> DIAG_KV_EMPTY;
+typedef std::vector<diagnostic_msgs__msg__KeyValue> DIAG_KV_PAIR_VEC;

Source Code/pico_ws/src/lib/Local_Helpers_lib/include/local_helpers_lib/Common_Diag_Msgs.h

@@ -25,13 +25,13 @@
 // ---- STATUS OK ----
 
 // ---- STATUS WARM ----
-#define DIAG_WARN_MSG_TIMER_EXEC_TIME_OVER  "One of the timers' execution intervals exceeded limits"
 #define DIAG_WARN_MSG_UROS_RC_CHECK_FAIL    "MicroROS RCL return checker fail (Soft Fail)"
 #define DIAG_WARN_MSG_BOOL_RT_CHECK_FAIL    "Boolean function return checker fail (Soft Fail)"
+#define DIAG_WARN_MSG_TIMER_EXEC_TIME_OVER  "One of the timers' execution intervals exceeded limits"
 
 // ---- STATUS ERROR ----
 #define DIAG_ERR_MSG_UROS_RC_CHECK_FAIL  "MicroROS RCL return checker fail (Hard Fail)"
-#define DIAG_ERR_MSG_INIT_FAILED         "Init. failed"
 #define DIAG_ERR_MSG_BOOL_RT_CHECK_FAIL  "Boolean function return checker fail (Hard Fail)"
+#define DIAG_ERR_MSG_INIT_FAILED         "Init. failed"
 
 // ---- STATUS STALE ----

Source Code/pico_ws/src/lib/Local_Helpers_lib/include/local_helpers_lib/Diagnostics_Definitons.h

@@ -23,53 +23,59 @@
 
 
 // ------- Diagnostics Hardware Names -------
-#define DIAG_HWNAME_MOTOR_CTRL_L   "motor_control_left"
-#define DIAG_HWNAME_MOTOR_CTRL_R   "motor_control_right"
-#define DIAG_HWNAME_ULTRASONICS    "ultrasonic_sensors"
-#define DIAG_HWNAME_ENV_SENSORS    "env_sensors"
-#define DIAG_HWNAME_IR_EDGE_FRONT  "ir_edge_sensors_front"
-#define DIAG_HWNAME_IR_EDGE_BACK   "ir_edge_sensors_back"
-#define DIAG_HWNAME_BATT_MON       "battery_monitoring"
-#define DIAG_HWNAME_SYS_PWR_MGMT   "system_power_management"
-#define DIAG_HWNAME_USER_INPUTS    "user_inputs"
-#define DIAG_HWNAME_UCONTROLLERS   "microcontrollers"
-#define DIAG_HWNAME_UROS           "microros"
+#define DIAG_NAME_MOTOR          "motors"
+#define DIAG_NAME_ULTRASONICS    "ultrasonic_sensors"
+#define DIAG_NAME_ENV_SENSORS    "env_sensors"
+#define DIAG_NAME_IR_EDGE        "ir_edge_sensors"
+#define DIAG_NAME_BATT_MON       "battery_monitoring"
+#define DIAG_NAME_SYS_PWR_MGMT   "power_management"
+#define DIAG_NAME_SYSTEM         "system"   // Firmware-related diagnostics
+#define DIAG_NAME_MCU            "mcus"
 
 
 // ------- Diagnostics Hardware IDs -------
-#define DIAG_HWID_MOTOR_ENC_L1    "motor_encoder_left_1"
-#define DIAG_HWID_MOTOR_ENC_L2    "motor_encoder_left_2"
-#define DIAG_HWID_MOTOR_ENC_R1    "motor_encoder_right_1"
-#define DIAG_HWID_MOTOR_ENC_R2    "motor_encoder_right_2"
-#define DIAG_HWID_MOTOR_DRV_L     "motor_driver_left"
-#define DIAG_HWID_MOTOR_DRV_R     "motor_driver_right"
-#define DIAG_HWID_ULTRASONIC_F    "ultrasonic_front"
-#define DIAG_HWID_ULTRASONIC_B    "ultrasonic_back"
-#define DIAG_HWID_ULTRASONIC_R    "ultrasonic_right"
-#define DIAG_HWID_ULTRASONIC_L    "ultrasonic_left"
-#define DIAG_HWID_ENV_DHT11       "env_sens_dht11"
-#define DIAG_HWID_ENV_IMU         "env_sens_imu"
-#define DIAG_HWID_IR_EDGE_F1      "ir_edge_front_sens_1"
-#define DIAG_HWID_IR_EDGE_F2      "ir_edge_front_sens_2"
-#define DIAG_HWID_IR_EDGE_F3      "ir_edge_front_sens_3"
-#define DIAG_HWID_IR_EDGE_F4      "ir_edge_front_sens_4"
-#define DIAG_HWID_IR_EDGE_B1      "ir_edge_back_sens_1"
-#define DIAG_HWID_IR_EDGE_B2      "ir_edge_back_sens_2"
-#define DIAG_HWID_IR_EDGE_B3      "ir_edge_back_sens_3"
-#define DIAG_HWID_IR_EDGE_B4      "ir_edge_back_sens_4"
-#define DIAG_HWID_IR_EDGE_FADC    "ir_edge_front_adc"
-#define DIAG_HWID_IR_EDGE_BADC    "ir_edge_back_adc"
-#define DIAG_HWID_BATT_PWR_MON    "batt_power_monitor"
-#define DIAG_HWID_BATT_TEMP_MON   "batt_temp_monitor"
-#define DIAG_HWID_SYSPWR_12V_MON  "sys_power_12v_bus_monitor"
-#define DIAG_HWID_SYSPWR_5V_MON   "sys_power_5v_bus_monitor"
-#define DIAG_HWID_SYSPWR_3v3_MON  "sys_power_3v3_bus_monitor"
-#define DIAG_HWID_SYSPWR_12V_REG  "sys_power_12v_regulator"
-#define DIAG_HWID_SYSPWR_5V_REG   "sys_power_5v_regulator"
-#define DIAG_HWID_SYSPWR_3v3_REG  "sys_power_3v3_regulator"
-#define DIAG_HWID_UI_NEXTION      "user_interface_nextion"
-#define DIAG_HWID_MCU_MABO_A      "mcu_mainboard_rp2040_a"
-#define DIAG_HWID_MCU_MABO_B      "mcu_mainboard_rp2040_b"
-#define DIAG_HWID_MCU_POBO        "mcu_powerboard_rp2040"
-#define DIAG_HWID_MCU_MOTBO       "mcu_motorboard_rp2040"
-#define DIAG_HWID_UROS            "microros"
+
+// ---- Motors ----
+#define DIAG_ID_MOTOR_DRV_L     "motor_driver_left"
+#define DIAG_ID_MOTOR_DRV_R     "motor_driver_right"
+#define DIAG_ID_MOTOR_CTRL_L    "motor_controller_left"
+#define DIAG_ID_MOTOR_CTRL_R    "motor_controller_right"
+
+// ---- Ultrasonic Sensors ----
+#define DIAG_ID_ULTRASONIC_F    "ultrasonic_front"
+#define DIAG_ID_ULTRASONIC_B    "ultrasonic_back"
+#define DIAG_ID_ULTRASONIC_R    "ultrasonic_right"
+#define DIAG_ID_ULTRASONIC_L    "ultrasonic_left"
+
+// ---- Environmental Sensors ----
+#define DIAG_ID_ENV_DHT11       "env_sens_dht11"
+#define DIAG_ID_ENV_IMU         "env_sens_imu"
+
+// ---- IR Edge Sensors ----
+#define DIAG_ID_IR_EDGE_F       "ir_edge_front"
+#define DIAG_ID_IR_EDGE_B       "ir_edge_back"
+#define DIAG_ID_IR_EDGE_FADC    "ir_edge_front_adc"
+#define DIAG_ID_IR_EDGE_BADC    "ir_edge_back_adc"
+
+// ---- Battery Monitoring ----
+#define DIAG_ID_BATT_PWR_MON    "batt_power_monitor"
+#define DIAG_ID_BATT_TEMP_MON   "batt_temp_monitor"
+
+// ---- Power Management ----
+#define DIAG_ID_SYSPWR_12V_MON  "sys_power_12v_bus_monitor"
+#define DIAG_ID_SYSPWR_5V_MON   "sys_power_5v_bus_monitor"
+#define DIAG_ID_SYSPWR_3v3_MON  "sys_power_3v3_bus_monitor"
+#define DIAG_ID_SYSPWR_12V_REG  "sys_power_12v_regulator"
+#define DIAG_ID_SYSPWR_5V_REG   "sys_power_5v_regulator"
+#define DIAG_ID_SYSPWR_3v3_REG  "sys_power_3v3_regulator"
+
+// ---- System ----
+#define DIAG_ID_SYS_GENERAL     "system_general"
+#define DIAG_ID_SYS_TIMERS      "system_timers"
+#define DIAG_ID_SYS_UROS        "microros"
+
+// ---- MCUs ----
+#define DIAG_ID_MCU_MABO_A      "mcu_mainboard_rp2040_a"
+#define DIAG_ID_MCU_MABO_B      "mcu_mainboard_rp2040_b"
+#define DIAG_ID_MCU_POBO        "mcu_powerboard_rp2040"
+#define DIAG_ID_MCU_MOTBO       "mcu_motorboard_rp2040"

Source Code/pico_ws/src/lib/Local_Helpers_lib/include/local_helpers_lib/Local_Helpers.h

@@ -56,6 +56,9 @@ bool check_bool(bool function, RT_CHECK_MODE mode, const char *func=__builtin_FU
 // ---- Set MicroROS publishing queue ----
 void set_diag_pub_queue(QueueHandle_t queue);
 
+// ---- Create a diagnostics key-value pair ----
+diagnostic_msgs__msg__KeyValue create_diag_kv_pair(std::string key, std::string value);
+
 // ---- Create a diagnostic status message ----
 diagnostic_msgs__msg__DiagnosticStatus create_diag_msg(uint8_t level, std::string hw_name, std::string hw_id, std::string msg, std::vector<diagnostic_msgs__msg__KeyValue> key_values);
 
@@ -89,4 +92,4 @@ bool ping_agent();
 // ---- Execution interval checker ----
 // ---- Checks the amount of time passed since the last time it was called (with the specific time storage varialble provided) ----
 // ---- Returns false if the execution time has exceeded the specified limit ----
-bool check_exec_interval(uint32_t &last_call_time_ms, uint16_t max_exec_time_ms, std::string log_msg, const char *func=__builtin_FUNCTION());
+bool check_exec_interval(uint32_t &last_call_time_ms, uint16_t max_exec_time_ms, std::string log_msg, bool publish_diag = false, const char *func=__builtin_FUNCTION());

Source Code/pico_ws/src/lib/Local_Helpers_lib/src/Local_Helpers.c++

@@ -52,25 +52,25 @@ bool check_rc(rcl_ret_t rctc, RT_CHECK_MODE mode, const char *func, uint16_t lin
 {
     if (rctc != RCL_RET_OK)
     {
-        char buffer[60];
+        char buffer[70];
 
         switch (mode)
         {
             case RT_HARD_CHECK:
-                sprintf(buffer, "RCL Return check failed: [code: %d, RT_HARD_CHECK]", rctc);
+                snprintf(buffer, sizeof(buffer), "RCL Return check failed: [code: %d, RT_HARD_CHECK]", rctc);
                 write_log(buffer, LOG_LVL_FATAL, FUNCNAME_LINE_ONLY, func, "", line);
-                publish_diag_report(DIAG_LVL_ERROR, DIAG_HWNAME_UROS, DIAG_HWID_UROS, DIAG_ERR_MSG_UROS_RC_CHECK_FAIL, DIAG_KV_EMPTY());
+                publish_diag_report(DIAG_LVL_ERROR, DIAG_NAME_SYSTEM, DIAG_ID_SYS_UROS, DIAG_ERR_MSG_UROS_RC_CHECK_FAIL, DIAG_KV_PAIR_VEC());
                 clean_shutdown();
                 break;
 
             case RT_SOFT_CHECK:
-                sprintf(buffer, "RCL Return check failed: [code: %d, RT_SOFT_CHECK]", rctc);
+                snprintf(buffer, sizeof(buffer), "RCL Return check failed: [code: %d, RT_SOFT_CHECK]", rctc);
                 write_log(buffer, LOG_LVL_ERROR, FUNCNAME_LINE_ONLY, func, "", line);
-                publish_diag_report(DIAG_LVL_WARN, DIAG_HWNAME_UROS, DIAG_HWID_UROS, DIAG_WARN_MSG_UROS_RC_CHECK_FAIL, DIAG_KV_EMPTY());
+                publish_diag_report(DIAG_LVL_WARN, DIAG_NAME_SYSTEM, DIAG_ID_SYS_UROS, DIAG_WARN_MSG_UROS_RC_CHECK_FAIL, DIAG_KV_PAIR_VEC());
                 break;
 
             case RT_LOG_ONLY_CHECK:
-                sprintf(buffer, "RCL Return check failed: [code: %d, RT_LOG_ONLY_CHECK]", rctc);
+                snprintf(buffer, sizeof(buffer), "RCL Return check failed: [code: %d, RT_LOG_ONLY_CHECK]", rctc);
                 write_log(buffer, LOG_LVL_WARN, FUNCNAME_LINE_ONLY, func, "", line);
                 break;
 
@@ -92,13 +92,13 @@ bool check_bool(bool function, RT_CHECK_MODE mode, const char *func, uint16_t li
         {
             case RT_HARD_CHECK:
                 write_log("BOOL Return check failed: [RT_HARD_CHECK]", LOG_LVL_FATAL, FUNCNAME_LINE_ONLY, func, "", line);
-                publish_diag_report(DIAG_LVL_ERROR, DIAG_HWNAME_UCONTROLLERS, DIAG_HWID_MCU_MABO_A, DIAG_ERR_MSG_BOOL_RT_CHECK_FAIL, DIAG_KV_EMPTY());
+                publish_diag_report(DIAG_LVL_ERROR, DIAG_NAME_SYSTEM, DIAG_ID_SYS_GENERAL, DIAG_ERR_MSG_BOOL_RT_CHECK_FAIL, DIAG_KV_PAIR_VEC());
                 clean_shutdown();
                 break;
 
             case RT_SOFT_CHECK:
                 write_log("BOOL Return check failed: [RT_SOFT_CHECK]", LOG_LVL_ERROR, FUNCNAME_LINE_ONLY, func, "", line);
-                publish_diag_report(DIAG_LVL_WARN, DIAG_HWNAME_UCONTROLLERS, DIAG_HWID_MCU_MABO_A, DIAG_WARN_MSG_BOOL_RT_CHECK_FAIL, DIAG_KV_EMPTY());
+                publish_diag_report(DIAG_LVL_WARN, DIAG_NAME_SYSTEM, DIAG_ID_SYS_GENERAL, DIAG_WARN_MSG_BOOL_RT_CHECK_FAIL, DIAG_KV_PAIR_VEC());
                 break;
 
             case RT_LOG_ONLY_CHECK:
@@ -121,6 +121,20 @@ void set_diag_pub_queue(QueueHandle_t queue)
 }
 
 
+// ---- Create a diagnostics key-value pair ----
+diagnostic_msgs__msg__KeyValue create_diag_kv_pair(std::string key, std::string value)
+{
+    diagnostic_msgs__msg__KeyValue kv_pair;
+
+    kv_pair.key.data = key.data();
+    kv_pair.key.size = strlen(kv_pair.key.data);
+    kv_pair.value.data = value.data();
+    kv_pair.value.size = strlen(kv_pair.value.data);
+
+    return kv_pair;
+}
+
+
 // ---- Create a diagnostic status message ----
 diagnostic_msgs__msg__DiagnosticStatus create_diag_msg(uint8_t level, std::string hw_name, std::string hw_id, std::string msg, std::vector<diagnostic_msgs__msg__KeyValue> key_values)
 {
@@ -279,7 +293,7 @@ bool ping_agent()
 // ---- Execution interval checker ----
 // ---- Checks the amount of time passed since the last time it was called (with the specific time storage varialble provided) ----
 // ---- Returns false if the execution time has exceeded the specified limit ----
-bool check_exec_interval(uint32_t &last_call_time_ms, uint16_t max_exec_time_ms, std::string log_msg, const char *func)
+bool check_exec_interval(uint32_t &last_call_time_ms, uint16_t max_exec_time_ms, std::string log_msg, bool publish_diag, const char *func)
 {
     // Initialize last_call_time_ms if it's 0 (first call).
     if (last_call_time_ms == 0) 
@@ -296,6 +310,16 @@ bool check_exec_interval(uint32_t &last_call_time_ms, uint16_t max_exec_time_ms,
         // This is also quite ugly, but it also works.
         log_msg = log_msg + " [act: " + std::to_string(exec_time_ms) + "ms, lim: " + std::to_string(max_exec_time_ms) + "ms]";
         write_log(log_msg, LOG_LVL_WARN, FUNCNAME_ONLY, func);
+
+        if (publish_diag)
+        {
+            std::string report_str = log_msg + " [func: " + func + "]";
+            DIAG_KV_PAIR_VEC kv_pairs;
+            kv_pairs.push_back(create_diag_kv_pair("actual_time", std::to_string(exec_time_ms) + "ms"));
+            kv_pairs.push_back(create_diag_kv_pair("time_limit", std::to_string(max_exec_time_ms) + "ms"));
+            publish_diag_report(DIAG_LVL_WARN, DIAG_NAME_SYSTEM, DIAG_ID_SYS_TIMERS, report_str, kv_pairs);
+        }
+
         return false;
     }
 

Source Code/pico_ws/src/pico_a/Pico_A.c++

@@ -34,7 +34,6 @@
 // ------- Global variables -------
 
 // ---- Misc. ----
-bool self_test_mode = false;
 alarm_pool_t *core_1_alarm_pool;
 
 // ---- Timer execution times storage (milliseconds) ----
@@ -292,8 +291,8 @@ void set_mtr_pid_tunings_callback(const void *req, void *res)
     rrp_pico_coms__srv__SetPidTunings_Request *req_in = (rrp_pico_coms__srv__SetPidTunings_Request *) req;
     rrp_pico_coms__srv__SetPidTunings_Response *res_in = (rrp_pico_coms__srv__SetPidTunings_Response *) res;
 
-    char buffer[65];
-    sprintf(buffer, "Received set_pid_tunings: [Kp: %.3f, Ki: %.3f, Kd: %.3f]", req_in->pid_kp, req_in->pid_ki, req_in->pid_kd);
+    char buffer[70];
+    snprintf(buffer, sizeof(buffer), "Received set_pid_tunings: [Kp: %.3f, Ki: %.3f, Kd: %.3f]", req_in->pid_kp, req_in->pid_ki, req_in->pid_kd);
     write_log(buffer, LOG_LVL_INFO, FUNCNAME_ONLY);
 
     r_motors.pid->SetTunings(req_in->pid_kp, req_in->pid_ki, req_in->pid_kd);
@@ -335,7 +334,7 @@ void cmd_vel_call(const void *msgin)
     const geometry_msgs__msg__Twist *msg = (const geometry_msgs__msg__Twist *) msgin;
 
     char buffer[60];
-    sprintf(buffer, "Received cmd_vel: [lin: %.2fm/s, ang: %.2frad/s]", msg->linear.x, msg->angular.z);
+    snprintf(buffer, sizeof(buffer), "Received cmd_vel: [lin: %.2fm/s, ang: %.2frad/s]", msg->linear.x, msg->angular.z);
     write_log(buffer, LOG_LVL_INFO, FUNCNAME_ONLY);
 
     float angular = msg->angular.z;     // rad/s
@@ -350,7 +349,7 @@ void cmd_vel_call(const void *msgin)
     r_motors.set_pid_ctrl_speed(abs(motor_r_speed_rpm));
     l_motors.set_pid_ctrl_speed(abs(motor_l_speed_rpm)); 
     r_motors.set_motor_direction((motor_r_speed_rpm > 0) ? Motor::FORWARD : Motor::BACKWARD);
-    l_motors.set_motor_direction((motor_l_speed_rpm > 0) ? Motor::FORWARD : Motor::BACKWARD);         
+    l_motors.set_motor_direction((motor_l_speed_rpm > 0) ? Motor::FORWARD : Motor::BACKWARD);      
 }
 
 
@@ -489,7 +488,7 @@ void motor_ctrl_odom_timer_call(void *parameters)
         xTaskNotifyWait(0, 0, NULL, portMAX_DELAY);   // Wait for notification indefinitely
 
         // Check execution time
-        check_bool(check_exec_interval(last_motor_odom_time, (motor_odom_rt_interval + 50), "Execution interval exceeded limits!"), RT_SOFT_CHECK);
+        check_exec_interval(last_motor_odom_time, (motor_odom_rt_interval + 50), "Execution interval exceeded limits!", true);
 
         // Calculate and set motor outputs
         r_motors.compute_outputs();
@@ -551,7 +550,7 @@ bool init_mpu6050()
     }
 
     write_log("MPU init failed.", LOG_LVL_WARN, FUNCNAME_ONLY);
-    publish_diag_report(DIAG_LVL_ERROR, DIAG_HWNAME_ENV_SENSORS, DIAG_HWID_ENV_IMU, DIAG_ERR_MSG_INIT_FAILED, DIAG_KV_EMPTY());
+    publish_diag_report(DIAG_LVL_ERROR, DIAG_HWNAME_ENV_SENSORS, DIAG_HWID_ENV_IMU, DIAG_ERR_MSG_INIT_FAILED, DIAG_KV_PAIR_VEC());
     return false;*/
 
     return true;

Source Code/pico_ws/src/pico_a/helpers/Definitions.h

@@ -72,8 +72,8 @@
 // ------- Other definitions -------
 
 // ---- MicroROS node config ----
-#define UROS_NODE_NAME              "pico_a"
-#define UROS_NODE_NAMESPACE         "io"
+#define UROS_NODE_NAME       "pico_a"
+#define UROS_NODE_NAMESPACE  "io"
 
 // ---- Repeating timer intervals ----
 #define motor_odom_rt_interval          100   // In milliseconds
@@ -87,6 +87,9 @@
 #define ultra_min_dist               1         // In cm
 #define ultra_max_dist               400       // In cm
 #define ultrasonic_signal_timout_us  14*1000   // 32 milliseconds
+#define ultra_selftest_measurements  5
+#define ultra_selftest_range_min_cm  18        // In cm
+#define ultra_selftest_range_max_cm  22        // In cm
 
 // ---- IR edge sensors ----
 #define ir_edge_detection_range          3   // In cm

Source Code/pico_ws/src/pico_a/helpers/Diag_Msgs.h

@@ -21,12 +21,16 @@
 
 
 // ---- STATUS OK ----
+#define DIAG_OK_MSG_IR_EDGE_TEST_PASS  "IR edge sensor self-test passed! [SENSOR: %d]"
+#define DIAG_OK_MSG_ULTRA_TEST_PASS    "Ultrasonic sensor self-test passed! [CODE: %d]"
 
 // ---- STATUS WARM ----
 #define DIAG_WARN_MSG_ULTRA_MIN_LIM_EXCEED  "Ultrasonic sensor distance reading exceeded ultra_min_dist"
 #define DIAG_WARN_MSG_ULTRA_MAX_LIM_EXCEED  "Ultrasonic sensor distance reading exceeded ultra_max_dist"
 
 // ---- STATUS ERROR ----
-#define DIAG_ERR_MSG_MOTOR_SAFETY  "A MotorSafety condition was triggered"
+#define DIAG_ERR_MSG_MOTOR_SAFETY       "A MotorSafety condition was triggered"
+#define DIAG_ERR_MSG_IR_EDGE_TEST_FAIL  "IR edge sensor self-test failed! [SENSOR: %d]"
+#define DIAG_ERR_MSG_ULTRA_TEST_FAIL    "Ultrasonic sensor self-test failed! [CODE: %d]"
 
 // ---- STATUS STALE ----