137 lines
4.4 KiB
C
137 lines
4.4 KiB
C
/**
|
|
****************************(C) COPYRIGHT 2019 DJI****************************
|
|
* @file can_receive.c/h
|
|
* @brief there is CAN interrupt function to receive motor data,
|
|
* and CAN send function to send motor current to control motor.
|
|
* @note
|
|
* @history
|
|
* Version Date Author Modification
|
|
* V1.0.0 Dec-26-2018 RM 1. done
|
|
*
|
|
@verbatim
|
|
==============================================================================
|
|
|
|
==============================================================================
|
|
@endverbatim
|
|
****************************(C) COPYRIGHT 2019 DJI****************************
|
|
*/
|
|
|
|
#include "CAN_receive.h"
|
|
#include "main.h"
|
|
|
|
|
|
|
|
extern CAN_HandleTypeDef hcan1;
|
|
//motor data read
|
|
#define get_motor_measure(ptr, data) \
|
|
{ \
|
|
(ptr)->last_ecd = (ptr)->ecd; \
|
|
(ptr)->ecd = (uint16_t)((data)[0] << 8 | (data)[1]); \
|
|
(ptr)->speed_rpm = (uint16_t)((data)[2] << 8 | (data)[3]); \
|
|
(ptr)->given_current = (uint16_t)((data)[4] << 8 | (data)[5]); \
|
|
(ptr)->temperate = (data)[6]; \
|
|
}
|
|
/*
|
|
motor data, 0:chassis motor1 3508;1:chassis motor2 3508;2:chassis motor3 3508;3:chassis motor4 3508;
|
|
*/
|
|
static motor_measure_t motor_chassis[4];
|
|
|
|
static CAN_TxHeaderTypeDef chassis_tx_message;
|
|
static uint8_t chassis_can_send_data[8];
|
|
|
|
/**
|
|
* @brief hal CAN fifo call back, receive motor data
|
|
* @param[in] hcan, the point to CAN handle
|
|
* @retval none
|
|
*/
|
|
void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan)
|
|
{
|
|
CAN_RxHeaderTypeDef rx_header;
|
|
uint8_t rx_data[8];
|
|
|
|
HAL_CAN_GetRxMessage(hcan, CAN_RX_FIFO0, &rx_header, rx_data);
|
|
|
|
switch (rx_header.StdId)
|
|
{
|
|
case CAN_3508_M1_ID:
|
|
case CAN_3508_M2_ID:
|
|
case CAN_3508_M3_ID:
|
|
case CAN_3508_M4_ID:
|
|
{
|
|
static uint8_t i = 0;
|
|
//get motor id
|
|
i = rx_header.StdId - CAN_3508_M1_ID;
|
|
get_motor_measure(&motor_chassis[i], rx_data);
|
|
break;
|
|
}
|
|
|
|
default:
|
|
{
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
|
|
/**
|
|
* @brief send CAN packet of ID 0x700, it will set chassis motor 3508 to quick ID setting
|
|
* @param[in] none
|
|
* @retval none
|
|
*/
|
|
void CAN_cmd_chassis_reset_ID(void)
|
|
{
|
|
uint32_t send_mail_box;
|
|
chassis_tx_message.StdId = 0x700;
|
|
chassis_tx_message.IDE = CAN_ID_STD;
|
|
chassis_tx_message.RTR = CAN_RTR_DATA;
|
|
chassis_tx_message.DLC = 0x08;
|
|
chassis_can_send_data[0] = 0;
|
|
chassis_can_send_data[1] = 0;
|
|
chassis_can_send_data[2] = 0;
|
|
chassis_can_send_data[3] = 0;
|
|
chassis_can_send_data[4] = 0;
|
|
chassis_can_send_data[5] = 0;
|
|
chassis_can_send_data[6] = 0;
|
|
chassis_can_send_data[7] = 0;
|
|
|
|
HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
|
|
}
|
|
|
|
|
|
/**
|
|
* @brief send control current of motor (0x201, 0x202, 0x203, 0x204)
|
|
* @param[in] motor1: (0x201) 3508 motor control current, range [-16384,16384]
|
|
* @param[in] motor2: (0x202) 3508 motor control current, range [-16384,16384]
|
|
* @param[in] motor3: (0x203) 3508 motor control current, range [-16384,16384]
|
|
* @param[in] motor4: (0x204) 3508 motor control current, range [-16384,16384]
|
|
* @retval none
|
|
*/
|
|
void CAN_cmd_chassis(int16_t motor1, int16_t motor2, int16_t motor3, int16_t motor4)
|
|
{
|
|
uint32_t send_mail_box;
|
|
chassis_tx_message.StdId = CAN_CHASSIS_ALL_ID;
|
|
chassis_tx_message.IDE = CAN_ID_STD;
|
|
chassis_tx_message.RTR = CAN_RTR_DATA;
|
|
chassis_tx_message.DLC = 0x08;
|
|
chassis_can_send_data[0] = motor1 >> 8;
|
|
chassis_can_send_data[1] = motor1;
|
|
chassis_can_send_data[2] = motor2 >> 8;
|
|
chassis_can_send_data[3] = motor2;
|
|
chassis_can_send_data[4] = motor3 >> 8;
|
|
chassis_can_send_data[5] = motor3;
|
|
chassis_can_send_data[6] = motor4 >> 8;
|
|
chassis_can_send_data[7] = motor4;
|
|
|
|
HAL_CAN_AddTxMessage(&CHASSIS_CAN, &chassis_tx_message, chassis_can_send_data, &send_mail_box);
|
|
}
|
|
|
|
/**
|
|
* @brief return the chassis 3508 motor data point
|
|
* @param[in] i: motor number,range [0,3]
|
|
* @retval motor data point
|
|
*/
|
|
const motor_measure_t *get_chassis_motor_measure_point(uint8_t i)
|
|
{
|
|
return &motor_chassis[(i & 0x03)];
|
|
} |