You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

426 lines
15 KiB

12 months ago
  1. #include "zcanreceiver.hpp"
  2. #include "stm32basic/mutex.hpp"
  3. #include "stm32halport/stm32halport.hpp"
  4. #define TAG "zcan"
  5. #define CANHANDLER &hcan1
  6. #define CAN_FILTER_INDEX 0
  7. #define CAN_MAX_FILTER_NUM 7
  8. #define CAN_FIFO_NUM CAN_RX_FIFO0
  9. #define OVER_TIME_MS 30
  10. #define HEART_OVERTIME (30 * 1000)
  11. using namespace iflytop;
  12. using namespace zscanprotocol;
  13. static uint8_t m_deviceId;
  14. static zcanbus_on_rx_t m_rxlistener[30];
  15. static uint32_t m_numListener = 0;
  16. static zcanbus_on_connected_t m_connectedlistener;
  17. static canrxbuffer_t m_rxbufcache;
  18. static uint16_t reportIndex;
  19. static uint8_t rxdata[200];
  20. static uint8_t txbuff[200];
  21. static uint32_t lastpacket_ticket;
  22. static bool m_is_connected;
  23. static uint8_t m_priority;
  24. zmutex m_lock;
  25. static void _oncanpacket(CAN_HandleTypeDef *hcan);
  26. static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData);
  27. static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/);
  28. extern "C" {
  29. void HAL_CAN_TxMailbox0CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0CompleteCallback"); }
  30. void HAL_CAN_TxMailbox1CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1CompleteCallback"); }
  31. void HAL_CAN_TxMailbox2CompleteCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2CompleteCallback"); }
  32. void HAL_CAN_TxMailbox0AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox0AbortCallback"); }
  33. void HAL_CAN_TxMailbox1AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox1AbortCallback"); }
  34. void HAL_CAN_TxMailbox2AbortCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_TxMailbox2AbortCallback"); }
  35. void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) { _oncanpacket(hcan); }
  36. void HAL_CAN_RxFifo0FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo0FullCallback"); }
  37. void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1MsgPendingCallback"); }
  38. void HAL_CAN_RxFifo1FullCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_RxFifo1FullCallback"); }
  39. void HAL_CAN_SleepCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_SleepCallback"); }
  40. void HAL_CAN_WakeUpFromRxMsgCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_WakeUpFromRxMsgCallback"); }
  41. void HAL_CAN_ErrorCallback(CAN_HandleTypeDef *hcan) { ZLOGI(TAG, "HAL_CAN_ErrorCallback"); }
  42. }
  43. /***********************************************************************************************************************
  44. * *
  45. ***********************************************************************************************************************/
  46. static void _oncanpacket(CAN_HandleTypeDef *hcan) {
  47. if (CANHANDLER != hcan) return;
  48. CAN_RxHeaderTypeDef pHeader;
  49. uint8_t aData[8] /*8byte table*/;
  50. while (_getRxMessage(&pHeader, aData)) {
  51. _processOneCanPacket(&pHeader, aData);
  52. }
  53. }
  54. static bool _getRxMessage(CAN_RxHeaderTypeDef *pHeader, uint8_t aData[] /*8byte table*/) {
  55. /**
  56. * @brief FIFO中缓存了多少帧的数据
  57. */
  58. uint32_t level = HAL_CAN_GetRxFifoFillLevel(CANHANDLER, CAN_FIFO_NUM);
  59. if (level == 0) {
  60. return false;
  61. }
  62. HAL_StatusTypeDef HAL_RetVal;
  63. HAL_RetVal = HAL_CAN_GetRxMessage(CANHANDLER, CAN_FIFO_NUM, pHeader, aData);
  64. if (HAL_OK == HAL_RetVal) {
  65. // 处理接收到的can总线数据
  66. return true;
  67. }
  68. return false;
  69. }
  70. static void _processOneCanPacket(CAN_RxHeaderTypeDef *pHeader, uint8_t *aData) {
  71. /**
  72. * @brief 12bit from,1bit emergency
  73. *
  74. * [1] [4bit] [8bit] [8bit] [4bit/4bit]
  75. * , from to frameNum/frameId
  76. */
  77. uint8_t from = (pHeader->ExtId & 0x00FF0000) >> 16;
  78. uint8_t to = (pHeader->ExtId & 0x0000FF00) >> 8;
  79. uint8_t nframe = (pHeader->ExtId & 0x000000F0) >> 4;
  80. uint8_t frameId = (pHeader->ExtId & 0x0000000F);
  81. // ZLOGI(TAG, "from:%d to:%d nframe:%d frameId:%d", from, to, nframe, frameId);
  82. if (pHeader->IDE == CAN_ID_STD) {
  83. return;
  84. }
  85. // 只接收主机消息
  86. if (from != 1) {
  87. return;
  88. }
  89. if (to != m_deviceId && to != 0xff) {
  90. return;
  91. }
  92. // 上次接收到的消息还没有来的急处理
  93. if (m_rxbufcache.dataIsReady) {
  94. ZLOGI(TAG, "discard rx packet ,last packet not processed");
  95. return;
  96. }
  97. if (frameId == 0) {
  98. m_rxbufcache.canPacketNum = 0;
  99. }
  100. if (frameId != m_rxbufcache.canPacketNum) {
  101. m_rxbufcache.canPacketNum = 0;
  102. ZLOGI(TAG, "discard rx packet ,due to lost packet");
  103. return;
  104. }
  105. if (m_rxbufcache.canPacketNum < ZARRAY_SIZE(m_rxbufcache.canPacket)) {
  106. if (m_rxbufcache.canPacketNum == 0) {
  107. m_rxbufcache.header = *pHeader;
  108. m_rxbufcache.from = from;
  109. m_rxbufcache.to = to;
  110. }
  111. m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].datalen = pHeader->DLC;
  112. memcpy(m_rxbufcache.canPacket[m_rxbufcache.canPacketNum].aData, aData, 8);
  113. m_rxbufcache.canPacketNum++;
  114. } else {
  115. ZLOGI(TAG, "discard rx packet ,due to buffer full");
  116. m_rxbufcache.canPacketNum = 0;
  117. return;
  118. }
  119. if (nframe == frameId + 1) {
  120. if (m_rxbufcache.canPacketNum != nframe) {
  121. m_rxbufcache.canPacketNum = 0;
  122. ZLOGI(TAG, "discard rx packet ,due to lost packet");
  123. return;
  124. } else {
  125. m_rxbufcache.dataIsReady = true;
  126. }
  127. }
  128. }
  129. /***********************************************************************************************************************
  130. * FUNC *
  131. ***********************************************************************************************************************/
  132. static HAL_StatusTypeDef activateRxIT() {
  133. HAL_StatusTypeDef hal_status = HAL_ERROR;
  134. if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
  135. hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
  136. } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
  137. hal_status = HAL_CAN_ActivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
  138. } else {
  139. ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
  140. return hal_status;
  141. }
  142. return hal_status;
  143. }
  144. // static HAL_StatusTypeDef deactivateRxIT() {
  145. // HAL_StatusTypeDef hal_status = HAL_ERROR;
  146. // if (CAN_FIFO_NUM == CAN_RX_FIFO0) {
  147. // hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO0_MSG_PENDING);
  148. // } else if (CAN_FIFO_NUM == CAN_RX_FIFO1) {
  149. // hal_status = HAL_CAN_DeactivateNotification(CANHANDLER, CAN_IT_RX_FIFO1_MSG_PENDING);
  150. // } else {
  151. // ZLOGE(TAG, "start can HAL_CAN_ActivateNotification CAN_IT_RX_FIFO0_MSG_PENDING fail\r\n");
  152. // return hal_status;
  153. // }
  154. // return hal_status;
  155. // }
  156. HAL_StatusTypeDef initializeFilter() {
  157. HAL_StatusTypeDef HAL_Status;
  158. CAN_FilterTypeDef sFilterConfig;
  159. uint32_t filterId;
  160. uint32_t mask;
  161. memset(&sFilterConfig, 0, sizeof(sFilterConfig));
  162. sFilterConfig.FilterMode = CAN_FILTERMODE_IDMASK; // 设为MASK模式
  163. sFilterConfig.FilterScale = CAN_FILTERSCALE_32BIT; // CAN_FILTERSCALE_16BIT
  164. sFilterConfig.FilterFIFOAssignment = CAN_FIFO_NUM; // 关联过滤器到rxfifoNum
  165. sFilterConfig.FilterActivation = ENABLE; // 激活过滤器
  166. sFilterConfig.SlaveStartFilterBank = CAN_MAX_FILTER_NUM; // slave filter start index
  167. /*******************************************************************************
  168. * *
  169. *******************************************************************************/
  170. filterId = (0); //
  171. mask = (0); //
  172. sFilterConfig.FilterBank = CAN_FILTER_INDEX; //
  173. sFilterConfig.FilterMaskIdLow = mask & 0xffff; //
  174. sFilterConfig.FilterMaskIdHigh = (mask & 0xffff0000) >> 16; //
  175. sFilterConfig.FilterIdLow = filterId & 0xffff; //
  176. sFilterConfig.FilterIdHigh = (filterId & 0xffff0000) >> 16; //
  177. HAL_Status = HAL_CAN_ConfigFilter(CANHANDLER, &sFilterConfig);
  178. if (HAL_Status != HAL_OK) {
  179. ZLOGE(TAG, "HAL_CAN_ConfigFilter filter0 fail");
  180. return HAL_Status;
  181. }
  182. // ZLOGI(TAG, "HAL_CAN_ConfigFilter filterID1 %08x", filterId >> 3);
  183. return HAL_Status;
  184. }
  185. void zcanbus_init(uint8_t deviceId) {
  186. m_deviceId = deviceId;
  187. HAL_StatusTypeDef hal_status;
  188. m_rxbufcache.dataIsReady = false;
  189. m_rxbufcache.id = 0; // 只接收来自主机的消息
  190. m_rxbufcache.canPacketNum = 0;
  191. hal_status = initializeFilter();
  192. if (hal_status != HAL_OK) {
  193. ZLOGE(TAG, "start can initializeFilter fail\r\n");
  194. return;
  195. }
  196. hal_status = HAL_CAN_Start(CANHANDLER); // 开启CAN
  197. if (hal_status != HAL_OK) {
  198. ZLOGE(TAG, "start can fail\r\n");
  199. return;
  200. }
  201. m_lock.init();
  202. HAL_StatusTypeDef status = activateRxIT();
  203. if (status != HAL_OK) {
  204. ZLOGE(TAG, "activateRxIT fail\r\n");
  205. ZASSERT(0);
  206. return;
  207. }
  208. ZLOGI(TAG, "zcanbus init done");
  209. }
  210. void zcanbus_reglistener(zcanbus_on_rx_t rxlistener) {
  211. ZASSERT(m_numListener < ZARRAY_SIZE(m_rxlistener));
  212. m_rxlistener[m_numListener++] = rxlistener;
  213. }
  214. void zcanbus_reg_on_connected_listener(zcanbus_on_connected_t connectedlistener) { m_connectedlistener = connectedlistener; }
  215. bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len) { return zcanbus_send_packet(to, packet, len, OVER_TIME_MS); }
  216. static char *hex2str(uint8_t *data, size_t len) {
  217. static char buf[200];
  218. memset(buf, 0, sizeof(buf));
  219. for (size_t i = 0; i < len; i++) {
  220. sprintf(buf + i * 2, "%02x", data[i]);
  221. }
  222. return buf;
  223. }
  224. bool zcanbus_send_packet(uint8_t to, uint8_t *packet, size_t len, int overtimems) {
  225. ZLOGI(TAG, "sendPacket to:%d, %s(%d)", to, hex2str(packet, len), len);
  226. int npacket = len / 8 + (len % 8 == 0 ? 0 : 1);
  227. if (npacket > 255) {
  228. ZLOGE(TAG, "sendPacket fail, len:%d", len);
  229. return false;
  230. }
  231. int finalpacketlen = len % 8 == 0 ? 8 : len % 8;
  232. for (uint8_t i = 0; i < npacket; i++) {
  233. bool suc = false;
  234. if (i == npacket - 1) {
  235. suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, finalpacketlen, overtimems);
  236. } else {
  237. suc = zcanbus_send_sub_packet(to, npacket, i, packet + i * 8, 8, overtimems);
  238. }
  239. if (!suc) {
  240. // ZLOGE(TAG, "sendPacket fail, packet(%d:%d)", npacket, i);
  241. return false;
  242. }
  243. }
  244. return true;
  245. }
  246. // static const char *canpacket_dump(uint8_t *data, int size) {
  247. // static char buf[20];
  248. // memset(buf, 0, sizeof(buf));
  249. // for (int i = 0; i < size; i++) {
  250. // sprintf(buf + i * 2, "%02x", data[i]);
  251. // }
  252. // return buf;
  253. // }
  254. bool zcanbus_send_sub_packet(uint8_t to, int npacket, int packetIndex, uint8_t *packet, size_t len, int overtimems) {
  255. CAN_TxHeaderTypeDef pHeader;
  256. uint8_t aData[8] /*8byte table*/;
  257. uint32_t txMailBox = 0;
  258. uint32_t enterticket = zget_ticket();
  259. memset(&pHeader, 0, sizeof(pHeader));
  260. memset(aData, 0, sizeof(aData));
  261. pHeader.StdId = 0x00;
  262. pHeader.ExtId = (m_deviceId << 16) | (to << 8) | (npacket << 4) | (packetIndex);
  263. pHeader.ExtId |= (m_priority & 0x0f) << 24;
  264. pHeader.ExtId |= (0x01 << 28);
  265. pHeader.IDE = CAN_ID_EXT;
  266. pHeader.RTR = CAN_RTR_DATA;
  267. pHeader.DLC = len;
  268. pHeader.TransmitGlobalTime = DISABLE;
  269. memcpy(aData, packet, len);
  270. // ZLOGI(TAG, "tx %s", canpacket_dump(aData, len));
  271. HAL_StatusTypeDef lastTransmitStatus = HAL_CAN_AddTxMessage(CANHANDLER, &pHeader, aData, &txMailBox);
  272. if (lastTransmitStatus != HAL_OK) {
  273. ZLOGE(TAG, "HAL_CAN_AddTxMessage fail");
  274. return false;
  275. }
  276. while (HAL_CAN_IsTxMessagePending(CANHANDLER, txMailBox)) {
  277. if (zhas_passedms(enterticket) > (uint32_t)overtimems) {
  278. lastTransmitStatus = HAL_TIMEOUT;
  279. HAL_CAN_AbortTxRequest(CANHANDLER, txMailBox);
  280. return false;
  281. }
  282. zos_delay_ms(1);
  283. }
  284. return true;
  285. }
  286. bool zcanbus_send_ack(zcanbus_packet_t *rxpacket, uint8_t *param, size_t len) {
  287. zlock_guard l(m_lock);
  288. zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  289. txpacket->index = rxpacket->index;
  290. txpacket->function_id = rxpacket->function_id;
  291. txpacket->ptype = kreceipt;
  292. if (param) memcpy(txpacket->params, param, len);
  293. return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len);
  294. }
  295. bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, int32_t val) { return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val)); }
  296. bool zcanbus_send_ack(iflytop::zscanprotocol::zcanbus_packet_t *rxpacket, bool _val) {
  297. int32_t val = _val ? 1 : 0;
  298. return zcanbus_send_ack(rxpacket, (uint8_t *)&val, sizeof(val));
  299. }
  300. bool zcanbus_send_errorack(zcanbus_packet_t *rxpacket, int32_t errcode) {
  301. zlock_guard l(m_lock);
  302. m_priority = kpriority_receipt;
  303. zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  304. txpacket->index = rxpacket->index;
  305. txpacket->function_id = rxpacket->function_id;
  306. txpacket->ptype = kerror_receipt;
  307. memcpy(txpacket->params, &errcode, sizeof(errcode));
  308. return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + 4);
  309. }
  310. bool zcanbus_send_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
  311. zlock_guard l(m_lock);
  312. m_priority = kpriority_report;
  313. zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  314. txpacket->index = reportIndex++;
  315. txpacket->function_id = function_id;
  316. txpacket->ptype = kreport;
  317. memcpy(txpacket->params, param, len);
  318. return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
  319. }
  320. bool zcanbus_send_emergency_report(uint16_t function_id, uint8_t *param, size_t len, int32_t overtime) {
  321. zlock_guard l(m_lock);
  322. m_priority = kpriority_emergency_report;
  323. zcanbus_packet_t *txpacket = (zcanbus_packet_t *)txbuff;
  324. txpacket->index = reportIndex++;
  325. txpacket->function_id = function_id;
  326. txpacket->ptype = kreport;
  327. memcpy(txpacket->params, param, len);
  328. return zcanbus_send_packet(1 /*mainboard*/, (uint8_t *)&txpacket[0], sizeof(zcanbus_packet_t) + len, overtime);
  329. }
  330. static void process_rx_packet(canrxbuffer_t *canrxbuf, uint8_t *rx, size_t len) {
  331. zcanbus_packet_t *packet = (zcanbus_packet_t *)rx;
  332. lastpacket_ticket = zget_ticket();
  333. if (!m_is_connected) {
  334. m_is_connected = true;
  335. if (m_connectedlistener) m_connectedlistener(true);
  336. }
  337. if (packet->ptype == kcmd) {
  338. for (size_t i = 0; i < m_numListener; i++) {
  339. m_rxlistener[i](canrxbuf->from, canrxbuf->to, rxdata, len);
  340. }
  341. }
  342. }
  343. void zcanbus_schedule() {
  344. canrxbuffer_t *rxbuf = &m_rxbufcache;
  345. uint16_t fromId = 0;
  346. uint16_t toId = 0;
  347. if (rxbuf->dataIsReady) {
  348. int dataoff = 0;
  349. // rxdata[0] = rxbuf->from;
  350. // rxdata[1] = rxbuf->to;
  351. for (size_t i = 0; i < rxbuf->canPacketNum; i++) {
  352. memcpy(rxdata + dataoff, rxbuf->canPacket[i].aData, rxbuf->canPacket[i].datalen);
  353. dataoff += rxbuf->canPacket[i].datalen;
  354. ZASSERT(dataoff < ZARRAY_SIZE(rxdata));
  355. }
  356. process_rx_packet(rxbuf, rxdata, dataoff);
  357. rxbuf->dataIsReady = false;
  358. }
  359. if (m_is_connected && zhas_passedms(lastpacket_ticket) > HEART_OVERTIME) {
  360. m_is_connected = false;
  361. if (m_connectedlistener) m_connectedlistener(false);
  362. }
  363. }
  364. bool zcanbus_is_connected() { return m_is_connected; }