HeartBeat System & Threads Performance

README.md

@@ -48,12 +48,10 @@ This project implements a real-time speed sensor and CAN communication system on
  - I2C3 SDA  -> PC_1
 
 # How to Extend The Project
- - Implement a mechanism to process incoming data and perform the appropriate actions.
  - Create a Heartbeat mechanism.
  - Integrate Watchdog timer that resets if the system "breaks".
  - Integration tests for CAN.
  - Latency tests
- - Redefine diferent prioritys for threads.
 
 ### Creating new Threads
  - In app_threadx.h, increase THREAD_COUNT by one, and remember to update the comments above that definition.

ThreadX_Os/CMakeLists.txt

@@ -44,20 +44,26 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
 # Add sources to executable
 target_sources(${CMAKE_PROJECT_NAME} PRIVATE
     # Add user sources here
+    #i2c_api
+    Core/Src/i2c_ina219.c
+    Core/Src/i2c_pca9685.c
+    #init
     Core/Src/init/init_can.c
     Core/Src/init/init_threads.c
+    #threads
+    #can
     Core/Src/threads/can/rx_can.c
     Core/Src/threads/can/tx_can.c
-    Core/Src/threads/motors/dc_motors.c
-    Core/Src/threads/motors/emergency_brake.c
-    Core/Src/threads/motors/servo.c
+    #motors
+    Core/Src/threads/motors/i2c_driving.c
+    #
     Core/Src/threads/battery.c
+    Core/Src/threads/emergency_brake.c
     Core/Src/threads/speed_sensor.c
+    #utils
     Core/Src/utils/debug_utils.c
     Core/Src/utils/i2c_utils.c
     Core/Src/utils/speed_rpm_utils.c
-    Core/Src/i2c_ina219.c
-    Core/Src/i2c_pca9685.c
 )
 
 # Add include paths

ThreadX_Os/Core/Inc/app_threadx.h

