c610+M2006+can总线代码

/****************************************************************************** /// @brief /// @copyright Copyright (c) 2017 <dji-innovations, Corp. RM Dept.> /// @license MIT License /// Permission is hereby granted, free of charge, to any person obtaining a copy /// of this software and associated documentation files (the "Software"), to deal /// in the Software without restriction,including without limitation the rights /// to use, copy, modify, merge, publish, distribute, sublicense,and/or sell /// copies of the Software, and to permit persons to whom the Software is furnished /// to do so,subject to the following conditions: /// /// The above copyright notice and this permission notice shall be included in /// all copies or substantial portions of the Software. /// /// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR /// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, /// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL /// THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER /// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, /// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN /// THE SOFTWARE. *******************************************************************************/ #include "includes.h" void Can2Init(void){ GPIO_InitTypeDef Gpio_Structure; CAN_InitTypeDef Can_Structure; NVIC_InitTypeDef NVIC_InitStructure; CAN_FilterInitTypeDef Can_filter_Structure; RCC_AHB1PeriphClockCmd( RCC_AHB1Periph_GPIOB, ENABLE ); // RCC_APB1PeriphClockCmd( RCC_APB1Periph_CAN1 | RCC_APB1Periph_CAN2, ENABLE ); GPIO_PinAFConfig( GPIOB, GPIO_PinSource5, GPIO_AF_CAN2 ); GPIO_PinAFConfig( GPIOB, GPIO_PinSource6, GPIO_AF_CAN2 ); Gpio_Structure.GPIO_Pin = GPIO_Pin_5 | GPIO_Pin_6; Gpio_Structure.GPIO_Mode = GPIO_Mode_AF; GPIO_Init( GPIOB, &Gpio_Structure ); NVIC_InitStructure.NVIC_IRQChannel = CAN2_RX0_IRQn; NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 5; NVIC_InitStructure.NVIC_IRQChannelSubPriority = 0; NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE; NVIC_Init( &NVIC_InitStructure ); CAN_DeInit(CAN2); CAN_StructInit( &Can_Structure ); Can_Structure.CAN_TTCM = DISABLE; Can_Structure.CAN_ABOM = ENABLE; Can_Structure.CAN_AWUM = DISABLE; Can_Structure.CAN_NART = DISABLE; Can_Structure.CAN_RFLM = DISABLE; Can_Structure.CAN_TXFP = ENABLE; Can_Structure.CAN_Mode = CAN_Mode_Normal; Can_Structure.CAN_SJW = CAN_SJW_1tq; Can_Structure.CAN_BS1 = CAN_BS1_9tq; Can_Structure.CAN_BS2 = CAN_BS2_4tq; Can_Structure.CAN_Prescaler = 3; CAN_Init(CAN2, &Can_Structure); Can_filter_Structure.CAN_FilterNumber = 14;//can2的过滤器从14开始 Can_filter_Structure.CAN_FilterMode = CAN_FilterMode_IdList; Can_filter_Structure.CAN_FilterScale = CAN_FilterScale_16bit; //使用列表模式，只关心地址为201~204的终端 Can_filter_Structure.CAN_FilterIdHigh = ((uint16_t)CAN_2006Moto1_ID << 5); Can_filter_Structure.CAN_FilterIdLow = ((uint16_t)CAN_2006Moto2_ID << 5); Can_filter_Structure.CAN_FilterMaskIdHigh = ((uint16_t)CAN_2006Moto3_ID << 5); Can_filter_Structure.CAN_FilterMaskIdLow = ((uint16_t)CAN_2006Moto4_ID << 5); Can_filter_Structure.CAN_FilterFIFOAssignment = 0; Can_filter_Structure.CAN_FilterActivation=ENABLE; CAN_FilterInit(&Can_filter_Structure); CAN_ITConfig( CAN2, CAN_IT_FMP0, ENABLE ); } /************************************************************************* CAN2_TX_IRQHandler 描述：CAN2的发送中断函数 *************************************************************************/ void CAN2_TX_IRQHandler(void) { OSIntEnter(); if (CAN_GetITStatus(CAN2,CAN_IT_TME)!= RESET) { CAN_ClearITPendingBit(CAN2,CAN_IT_TME); } OSIntExit(); } /************************************************************************* CAN2_RX0_IRQHandler 描述：CAN2的接收中断函数 *************************************************************************/ moto_measure_t moto_chassis[4] = {0};//4 chassis moto void CAN2_RX0_IRQHandler(void) { CanRxMsg rx_message; u8 i; OSIntEnter(); // extern moto_measure_t moto_chassis[4]; if (CAN_GetITStatus(CAN2,CAN_IT_FMP0)!= RESET) { CAN_ClearITPendingBit(CAN2, CAN_IT_FMP0); CAN_Receive(CAN2, CAN_FIFO0, &rx_message); if((rx_message.IDE == CAN_Id_Standard)&&(rx_message.IDE == CAN_RTR_Data)&&(rx_message.DLC == 8)) //标准帧、数据帧、数据长度为8 { if((rx_message.StdId == CAN_2006Moto1_ID)||(rx_message.StdId == CAN_2006Moto2_ID)\ ||(rx_message.StdId == CAN_2006Moto3_ID)||(rx_message.StdId == CAN_2006Moto4_ID)) { i = rx_message.StdId - CAN_2006Moto1_ID; get_moto_measure(&moto_chassis[i], &rx_message); } } } OSIntExit(); } /******************************************************************************************* * @Func void get_moto_measure(moto_measure_t *ptr, CAN_HandleTypeDef* hcan) * @Brief 接收3508电机通过CAN发过来的信息 * @Param * @Retval None * @Date 2015/11/24 *******************************************************************************************/ //int32_t angle; //int16_t speed_rpm; void get_moto_measure(moto_measure_t *ptr, CanRxMsg* rx_message) { ptr->last_angle = ptr->angle; ptr->angle = (uint16_t)(rx_message->Data[0]<<8 | rx_message->Data[1]) ; ptr->speed_rpm = (int16_t)(rx_message->Data[2]<<8 | rx_message->Data[3]); ptr->real_current = (rx_message->Data[4]<<8 | rx_message->Data[5])*5.f/16384.f; ptr->hall = rx_message->Data[6]; if(ptr->angle - ptr->last_angle > 4096) ptr->round_cnt --; else if (ptr->angle - ptr->last_angle < -4096) ptr->round_cnt ++; ptr->total_angle = ptr->round_cnt * 8192 + ptr->angle - ptr->offset_angle; // angle = ptr->total_angle; // speed_rpm = ptr->speed_rpm; } /*this function should be called after system+can init */ void get_moto_offset(moto_measure_t *ptr, CanRxMsg* rx_message) { ptr->angle = (uint16_t)(rx_message->Data[0]<<8 | rx_message->Data[1]) ; ptr->offset_angle = ptr->angle; } #define ABS(x) ( (x>0) ? (x) : (-x) ) /** *@bref 电机上电角度=0， 之后用这个函数更新3510电机的相对开机后（为0）的相对角度。 */ void get_total_angle(moto_measure_t *p) { int res1, res2, delta; if(p->angle < p->last_angle){ //可能的情况 res1 = p->angle + 8192 - p->last_angle; //正转，delta=+ res2 = p->angle - p->last_angle; //反转 delta=- }else{ //angle > last res1 = p->angle - 8192 - p->last_angle ;//反转 delta - res2 = p->angle - p->last_angle; //正转 delta + } //不管正反转，肯定是转的角度小的那个是真的 if(ABS(res1)<ABS(res2)) delta = res1; else delta = res2; p->total_angle += delta; p->last_angle = p->angle; } void set_moto_current(s16 iq1, s16 iq2, s16 iq3, s16 iq4) { CanTxMsg tx_message; tx_message.StdId = 0x200; tx_message.IDE = CAN_ID_STD; tx_message.RTR = CAN_RTR_DATA; tx_message.DLC = 0x08; tx_message.Data[0] = (iq1 >> 8); tx_message.Data[1] = iq1; tx_message.Data[2] = (iq2 >> 8); tx_message.Data[3] = iq2; tx_message.Data[4] = iq3 >> 8; tx_message.Data[5] = iq3; tx_message.Data[6] = iq4 >> 8; tx_message.Data[7] = iq4; CAN_Transmit(CAN2,&tx_message); } float Turn