|
| 1 | +/** |
| 2 | + ******************************************************************************** |
| 3 | + * @file PubSubReceive.cpp |
| 4 | + * @author shiva |
| 5 | + * @date Dec 14, 2024 |
| 6 | + * @brief |
| 7 | + ******************************************************************************** |
| 8 | + */ |
| 9 | + |
| 10 | +/************************************ |
| 11 | + * INCLUDES |
| 12 | + ************************************/ |
| 13 | +#include "PubSubReceive.hpp" |
| 14 | +#include "SystemDefines.hpp" |
| 15 | +#include "SensorDataTypes.hpp" |
| 16 | +#include "DataBroker.hpp" |
| 17 | + |
| 18 | +/************************************ |
| 19 | + * PRIVATE MACROS AND DEFINES |
| 20 | + ************************************/ |
| 21 | + |
| 22 | +/************************************ |
| 23 | + * VARIABLES |
| 24 | + ************************************/ |
| 25 | + |
| 26 | +/************************************ |
| 27 | + * FUNCTION DECLARATIONS |
| 28 | + ************************************/ |
| 29 | + |
| 30 | +/************************************ |
| 31 | + * FUNCTION DEFINITIONS |
| 32 | + ************************************/ |
| 33 | + |
| 34 | +/** |
| 35 | + * @brief Constructor for PubSubReceive |
| 36 | + */ |
| 37 | +PubSubReceive::PubSubReceive() : Task(PUBSUB_RECEIVE_TASK_QUEUE_DEPTH_OBJS) {} |
| 38 | + |
| 39 | +/** |
| 40 | + * @brief Initialize the PubSubReceive |
| 41 | + * Do not modify this function aside from adding the task name |
| 42 | + */ |
| 43 | +void PubSubReceive::InitTask() { |
| 44 | + // Make sure the task is not already initialized |
| 45 | + SOAR_ASSERT(rtTaskHandle == nullptr, "Cannot initialize watchdog task twice"); |
| 46 | + |
| 47 | + BaseType_t rtValue = xTaskCreate((TaskFunction_t)PubSubReceive::RunTask, (const char*)"PubSubReceive", |
| 48 | + (uint16_t)PUBSUB_RECEIVE_TASK_STACK_DEPTH_WORDS, (void*)this, |
| 49 | + (UBaseType_t)PUBSUB_RECEIVE_TASK_RTOS_PRIORITY, (TaskHandle_t*)&rtTaskHandle); |
| 50 | + |
| 51 | + SOAR_ASSERT(rtValue == pdPASS, "PubSubReceive::InitTask() - xTaskCreate() failed"); |
| 52 | +} |
| 53 | + |
| 54 | +/** |
| 55 | + * @brief Instance Run loop for the Task, runs on scheduler start as long as the task is initialized. |
| 56 | + * @param pvParams RTOS Passed void parameters, contains a pointer to the object instance, should not be used |
| 57 | + */ |
| 58 | +void PubSubReceive::Run(void* pvParams) { |
| 59 | + // SOAR_PRINT("PUBSUB RECIEVE STARTED\n"); |
| 60 | + DataBroker::Subscribe<IMUData>(this); |
| 61 | + // DataBroker::Unsubscribe<IMUData>(this); |
| 62 | + while (1) { |
| 63 | + /* Process commands in blocking mode */ |
| 64 | + Command cm; |
| 65 | + bool res = qEvtQueue->ReceiveWait(cm); |
| 66 | + if (res) { |
| 67 | + HandleCommand(cm); |
| 68 | + } |
| 69 | + } |
| 70 | +} |
| 71 | + |
| 72 | +/** |
| 73 | + * @brief Handles a command |
| 74 | + * @param cm Command reference to handle |
| 75 | + */ |
| 76 | +void PubSubReceive::HandleCommand(Command& cm) { |
| 77 | + switch (cm.GetCommand()) { |
| 78 | + case DATA_BROKER_COMMAND: |
| 79 | + HandleDataBrokerCommand(cm); |
| 80 | + break; |
| 81 | + |
| 82 | + default: |
| 83 | + SOAR_PRINT("PubSubReceive - Received Unsupported Command {%d}\n", cm.GetCommand()); |
| 84 | + break; |
| 85 | + } |
| 86 | + |
| 87 | + // No matter what we happens, we must reset allocated data |
| 88 | + cm.Reset(); |
| 89 | +} |
| 90 | + |
| 91 | +/** |
| 92 | + * @brief Handle all data broker commands |
| 93 | + * @param cm The command object with the data |
| 94 | + * Use cm.GetTaskCommand() to get the message type |
| 95 | + * Message types must be cast back into DataBrokerMessageTypes enum |
| 96 | + * Use cm.GetDataPointer() to get the pointer to the data |
| 97 | + */ |
| 98 | +void PubSubReceive::HandleDataBrokerCommand(const Command& cm) { |
| 99 | + DataBrokerMessageTypes messageType = DataBroker::getMessageType(cm); |
| 100 | + switch (messageType) { |
| 101 | + case DataBrokerMessageTypes::IMU_DATA: { |
| 102 | + IMUData imu_data = DataBroker::ExtractData<IMUData>(cm); |
| 103 | + SOAR_PRINT("\n IMU DATA : \n"); |
| 104 | + SOAR_PRINT(" X -> %d \n", imu_data.accelX); |
| 105 | + SOAR_PRINT(" Y -> %d \n", imu_data.accelY); |
| 106 | + SOAR_PRINT(" Z -> %d \n", imu_data.accelZ); |
| 107 | + SOAR_PRINT("--DATA_END--\n\n"); |
| 108 | + break; |
| 109 | + } |
| 110 | + |
| 111 | + case DataBrokerMessageTypes::THERMOCOUPLE_DATA: |
| 112 | + break; |
| 113 | + |
| 114 | + case DataBrokerMessageTypes::INVALID: |
| 115 | + [[fallthrough]]; |
| 116 | + default: |
| 117 | + break; |
| 118 | + } |
| 119 | +} |
0 commit comments