TuxMonkey/half-steering-basic-framework
1
0
Fork 0
mirror of https://gitee.com/dlmu-cone/half-steering-basic-framework synced 2026-10-24 01:52:20 +08:00
Code Issues Packages Projects Releases Wiki Activity

damiao motor driver init_1

Browse Source
This commit is contained in:
Zorua
2026-03-15 12:52:05 +08:00
parent fbc83771a8
commit 3afa755680
7 changed files with 53 additions and 54 deletions
Download Patch File Download Diff File Expand all files Collapse all files

47
modules/motor/DJImotor/dji_motor.c
View File

@@ -20,7 +20,6 @@ static DJIMotorInstance *dji_motor_instance[DJI_MOTOR_CNT] = {NULL}; // 会在co
* can2: [3]:0x1FF,[4]:0x200,[5]:0x2FF
*/
static CANInstance sender_assignment[6] = {
//[0] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[0] = {.can_handle = &hcan1, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[1] = {.can_handle = &hcan1, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
[2] = {.can_handle = &hcan1, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}},
@@ -52,12 +51,12 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
if (motor_id < 4) // 根据ID分组
{
motor_send_num = motor_id;
motor_grouping = config->can_handle == &hcan2 ? 1 : 4;
motor_grouping = config->can_handle == &hcan1 ? 1 : 4;
}
else
{
motor_send_num = motor_id - 4;
motor_grouping = config->can_handle == &hcan2 ? 0 : 3;
motor_grouping = config->can_handle == &hcan1 ? 0 : 3;
}
// 计算接收id并设置分组发送id
@@ -72,7 +71,7 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan2 ? 1 : 2;
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
@@ -91,6 +90,22 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf
motor_grouping = config->can_handle == &hcan1 ? 2 : 5;
}
config->rx_id = 0x204 + motor_id + 1; // 把ID+1,进行分组设置
sender_enable_flag[motor_grouping] = 1; // 只要有电机注册到这个分组,置为1;在发送函数中会通过此标志判断是否有电机注册
motor->message_num = motor_send_num;
motor->sender_group = motor_grouping;
for (size_t i = 0; i < idx; ++i)
{
if (dji_motor_instance[i]->motor_can_instance->can_handle == config->can_handle && dji_motor_instance[i]->motor_can_instance->rx_id == config->rx_id)
{
LOGERROR("[dji_motor] ID crash. Check in debug mode, add dji_motor_instance to watch to get more information.");
uint16_t can_bus = config->can_handle == &hcan1 ? 1 : 2;
while (1) // 6020的id 1-4和2006/3508的id 5-8会发生冲突(若有注册,即1!5,2!6,3!7,4!8) (1!5!,LTC! (((不是)
LOGERROR("[dji_motor] id [%d], can_bus [%d]", config->rx_id, can_bus);
}
}
break;
case DM4310:
if (motor_id < 4) // 根据ID分组
{
@@ -292,34 +307,34 @@ void DJIMotorControl()
pid_ref = PIDCalculate(&motor_controller->speed_PID, pid_measure, pid_ref);
}
//计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
// 计算电流环,目前只要启用了电流环就计算,不管外层闭环是什么,并且电流只有电机自身传感器的反馈
if (motor_setting->feedforward_flag & CURRENT_FEEDFORWARD)
pid_ref += *motor_controller->current_feedforward_ptr;
if (motor_setting->close_loop_type & CURRENT_LOOP)
{
pid_ref = PIDCalculate(&motor_controller->current_PID, measure->real_current, pid_ref);
}
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1;
if (motor_setting->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE)
pid_ref *= -1;
// 获取最终输出
set = (int16_t)pid_ref;
// 分组填入发送数据
group = motor->sender_group;
num = motor->message_num;
//DM电机单独处理,因为它的发送格式与DJI电机不同,但仍然使用sender_assignment进行分组发送
if (i == 1)
{
sender_assignment[4].tx_buff[6] = (uint8_t)(set & 0x00ff); // 高八位
sender_assignment[4].tx_buff[7] = (uint8_t)(set >> 8); // 低八位
}
else
{
sender_assignment[group].tx_buff[2 * num] = (uint8_t)(set >> 8); // 低八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
sender_assignment[group].tx_buff[2 * num + 1] = (uint8_t)(set & 0x00ff); // 高八位
}
// 若该电机处于停止状态,直接将buff置零