Merge branch 'master' of ssh://gitlab.iri.upc.edu:2202/humanoides/tools/stm32_libraries

f4/can/src/can.c

@@ -18,6 +18,8 @@ CAN_HandleTypeDef CANHandle;
 CAN_FilterTypeDef CANFilterConfig;
 CAN_TxHeaderTypeDef CAN_txHeader;
 uint8_t CAN_txMessage[8];
+uint8_t arbitrationBakCAN_txMessage[8];
+
 uint32_t TxMailbox;
 CAN_RxHeaderTypeDef CAN_rxHeader;
 uint8_t CAN_rxMessage[8];
@@ -124,6 +126,7 @@ void can_init(TComm *comm_dev,CAN_InitTypeDef *conf,TCAN_IRQ_Priorities *priorit
   //Activate default interrupt when message in FIFO0
   HAL_CAN_ActivateNotification(&CANHandle, CAN_IT_RX_FIFO0_MSG_PENDING);
   HAL_CAN_ActivateNotification(&CANHandle, CAN_IT_TX_MAILBOX_EMPTY);
+  HAL_CAN_ActivateNotification(&CANHandle, CAN_IT_ERROR);
 }
 
 void can_config(TComm *comm_dev,CAN_InitTypeDef *conf)
@@ -228,7 +231,10 @@ void can_set_can_id(TComm *comm_dev,unsigned int id)
  */
 void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* CANHandle)
 {
-  //HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4,GPIO_PIN_SET);
+  if(HAL_CAN_GetError(CANHandle) == HAL_CAN_ERROR_TX_ALST0)
+  {
+    HAL_CAN_AddTxMessage(CANHandle, &CAN_txHeader, arbitrationBakCAN_txMessage, &TxMailbox);
+  }
 }
 
 void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef* CANHandle)
@@ -359,12 +365,20 @@ unsigned char can_send_dma(unsigned char *data,unsigned short int length)
     }
     can_send_lenght = length - 8;
     can_send_iterations = 1;
+    for (uint8_t i=0;i<8;i++)
+    {
+      arbitrationBakCAN_txMessage[i] = can_send_data[i];
+    }
     HAL_CAN_AddTxMessage(&CANHandle, &CAN_txHeader, data, &TxMailbox);
   }
   else{
     CAN_txHeader.DLC = length;
     can_send_lenght = 0;
     can_send_iterations = 0;
+    for (uint8_t i=0;i<length;i++)
+    {
+      arbitrationBakCAN_txMessage[i] = data[i];
+    }
     HAL_CAN_AddTxMessage(&CANHandle, &CAN_txHeader, data, &TxMailbox);
     uint8_t byte;
     comm_do_dma_send(can_comm_dev);
@@ -386,6 +400,10 @@ void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* CANHandle)
       }
       can_send_lenght = can_send_lenght - 8;
       can_send_iterations++;
+      for (uint8_t i=0;i<8;i++)
+      {
+        arbitrationBakCAN_txMessage[i] = CAN_txMessage[i];
+      }
       HAL_CAN_AddTxMessage(CANHandle, &CAN_txHeader, CAN_txMessage, &TxMailbox);
     }
     else
@@ -397,6 +415,10 @@ void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef* CANHandle)
       }
       can_send_lenght = 0;
       can_send_iterations = 0;
+      for (uint8_t i=0;i<8;i++)
+      {
+        arbitrationBakCAN_txMessage[i] = CAN_txMessage[i];
+      }
       HAL_CAN_AddTxMessage(CANHandle, &CAN_txHeader, CAN_txMessage, &TxMailbox);
       uint8_t byte;
       comm_do_dma_send(can_comm_dev);