tx_helper.cpp

/********************************* Includes **********************************/
#include "tx_helper.h"
#include "cmsis_os.h"
#include "UART_Handler.h"
#include "DynamixelProtocolV1.h"
#include "robotState.h"
#include "Communication.h"
#include "MPU6050.h"
#include "Notification.h"

/********************************** Externs ***********************************/
extern osThreadId PCUARTHandle;
extern osThreadId TXQueueHandle;

/***************************** Private Variables *****************************/
static TXData_t receivedData;
static Dynamixel_HandleTypeDef* motorPtr = nullptr;
static IMUStruct* imuPtr = nullptr;
static char* const pIMUXGyroData = &robotState.msg[ROBOT_STATE_MPU_DATA_OFFSET];

static HAL_StatusTypeDef status;
static uint32_t notification;
static uint32_t dataReadyFlags = 0; // Bits in this are set based on which sensor data is ready

static uint32_t NOTIFICATION_MASK = 0x80000000;

/******************************** Functions **********************************/
/*  StartTxTask Helper Functions                                             */
/*                                                                           */
/*                                                                           */
/*                                                                           */
/*                                                                           */
/*                                                                           */
/*                                                                           */
/*****************************************************************************/
void shiftNotificationMask(void) {
    for (uint8_t i = 1; i <= 12; i++) {
        NOTIFICATION_MASK |= (1 << i);
    }
}

void copySensorDataToSend(void) {
    while ((dataReadyFlags & NOTIFICATION_MASK) != NOTIFICATION_MASK) {
        while (xQueueReceive(TXQueueHandle, &receivedData, portMAX_DELAY) != pdTRUE);

        switch (receivedData.eDataType) {
        case eMotorData:
            motorPtr = (Dynamixel_HandleTypeDef*) receivedData.pData;

            if (motorPtr == NULL) {
                break;
            }

            // Validate data and store it in robotState
            if (motorPtr->_ID <= NUM_MOTORS) {
                // Copy sensor data for this motor into its section of robotState.msg
                memcpy(&robotState.msg[4 * (motorPtr->_ID - 1)],
                        &(motorPtr->_lastPosition), sizeof(float));

                // Set flag indicating the motor with this id has reported in with position data
                dataReadyFlags |= (1 << motorPtr->_ID);
            }
            break;
        case eIMUData:
            imuPtr = (IMUStruct*)receivedData.pData;

            if(imuPtr == NULL){ break; }

            // Copy sensor data into the IMU data section of robotState.msg
            memcpy(pIMUXGyroData, (&imuPtr->_x_Gyro), sizeof(IMUStruct));

            // Set flag indicating IMU data has reported in
            dataReadyFlags |= 0x80000000;
            break;
        default:
            break;
        }
    }
    dataReadyFlags = 0; // Clear all flags
}

void transmitStatusFromPC(void) {
    do {
        xSemaphoreTake(PCUARTHandle, 1);
        status = HAL_UART_Transmit_DMA(&huart5, (uint8_t*) &robotState,
                sizeof(RobotState));
        xSemaphoreGive(PCUARTHandle);
    } while (status != HAL_OK);
}

void waitForNotificationTX(void) {
    do {
        xTaskNotifyWait(0, NOTIFIED_FROM_TX_ISR, &notification, portMAX_DELAY);
    } while ((notification & NOTIFIED_FROM_TX_ISR) != NOTIFIED_FROM_TX_ISR);
}

/*****************************************************************************/
/* end - TxHelperFunctions */