@@ -34,39 +34,38 @@ extern "C" {
 #include <string.h>
 #include <stdio.h>
 #include <stdint.h>
-#include <utils.h>
+#include <utils_testing.h>
 
 /* USER CODE END Includes */
 
 /* Exported types ------------------------------------------------------------*/
 /* USER CODE BEGIN ET */
 
 // Thread with max priority
-#define MAX_PRIO   0
+#define MAX_PRIO   1
 
 // Thread with medium priority
-#define MEDIUM_PRIO   5
+#define MEDIUM_PRIO   3
 
 // Thread with low priority
-#define LOW_PRIO   10
+#define LOW_PRIO   5
 
 // Thread with no priority (lowest)
-#define NONE_PRIO   15
+#define NONE_PRIO   7
 
 //Queue size (number of messages)
-#define QUEUE_SIZE      8
+#define QUEUE_SIZE      6
 
 /*
 Number of threads
 1 -> Speed sensor thread
 2 -> CAN TX thread
 3 -> CAN RX thread
 4 -> dc_motors thread
-5 -> servo thread
-6 -> battery thread
-7 -> emergency brake thread
+5 -> battery thread
+6 -> emergency brake thread
 */
-#define THREAD_COUNT    7
+#define THREAD_COUNT    6
 
 // Thread structure
 typedef struct s_threads {
@@ -77,7 +76,6 @@ typedef struct s_threads {
 // CAN frames structure
 typedef struct s_canFrames {
   FDCAN_TxHeaderTypeDef tx_header_speed;
-  FDCAN_TxHeaderTypeDef tx_header_heart_beat;
   FDCAN_TxHeaderTypeDef tx_header_battery;
 } t_canFrames;
 
@@ -91,9 +89,8 @@ extern TIM_HandleTypeDef    htim1;
 extern I2C_HandleTypeDef    hi2c3;
 
 extern TX_QUEUE             can_tx_queue;
-extern TX_QUEUE             can_emergency_brake_queue;
-extern TX_QUEUE             i2c_dc_motors_queue;
-extern TX_QUEUE             i2c_servo_queue;
+extern TX_QUEUE             i2c_driving_queue;
+extern TX_QUEUE             emergency_brake_queue;
 extern TX_MUTEX             i2c_mutex;
 extern t_threads            threads[THREAD_COUNT];
 /* USER CODE END EC */
@@ -112,6 +109,9 @@ extern t_threads            threads[THREAD_COUNT];
 /* Exported macro ------------------------------------------------------------*/
 /* USER CODE BEGIN EM */
 
+// Macro to convert milliseconds to ThreadX ticks
+#define TX_MS_TO_TICKS(ms)    ((ms) * TX_TIMER_TICKS_PER_SECOND / 1000)
+
 /* USER CODE END EM */
 
 /* Exported functions prototypes ---------------------------------------------*/
@@ -124,8 +124,7 @@ void MX_ThreadX_Init(void);
 VOID  thread_SensorSpeed(ULONG thread_input);
 VOID  thread_tx_can(ULONG thread_input);
 VOID  thread_rx_can(ULONG thread_input);
-VOID  thread_dc_motors(ULONG thread_input);
-VOID  thread_servo(ULONG thread_input);
+VOID  thread_driving_command(ULONG thread_input);
 VOID  thread_battery(ULONG thread_input);
 VOID  thread_emergency_brake(ULONG thread_input);
 
@@ -134,13 +133,6 @@ void  initCanFrames(t_canFrames *canFrames);
 UINT  init_threads(VOID);
 UINT  init_queue(VOID);
 
-//utils
-VOID                uart_send(const char *msg);
-VOID                uart_send_int(int32_t value);
-VOID                rpm_debug_print(ULONG rpm, 
-                        ULONG cr1_reg, ULONG cnt_reg);
-HAL_StatusTypeDef   i2c_scan_bus(VOID);
-
 /* USER CODE END EFP */
 
 /* USER CODE BEGIN 1 */

ThreadX_Os/Core/Inc/can_protocol.h

@@ -3,6 +3,8 @@
 
 #include "app_threadx.h"
 
+#define EMERGENCY_BRAKE 150
+
 // CAN message types
 typedef enum {
   CAN_MSG_SPEED,
@@ -22,10 +24,4 @@ typedef struct s_rx_can_message {
   uint8_t   len;
 } t_rx_can_msg;
 
-/* // Steering/throttle CAN message instructions
-typedef struct s_i2c_message {
-    int8_t steering;
-    int8_t throttle;
-} t_i2c_msg; */
-
 #endif

ThreadX_Os/Core/Inc/i2c_ina219.h

@@ -1,7 +1,7 @@
 #ifndef I2C_INA219_H
 # define I2C_INA219_H
 
-#include "app_threadx.h"
+#include "utils.h"
 
 #define INA219_ADDR  0x41
 

ThreadX_Os/Core/Inc/i2c_pca9685.h

@@ -1,7 +1,7 @@
 #ifndef I2C_PCA9685_H
 # define I2C_PCA9685_H
 
-#include "app_threadx.h"
+#include "utils.h"
 
 #define PCA9685_ADDR_SERVO  0x40
 #define PCA9685_ADDR_MOTOR  0x60

ThreadX_Os/Core/Inc/utils.h

@@ -1,34 +1,13 @@
 #ifndef UTILS_H_
  #define UTILS_H_
 
-#include <stdint.h>
-
-// Use ThreadX types if available, otherwise define our own
-#ifndef TX_API_H
-    typedef uint32_t    UINT;
-    typedef uint32_t    ULONG;
-    typedef void        VOID;
-#endif
-
-// Timer ticks per second definition
-#ifndef TX_TIMER_TICKS_PER_SECOND
-  #define TX_TIMER_TICKS_PER_SECOND    1000
-#endif
-
-//Maximum RPM value to prevent overflow
-#define MAX_RPM   10000
-
-//Pulses Per Revolution
-#define PPR       40
-
-// RPM calculation state, created for hardware abstraction and testability
-typedef struct s_rpm_state {
-    ULONG   last_time_ticks;
-    ULONG   last_count;
-    UINT    first_run;
-} t_rpm_state;
-
-UINT    convertValuesRPM(ULONG count, ULONG ticks, 
-    ULONG period, t_rpm_state *state);
+#include "app_threadx.h"
+
+HAL_StatusTypeDef   i2c_scan_bus(VOID);
+VOID                uart_send(const char *msg);
+VOID                uart_send_int(int32_t value);
+VOID                battery_debug_print(float voltage);
+VOID                rpm_debug_print(ULONG rpm, 
+                        ULONG cr1_reg, ULONG cnt_reg);
 
 #endif

ThreadX_Os/Core/Inc/utils_testing.h

@@ -0,0 +1,34 @@
+#ifndef UTILS_TESTING_H_
+ #define UTILS_TESTING_H_
+
+#include <stdint.h>
+
+// Use ThreadX types if available, otherwise define our own
+#ifndef TX_API_H
+    typedef uint32_t    UINT;
+    typedef uint32_t    ULONG;
+    typedef void        VOID;
+#endif
+
+// Timer ticks per second definition
+#ifndef TX_TIMER_TICKS_PER_SECOND
+  #define TX_TIMER_TICKS_PER_SECOND    1000
+#endif
+
+//Maximum RPM value to prevent overflow
+#define MAX_RPM   10000
+
+//Pulses Per Revolution
+#define PPR       40
+
+// RPM calculation state, created for hardware abstraction and testability
+typedef struct s_rpm_state {
+    ULONG   last_time_ticks;
+    ULONG   last_count;
+    UINT    first_run;
+} t_rpm_state;
+
+UINT    convertValuesRPM(ULONG count, ULONG ticks, 
+    ULONG period, t_rpm_state *state);
+
+#endif

ThreadX_Os/Core/Src/app_threadx.c

@@ -46,9 +46,8 @@
 /* Private variables ---------------------------------------------------------*/
 /* USER CODE BEGIN PV */
 TX_QUEUE    can_tx_queue;
-TX_QUEUE    can_emergency_brake_queue;
-TX_QUEUE    i2c_dc_motors_queue;
-TX_QUEUE    i2c_servo_queue;
+TX_QUEUE    i2c_driving_queue;
+TX_QUEUE    emergency_brake_queue;
 TX_MUTEX    i2c_mutex;
 t_threads   threads[THREAD_COUNT];
 
@@ -79,30 +78,21 @@ UINT App_ThreadX_Init(VOID *memory_ptr)
   if (ret != TX_SUCCESS)
     uart_send("ERROR! Failed TX queue creation.\r\n");
 
-  // Create Emergency Brake queue
-  UCHAR *can_emergency_brake_queue_memory = (UCHAR *)memory_ptr + QUEUE_SIZE * sizeof(t_tx_can_msg);
-  ret = tx_queue_create(&can_emergency_brake_queue, "CAN RX Queue", 
-                        sizeof(t_rx_can_msg) / sizeof(ULONG),
-                        can_emergency_brake_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
-  if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed Emergency Brake queue creation.\r\n");
-
   // Create I2C DC Motors queue
-  UCHAR *i2c_motors_queue_memory = can_emergency_brake_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
-  ret = tx_queue_create(&i2c_dc_motors_queue, "I2C DC Motors Queue", 
+  UCHAR *i2c_motors_queue_memory = (UCHAR *)memory_ptr + QUEUE_SIZE * sizeof(t_tx_can_msg);
+  ret = tx_queue_create(&i2c_driving_queue, "I2C Driving Command Queue", 
                         sizeof(t_rx_can_msg) / sizeof(ULONG),
                         i2c_motors_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
   if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed I2C DC Motors queue creation.\r\n");
+    uart_send("ERROR! Failed I2C Driving Command queue creation.\r\n");
 
-  // Create I2C Servo queue
-  UCHAR *i2c_servo_queue_memory = i2c_motors_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
-  ret = tx_queue_create(&i2c_servo_queue, "I2C Servo Queue", 
-                        sizeof(t_rx_can_msg) / sizeof(ULONG),
-                        i2c_servo_queue_memory, QUEUE_SIZE * sizeof(t_rx_can_msg));
+  // Create Emergency brake queue
+  UCHAR *emergency_brake_queue_memory = i2c_motors_queue_memory + QUEUE_SIZE * sizeof(t_rx_can_msg);
+  ret = tx_queue_create(&emergency_brake_queue, "emergency brake Queue", 
+                        sizeof(t_tx_can_msg) / sizeof(ULONG),
+                        emergency_brake_queue_memory, QUEUE_SIZE * sizeof(t_tx_can_msg));
   if (ret != TX_SUCCESS)
-    uart_send("ERROR! Failed I2C Servo queue creation.\r\n");
-
+    uart_send("ERROR! Failed Emergency brake queue creation.\r\n");
   if (init_threads() != TX_SUCCESS)
     exit(EXIT_FAILURE);
   else

ThreadX_Os/Core/Src/i2c_ina219.c → ThreadX_Os/Core/Src/i2c_api/i2c_ina219.c

@@ -2,8 +2,8 @@
 
 HAL_StatusTypeDef   ina219_init(UINT addr) {
 
-    HAL_StatusTypeDef status;
-    uint8_t data[2];
+    HAL_StatusTypeDef   status;
+    uint8_t             data[2];
 
     // Config:
     // Bus voltage range = 32V
@@ -45,8 +45,8 @@ HAL_StatusTypeDef   ina219_init(UINT addr) {
 
 HAL_StatusTypeDef ina219_read_voltage(float *voltage)
 {
-    uint8_t buf[2];
-    uint16_t raw;
+    uint8_t     buf[2];
+    uint16_t    raw;
 
     tx_mutex_get(&i2c_mutex, TX_WAIT_FOREVER);
 
@@ -93,8 +93,8 @@ HAL_StatusTypeDef ina219_read_current(float *current)
 
 HAL_StatusTypeDef ina219_read_power(float *power)
 {
-    uint8_t buf[2];
-    uint16_t raw;
+    uint8_t     buf[2];
+    uint16_t    raw;
 
     tx_mutex_get(&i2c_mutex, TX_WAIT_FOREVER);