/*******************************************************************************
* Function Name : CAN1_RX1_IRQHandler
* Description : This function handles CAN1 RX1 request.
* Input : None
* Output : None
* Return : None
*******************************************************************************/
void CAN1_RX1_IRQHandler(void)
{ if(CAN_GetITStatus(CAN1,CAN_IT_FMP1)== SET)
{
// if(Bridge.CAN_State!= CAN_PROC_MSGINQUEUE && Bridge.CAN_State!= CAN_PROCESSING )
//{
CAN_Receive(CAN1, CAN_FIFO1, &Bridge.RxMessage1);
CANpacket_receive=1;
Bridge.CAN_State=ChangeState_level_Busier(Bridge.CAN_State);
//CAN_ClearITPendingBit(CAN1, CAN_IT_FMP1) //Don't need this line the intterruption is cleared automatically when read
/* }
else
{
Bridge.CAN_Err.ErrorId=CAN_FULLQUEUE;
Bridge.CAN_Err.ErrorData= 0;
}*/
}
else
{ /*This case has to be treated as an important problem of using the multiport communication: The priority of the CAN communication have to be modified after tests or make an automated algorithm that (numerically ) */
if(CAN_GetITStatus(CAN1,CAN_FLAG_FF1)== SET)
{
//CAN_ClearITPendingBit( CAN1,CAN_FLAG_FF1);
Bridge.CAN_Err.ErrorId=CAN_OVERFLOW;
Bridge.CAN_Err.ErrorData=CAN_MessagePending( CAN1,CAN_FIFO1);
}
/*Interrupt when error iss reported by the CAN BUS controller*/
else if(CAN_GetITStatus(CAN1,CAN_IT_ERR)== SET)
{
Bridge.CAN_Err.ErrorId=CAN_RCVERR;
Bridge.CAN_Err.ErrorData=CAN_GetLastErrorCode(CAN1);
}
}
}
void CAN1_RX0_IRQHandler(){
CanRxMsg RxMessage;
CanRxMsg* RxMsg;
RxMsg = &RxMessage;
volatile long DataLow;
volatile long DataHigh;
/*-----During initialization, just reading the FIFO to clear it-----*/
if (init==1)
{
//toggle_led(green2);
//Delay(10000);
CAN_Receive(CAN1,CAN_FIFO0,&RxMsg3);
//CAN_Receive(CAN1,CAN_FIFO1,&RxMsg3);
}
/*-------------------------After initialization--------------------*/
if ((CAN_GetLastErrorCode(CAN1)== CAN_ErrorCode_NoErr)&(init==0)) //If No error occurred
{
if (CAN_GetFlagStatus(CANx,CAN_FLAG_FMP0) == SET) // check FIFO_0
{
//toggle_led(red);
CAN_Receive(CANx,CAN_FIFO0,RxMsg); //Read the message
DataLow = RxMsg -> Data[3]<<24 |RxMsg -> Data[2]<<16 |RxMsg -> Data[1]<<8 | RxMsg -> Data[0];
DataHigh = RxMsg -> Data[7]<<24 |RxMsg -> Data[6]<<16 |RxMsg -> Data[5]<<8 | RxMsg -> Data[4];
/*---------------Check the Filter Match Index to see which filter is activated-----------*/
/*----------------------*/
/* FIFO 0 */
/* FMI = 0 -> EPOS 1 */
/* FMI = 1 -> EPOS 2 */
/*----------------------*/
if ((RxMessage.FMI == 0))// && (available_data.EPOS1 != 1)) //avoid taking a message already taken from the other FIFO mailbox
{
toggle_led(red);
switch ( DataLow )
{
case RECEIVE_ACT_POS:
Sensor_val.motor_pos_1 = DataHigh ;
break;
case RECEIVE_ACT_VEL:
Sensor_val.motor_vel_1 = DataHigh;
break;
case RECEIVE_ACT_CUR:
Sensor_val.current_EPOS1 = DataHigh;
break;
case ACK_POS_SEND: ; break;
case ACK_VEL_SEND: ; break;
case ACK_CUR_SEND: ; break;
default: break;
}
available_data.EPOS1 = 1;
}
if (RxMessage.FMI == 1)// && (available_data.EPOS2 != 1))
{
toggle_led(yellow);
switch ( DataLow )
{
case RECEIVE_ACT_POS :
Sensor_val.motor_pos_2 = DataHigh ;
break;
case RECEIVE_ACT_VEL:
Sensor_val.motor_vel_2 = DataHigh;
break;
case RECEIVE_ACT_CUR:
Sensor_val.current_EPOS2 = DataHigh;
break;
case ACK_POS_SEND: ; break;
case ACK_VEL_SEND: ; break;
case ACK_CUR_SEND: ; break;
default: break;
}
available_data.EPOS2 = 1;
}
}
}
//-----NOTE: At the old board nothing happens if there are errors-----------------//
//if ((available_data.EPOS1 == 1 ) && (available_data.EPOS2 == 1) && (available_data.EPOS3 == 1))
// {
// // Odometry and control
// Gyro_Values_to_Theta_dot();
// odometry();
// //kalman();
//
// setpoint_filter();
// state_feedback_control();
// //non_linear_control();
// //planar_control();
//
//
// // Send the commands to the EPOS
// Safety_first();
// //Motor_current_real.I_1 = 0;
// //Motor_current_real.I_2 = 0;
// //Motor_current_real.I_3 = 0;
// EPOS_set_current_SDO(1,(short)Motor_current_real.I_1);
// EPOS_set_current_SDO(2,(short)Motor_current_real.I_2);
// EPOS_set_current_SDO(4,(short)Motor_current_real.I_3);
//
//
// // Reset the EPOS data available flag
// available_data.EPOS1=0;
// available_data.EPOS2=0;
// available_data.EPOS3=0;
//
// //Disable CAN interrupt
// //CANx -> IER &= ~(0x0000007e);
// CAN_ITConfig(CANx,CAN_IT_FF0 | CAN_IT_FOV0 | CAN_IT_FMP0 | CAN_IT_FF1 | CAN_IT_FOV1 | CAN_IT_FMP1, DISABLE);
// //Clear pending interrupts
// CAN_ClearITPendingBit(CANx,CAN_IT_FF0 | CAN_IT_FOV0 | CAN_IT_FF1 | CAN_IT_FOV1);
//
// //Enable IMU interrupts
// USART_ITConfig(USART3,USART_IT_RXNE,ENABLE);
// USART_ITConfig(USART3,USART_IT_TC,ENABLE);
//
// }
}
