uint32_t CAN_GetMessage(void)
{
    uint32_t mot_idx;
    uint32_t opcode;
    uint32_t message_num;
    
    uint8_t position;
  
    CAN_Receive(CAN1, can_params.fifo_num,  &RxMessage);
    
    mot_idx = (RxMessage.StdId >> 8);
    
    opcode = (RxMessage.StdId - (mot_idx * 0x100));
          
    switch (opcode)
    {      
        case CAN_COM_IGET:
            motor_data[mot_idx].current = (RxMessage.Data[3]<<24) | (RxMessage.Data[2]<<16) |
                                          (RxMessage.Data[1]<<8)  | RxMessage.Data[0];
            position = CAN_IGET_POS;
        break;
        
        case CAN_COM_PGET:
            motor_data[mot_idx].position = (int32_t)((RxMessage.Data[3]<<24) | (RxMessage.Data[2]<<16) |
                                           (RxMessage.Data[1]<<8)  | RxMessage.Data[0]);
            position = CAN_PGET_POS;
        break;
        
        case CAN_COM_SGET:
            motor_data[mot_idx].speed = (RxMessage.Data[3]<<24) | (RxMessage.Data[2]<<16) |
                                        (RxMessage.Data[1]<<8)  | RxMessage.Data[0];
            position = CAN_SGET_POS;
        break;
    }
    
    message_num = (RxMessage.Data[7]<<8) | RxMessage.Data[6];
    if (can_message_num == message_num)
    {
         motor_data[mot_idx].can_rx_flags |= (1<<position);
    }
  
     return CAN_OK;
}

uint32_t MOT_DataReady(void)
{
    int i;
    uint32_t resp = 0;
    
    //check if all motors replied
    for (i = 0; i < MAX_MOTORS; i++)
    {
        if (motor_data[i].ena)
        {
            if ((motor_data[i].can_rx_flags & 0x0F) == 0x0F)
                resp++;
        }      
    }
    
    //all motors responded with all data
    if (resp == g_motors_num)
    {
        //clear all flags - get ready for the next data request
        for (i = 0; i < MAX_MOTORS; i++)
        {
            if (motor_data[i].ena)
                motor_data[i].can_rx_flags = 0;  
        }
        //next data request number
        can_message_num++;
        return 1;
    }
    else
        return 0;
}

while (!MOT_DataReady());