diff --git a/f1/can/include/can.h b/f1/can/include/can.h
index b1cbd78e66c295111989fa13753c6d5fbfe3ec07..3c1d1f589e3b588c609559a9fc3a1f385c2fc1a9 100644
--- a/f1/can/include/can.h
+++ b/f1/can/include/can.h
@@ -90,6 +90,7 @@ void can_config(TComm *comm_dev,CAN_InitTypeDef *conf);
void can_set_priorities(TComm *comm_dev,TCAN_IRQ_Priorities *priorities);
void can_set_bitrate(TComm *comm_dev,CAN_SPEED bitrate);
void can_set_filter(TComm *comm_dev,CAN_FilterConfTypeDef *filter);
+void can_set_can_id(TComm *comm_dev,unsigned char id);
/* IRQ functions */
unsigned char can_send_irq(unsigned char first_byte);
unsigned char can_enable_tx_irq(void);
diff --git a/f1/can/src/can.c b/f1/can/src/can.c
index c78107c986cde75bdc8a2c966a20bc2fe46d48e8..bc95bcb1affd6685e2bfdcc5212e49d8da8a549e 100644
--- a/f1/can/src/can.c
+++ b/f1/can/src/can.c
@@ -26,6 +26,14 @@ unsigned char *can_dma_phase_data;
unsigned short int can_dma_phase_length;
unsigned short int can_dma_phase_write_ptr;
+unsigned char can_missing_bytes_data[255];
+unsigned short int can_missing_bytes_length;
+unsigned short int can_missing_bytes_write_ptr;
+
+unsigned short int can_send_lenght;
+unsigned short int can_send_iterations;
+unsigned char can_send_data[255];
+
TDynamixelMessage message;
// 1
@@ -86,6 +94,10 @@ void can_init(TComm *comm_dev,CAN_InitTypeDef *conf,TCAN_IRQ_Priorities *priorit
can_dma_phase_data=0x00;
can_dma_phase_length=0;
can_dma_phase_write_ptr=0;
+ can_missing_bytes_length = 0;
+ can_missing_bytes_write_ptr = 0;
+ can_send_lenght = 0;
+ can_send_iterations = 0;
message.packet_init = 0x00;
message.packet_incomplete = 0x00;
@@ -109,12 +121,12 @@ void can_init(TComm *comm_dev,CAN_InitTypeDef *conf,TCAN_IRQ_Priorities *priorit
CANHandle.pRxMsg = &CAN_rxMessage;
CANHandle.pTxMsg = &CAN_txMessage;
- CANHandle.pTxMsg->StdId = 0x321;
+ CANHandle.pTxMsg->StdId = 0x00;
CANHandle.pTxMsg->ExtId = 0x01;
CANHandle.pTxMsg->RTR = CAN_RTR_DATA;
CANHandle.pTxMsg->IDE = CAN_ID_STD;
- HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
+ //HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
}
@@ -204,198 +216,127 @@ void can_set_bitrate(TComm *comm_dev,CAN_SPEED bitrate)
void can_set_filter(TComm *comm_dev,CAN_FilterConfTypeDef *filter)
{
HAL_CAN_ConfigFilter(&CANHandle, filter);
- HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
+ //HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
}
-void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *CANHandle)
+void can_set_can_id(TComm *comm_dev,unsigned char id)
{
- HAL_CAN_Receive_IT(CANHandle,CAN_FIFO0);
- // HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_4);
+ CANHandle.pTxMsg->StdId = id;
}
-void HAL_CAN_TxCpltCallback(CAN_HandleTypeDef* CANHandle)
+/**
+ * HAL_CAN_ErrorCallback
+ * Callback function for CAN errors.
+ * @param CANHandle Pointer to a CAN_HandleTypeDef structure that contains
+ * the configuration information for a given CAN peripheral.
+ */
+void HAL_CAN_ErrorCallback(CAN_HandleTypeDef* CANHandle)
{
- // HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_4);
+ //HAL_GPIO_WritePin(GPIOA, GPIO_PIN_4,GPIO_PIN_SET);
}
-void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* CANHandle)
+
+void HAL_CAN_TxCpltCallback(CAN_HandleTypeDef* CANHandle)
{
- // for (int i = 0; i<10; i++){
- // HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_4);
- // for (int i = 0; i<100000; i++){;}
+ // HAL_CAN_StateTypeDef status = HAL_CAN_GetState(CANHandle);
+ // while(HAL_CAN_STATE_READY != status)
+ // {
+ // status = HAL_CAN_GetState(CANHandle);
// }
- for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
+ if (can_send_lenght != 0)
{
- if(!can_dma_phase)
- comm_do_irq_receive(can_comm_dev,CAN_rxMessage.Data[i]);
+ if (can_send_lenght > 8)
+ {
+ for (uint8_t i=0;i<8;i++)
+ {
+ CANHandle->pTxMsg->Data[i] = can_send_data[i+(8*can_send_iterations)];
+ }
+ can_send_lenght = can_send_lenght - 8;
+ can_send_iterations++;
+ HAL_CAN_Transmit_IT(CANHandle);
+ }
else
{
- can_dma_phase_data[can_dma_phase_write_ptr]=CAN_rxMessage.Data[i];
- can_dma_phase_length--;
- can_dma_phase_write_ptr++;
- if(can_dma_phase_length==0)
- comm_do_dma_receive(can_comm_dev);
+ for (uint8_t i=0;i<can_send_lenght;i++)
+ {
+ CANHandle->pTxMsg->Data[i] = can_send_data[i+(8*can_send_iterations)];
+ }
+ can_send_lenght = 0;
+ can_send_iterations = 0;
+ HAL_CAN_Transmit_IT(CANHandle);
+ uint8_t byte;
+ comm_do_dma_send(can_comm_dev);
+ comm_do_irq_send(can_comm_dev,&byte);
+ }
+ }
+ //__HAL_CAN_DISABLE_IT(CANHandle,CAN_IT_TME);
+ __HAL_CAN_CLEAR_FLAG(CANHandle,CAN_FLAG_RQCP0);
+ __HAL_CAN_CLEAR_FLAG(CANHandle,CAN_FLAG_TXOK0);
+}
+
+void HAL_CAN_RxCpltCallback(CAN_HandleTypeDef* CANHandle)
+{
+ if(can_missing_bytes_length == 0){
+ for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
+ {
+ if(!can_dma_phase)
+ comm_do_irq_receive(can_comm_dev,CAN_rxMessage.Data[i]);
+ else
+ {
+ if(can_dma_phase_length<5){
+ can_dma_phase_data[can_dma_phase_write_ptr]=CAN_rxMessage.Data[i];
+ can_dma_phase_length--;
+ can_dma_phase_write_ptr++;
+ if(can_dma_phase_length==0){
+ comm_do_dma_receive(can_comm_dev);
+ }
+ }
+ }
+ }
+ if(can_dma_phase_length>4){
+ for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
+ can_missing_bytes_data[i] = CAN_rxMessage.Data[i];
+ can_missing_bytes_length = can_dma_phase_length-4;
+ can_missing_bytes_write_ptr = 7;
+ }
+ }
+ else
+ {
+ if(can_missing_bytes_length<=8){
+ for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
+ {
+ can_missing_bytes_write_ptr++;
+ can_missing_bytes_length--;
+ can_missing_bytes_data[can_missing_bytes_write_ptr] = CAN_rxMessage.Data[i];
+ }
+ for(uint8_t i=0;i<=can_missing_bytes_write_ptr;i++)
+ {
+ if(!can_dma_phase)
+ comm_do_irq_receive(can_comm_dev,can_missing_bytes_data[i]);
+ else
+ {
+ can_dma_phase_data[can_dma_phase_write_ptr]=can_missing_bytes_data[i];
+ can_dma_phase_length--;
+ can_dma_phase_write_ptr++;
+ if(can_dma_phase_length==0){
+ comm_do_dma_receive(can_comm_dev);
+ can_missing_bytes_length = 0;
+ can_missing_bytes_write_ptr = 0;
+ }
+ }
+ }
+ }
+ else
+ {
+ for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
+ {
+ can_missing_bytes_write_ptr++;
+ can_missing_bytes_length--;
+ can_missing_bytes_data[can_missing_bytes_write_ptr] = CAN_rxMessage.Data[i];
+ }
}
}
- HAL_CAN_Receive_IT(CANHandle,CAN_FIFO0);
- // message.received_bytes = 0;
- // if(!message.packet_init)
- // {
- // if (CAN_rxMessage.DLC<6)
- // {
- // message.Len_L = 0x00;
- // message.Len_H = 0x00;
- // message.length = 0x00;
- // message.packet_init = 0x00;
- // message.packet_incomplete = 0x00;
- // message.send_packet = 0x00;
- // message.version = VERSION_NULL;
- // }
- // else
- // {
- // for(uint8_t i=0;i<CAN_rxMessage.DLC;i++)
- // {
- // uint8_t byte = CAN_rxMessage.Data[i];
- //
- // switch(message.received_bytes)
- // {
- // case 0: if(byte==0xFF)
- // {
- // message.rx_buffer[message.received_bytes]=byte;
- // message.received_bytes++;
- // }
- // break;
- // case 1: if(byte==0xFF)
- // {
- // message.rx_buffer[message.received_bytes]=byte;
- // message.received_bytes++;
- // }
- // else
- // message.received_bytes--;
- // break;
- // case 2: message.rx_buffer[message.received_bytes]=byte; //ID vs 0xFD
- // message.received_bytes++;
- // break;
- // case 3: if(byte!=0x00) //Length vs 0x00
- // {
- // message.version = VERSION_1;
- // message.Len_L = byte;
- // message.length = byte+4;
- // message.packet_init = 0x01;
- // message.rx_buffer[message.received_bytes] = byte;
- // }
- // else
- // {
- // message.version = VERSION_2;
- // }
- // message.received_bytes++;
- // break;
- // case 4: if (message.version == VERSION_2) //Instruction vs ID
- // {
- // message.rx_buffer[message.received_bytes] = byte;
- // }
- // message.received_bytes++;
- // break;
- // case 5: if (message.version == VERSION_2) //Pn vs Len_L
- // {
- // message.rx_buffer[message.received_bytes] = byte;
- // message.Len_L = byte;
- // }
- // message.received_bytes++;
- // break;
- // case 6: if (message.version == VERSION_2) //Pn vs Len_L
- // {
- // message.rx_buffer[message.received_bytes] = byte;
- // message.Len_H = byte;
- // message.length = (message.Len_H << 8) + message.Len_L;
- // message.packet_init = 0x01;
- // }
- // message.received_bytes++;
- // break;
- // default: break;
- // }
- // }
- // }
- // }
- //
- // if(message.packet_init)
- // {
- // if (message.version == VERSION_1)
- // {
- // if (!message.packet_incomplete)
- // {
- // if (message.Len_L<=4)
- // {
- // for(uint8_t i=4;i<(message.Len_L+4);i++)
- // {
- // message.rx_buffer[i]=CAN_rxMessage.Data[i];
- // }
- // message.send_packet = 0x01;
- // message.packet_incomplete = 0x00;
- // }
- // else
- // {
- // for(uint8_t i=4;i<8;i++)
- // {
- // message.rx_buffer[i]=CAN_rxMessage.Data[i];
- // }
- // message.Len_L = message.Len_L-4;
- // message.packet_incomplete = 0x01;
- // }
- // }
- // else
- // {
- // if (message.Len_L<8)
- // {
- // for(uint8_t i=0;i<message.Len_L;i++)
- // {
- // message.rx_buffer[i+(message.iterations*8)]=CAN_rxMessage.Data[i];
- // }
- // message.send_packet = 0x01;
- // }
- // else
- // {
- // for(uint8_t i=0;i<8;i++)
- // {
- // message.rx_buffer[i+(message.iterations*8)]=CAN_rxMessage.Data[i];
- // }
- // message.Len_L = message.Len_L-8;
- // }
- // message.iterations++;
- // }
- // }
- // else if (message.version == VERSION_2)
- // {
- //
- // }
- // }
- //
- // if (message.send_packet)
- // {
- // for(unsigned int i=0;i<message.length;i++)
- // {
- // if(!can_dma_phase)
- // comm_do_irq_receive(can_comm_dev,message.rx_buffer[i]);
- // else /* do DMA phase */
- // {
- // can_dma_phase_data[can_dma_phase_write_ptr]=message.rx_buffer[i];
- // can_dma_phase_length--;
- // can_dma_phase_write_ptr++;
- // if(can_dma_phase_length==0)
- // {
- // comm_do_dma_receive(can_comm_dev);
- // }
- // }
- // }
- //
- // message.Len_L = 0x00;
- // message.Len_H = 0x00;
- // message.length = 0x00;
- // message.packet_init = 0x00;
- // message.packet_incomplete = 0x00;
- // message.send_packet = 0x00;
- // }
}
// interrupt handlers
@@ -434,6 +375,8 @@ unsigned char can_enable_tx_irq(void)
unsigned char can_receive_irq(void)
{
can_dma_phase=0x00;
+ can_dma_phase_write_ptr=0x00;
+ HAL_CAN_Receive_IT(&CANHandle,CAN_FIFO0);
return 0x00;
}
@@ -445,48 +388,63 @@ unsigned char can_cancel_receive_irq(void)
/* DMA functions */
unsigned char can_send_dma(unsigned char *data,unsigned short int length)
{
- if (length <= 8)
- {
- CANHandle.pTxMsg->DLC = length;
+ // if (length <= 8)
+ // {
+ // CANHandle.pTxMsg->DLC = length;
+ // for (uint8_t i=0;i<length;i++)
+ // {
+ // CANHandle.pTxMsg->Data[i] = data[i];
+ // }
+ // HAL_CAN_Transmit_IT(&CANHandle);
+ // }
+ // else
+ // {
+ // unsigned char iterations = 0x00;
+ // while (length > 0)
+ // {
+ // if(length > 8)
+ // CANHandle.pTxMsg->DLC = 8;
+ // else
+ // CANHandle.pTxMsg->DLC = length;
+ // for (uint8_t i=0;i<CANHandle.pTxMsg->DLC;i++)
+ // {
+ // CANHandle.pTxMsg->Data[i] = data[i+iterations];
+ // }
+ // HAL_CAN_Transmit_IT(&CANHandle);
+ // HAL_Delay(5);
+ // iterations = iterations + 8;
+ // length = length - CANHandle.pTxMsg->DLC;
+ // }
+ // }
+
+ if(length > 8){
+ CANHandle.pTxMsg->DLC = 8;
for (uint8_t i=0;i<length;i++)
+ {
+ can_send_data[i] = data[i];
+ }
+ can_send_lenght = length - 8;
+ can_send_iterations = 1;
+ for (uint8_t i=0;i<8;i++)
{
CANHandle.pTxMsg->Data[i] = data[i];
}
- //HAL_CAN_Transmit(&CANHandle, 10);
HAL_CAN_Transmit_IT(&CANHandle);
}
- else
- {
- // unsigned char completed = 0x00;
- // unsigned char iterations = 0x00;
- // while (!completed) {
- // if (length>8)
- // {
- // CANHandle.pTxMsg->DLC = 8;
- // for (uint8_t i=0;i<8;i++)
- // {
- // CANHandle.pTxMsg->Data[i] = data[i+iterations];
- // }
- // iterations = iterations+8;
- // HAL_CAN_Transmit(&CANHandle, 10);
- // length = length-8;
- // }
- // else
- // {
- // CANHandle.pTxMsg->DLC = length;
- // for (uint8_t i=0;i<length;i++)
- // {
- // CANHandle.pTxMsg->Data[i] = data[i+iterations];
- // }
- // HAL_CAN_Transmit(&CANHandle, 10);
- // completed = 0x01;
- // }
- // }
+ else{
+ CANHandle.pTxMsg->DLC = length;
+ for (uint8_t i=0;i<length;i++)
+ {
+ CANHandle.pTxMsg->Data[i] = data[i];
+ }
+ HAL_CAN_Transmit_IT(&CANHandle);
+ uint8_t byte;
+ comm_do_dma_send(can_comm_dev);
+ comm_do_irq_send(can_comm_dev,&byte);
}
- uint8_t byte;
- comm_do_dma_send(can_comm_dev);
- comm_do_irq_send(can_comm_dev,&byte);
-
+ //HAL_CAN_Transmit_IT(&CANHandle);
+ __HAL_CAN_CLEAR_FLAG(&CANHandle,CAN_FLAG_RQCP0);
+ __HAL_CAN_CLEAR_FLAG(&CANHandle,CAN_FLAG_TXOK0);
return 0x00;
}