Case 1, get motor Mode status. Using this testing to verify the communication between STM32 Blue Pill, MCP2551 and Servo42D via CAN.

#include "main.h"

/* Private variables ---------------------------------------------------------*/
CAN_HandleTypeDef hcan;

/* USER CODE BEGIN PV */

#define SERVO42D_ID 0x01
#define SERVO42D_READ 0x00
#define SERVO42D_WORK_MODE 0x82
CAN_TxHeaderTypeDef TxHeader;
CAN_RxHeaderTypeDef RxHeader;
uint8_t TxData[8];
uint8_t RxData[8];
uint32_t TxMailbox;
uint8_t ServoMode=0xFF;

/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MX_GPIO_Init(void);
static void MX_CAN_Init(void);
/* USER CODE BEGIN PFP */

void Servo42D_ReadWorkMode(void);

int main(void)
{

  HAL_Init();
  SystemClock_Config();
  MX_GPIO_Init();
  MX_CAN_Init();
  if(HAL_CAN_Start(&hcan)!=HAL_OK)
  {
      Error_Handler();
  }
  while (1)
  {
    /* USER CODE END WHILE */
	  Servo42D_ReadWorkMode();
	  HAL_Delay(100);
	  if(HAL_CAN_GetRxFifoFillLevel(&hcan,CAN_RX_FIFO0)>0)
	  {
		  if(HAL_CAN_GetRxMessage(&hcan,CAN_RX_FIFO0,&RxHeader,RxData)==HAL_OK)
		  {
			  if(RxHeader.StdId==SERVO42D_ID&&RxHeader.DLC==3&&RxData[0]==SERVO42D_WORK_MODE)
			  {
				  uint8_t crc=(SERVO42D_ID+RxData[0]+RxData[1])&0xFF;
				  if(RxData[2]==crc)
				  {
					  ServoMode=RxData[1];
				  }
			  }
		  }
	  }
	  HAL_Delay(900);
    /* USER CODE BEGIN 3 */
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.HSEPredivValue = RCC_HSE_PREDIV_DIV1;
  RCC_OscInitStruct.HSIState = RCC_HSI_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLMUL = RCC_PLL_MUL9;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_HCLK_DIV2;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_HCLK_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_2) != HAL_OK)
  {
    Error_Handler();
  }
}

/**
  * @brief CAN Initialization Function
  * @param None
  * @retval None
  */
static void MX_CAN_Init(void)
{

  hcan.Instance = CAN1;
  hcan.Init.Prescaler = 4;
  hcan.Init.Mode = CAN_MODE_NORMAL;
  hcan.Init.SyncJumpWidth = CAN_SJW_1TQ;
  hcan.Init.TimeSeg1 = CAN_BS1_15TQ;
  hcan.Init.TimeSeg2 = CAN_BS2_2TQ;
  hcan.Init.TimeTriggeredMode = DISABLE;
  hcan.Init.AutoBusOff = DISABLE;
  hcan.Init.AutoWakeUp = DISABLE;
  hcan.Init.AutoRetransmission = DISABLE;
  hcan.Init.ReceiveFifoLocked = DISABLE;
  hcan.Init.TransmitFifoPriority = DISABLE;
  if (HAL_CAN_Init(&hcan) != HAL_OK)
  {
    Error_Handler();
  }
  /* USER CODE BEGIN CAN_Init 2 */
  CAN_FilterTypeDef CAN_Filter;
  CAN_Filter.FilterBank=0;
  CAN_Filter.FilterMode=CAN_FILTERMODE_IDMASK;
  CAN_Filter.FilterScale=CAN_FILTERSCALE_32BIT;
  CAN_Filter.FilterIdHigh=(SERVO42D_ID<<5);
  CAN_Filter.FilterIdLow=0;
  CAN_Filter.FilterMaskIdHigh=(0x7FF<<5);
  CAN_Filter.FilterMaskIdLow=0;
  CAN_Filter.FilterFIFOAssignment=CAN_FILTER_FIFO0;
  CAN_Filter.FilterActivation=ENABLE;
  CAN_Filter.SlaveStartFilterBank=14;
  if(HAL_CAN_ConfigFilter(&hcan,&CAN_Filter)!=HAL_OK)
  {
      Error_Handler();
  }
  /* USER CODE END CAN_Init 2 */

}

static void MX_GPIO_Init(void)
{

  /* GPIO Ports Clock Enable */
  __HAL_RCC_GPIOD_CLK_ENABLE();
  __HAL_RCC_GPIOA_CLK_ENABLE();
  __HAL_RCC_GPIOB_CLK_ENABLE();

}

/* USER CODE BEGIN 4 */
void Servo42D_ReadWorkMode(void)
{
    TxHeader.StdId=SERVO42D_ID;
    TxHeader.ExtId=0;
    TxHeader.IDE=CAN_ID_STD;
    TxHeader.RTR=CAN_RTR_DATA;
    TxHeader.DLC=3;
    TxHeader.TransmitGlobalTime=DISABLE;
    TxData[0]=SERVO42D_READ;
    TxData[1]=SERVO42D_WORK_MODE;
    TxData[2]=(SERVO42D_ID+TxData[0]+TxData[1])&0xFF;
    HAL_CAN_AddTxMessage(&hcan,&TxHeader,TxData,&TxMailbox);
}
/* USER CODE END 4 */

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}
#ifdef USE_FULL_ASSERT
void assert_failed(uint8_t *file, uint32_t line)
{
}
#endif /* USE_FULL_ASSERT */