diff --git a/application/gimbal/gimbal.c b/application/gimbal/gimbal.c index a6599b2..cbaea8a 100644 --- a/application/gimbal/gimbal.c +++ b/application/gimbal/gimbal.c @@ -96,7 +96,7 @@ void GimbalInit() .close_loop_type = SPEED_LOOP | ANGLE_LOOP, .motor_reverse_flag = MOTOR_DIRECTION_NORMAL, }, - .motor_type = M3508, + .motor_type = DM4310, }; // 电机对total_angle闭环,上电时为零,会保持静止,收到遥控器数据再动 yaw_motor = DJIMotorInit(&yaw_config); diff --git a/modules/motor/DJImotor/dji_motor.c b/modules/motor/DJImotor/dji_motor.c index 2568faa..a1fc96e 100644 --- a/modules/motor/DJImotor/dji_motor.c +++ b/modules/motor/DJImotor/dji_motor.c @@ -24,7 +24,7 @@ static CANInstance sender_assignment[6] = { [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}}, [3] = {.can_handle = &hcan2, .txconf.StdId = 0x1ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, - [4] = {.can_handle = &hcan2, .txconf.StdId = 0x200, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, + [4] = {.can_handle = &hcan2, .txconf.StdId = 0x3FE, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, [5] = {.can_handle = &hcan2, .txconf.StdId = 0x2ff, .txconf.IDE = CAN_ID_STD, .txconf.RTR = CAN_RTR_DATA, .txconf.DLC = 0x08, .tx_buff = {0}}, }; @@ -106,7 +106,36 @@ static void MotorSenderGrouping(DJIMotorInstance *motor, CAN_Init_Config_s *conf } } break; + case DM4310: + if (motor_id < 4) // 根据ID分组 + { + motor_send_num = motor_id; + motor_grouping = config->can_handle == &hcan2 ? 1 : 4; + } + else + { + motor_send_num = motor_id - 4; + motor_grouping = config->can_handle == &hcan2 ? 0 : 3; + } + // 计算接收id并设置分组发送id + config->rx_id = 0x300 + motor_id + 1; // 把ID+1,进行分组设置 + sender_enable_flag[motor_grouping] = 1; // 设置发送标志位,防止发送空帧 + motor->message_num = motor_send_num; + motor->sender_group = motor_grouping; + + // 检查是否发生id冲突 + 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 == &hcan2 ? 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; default: // other motors should not be registered here while (1) LOGERROR("[dji_motor]You must not register other motors using the API of DJI motor."); // 其他电机不应该在这里注册 @@ -313,3 +342,86 @@ void DJIMotorControl() } } } +void DM_MotorControl() +{ + // 直接保存一次指针引用从而减小访存的开销,同样可以提高可读性 + uint8_t group, num; // 电机组号和组内编号 + int16_t set; // 电机控制CAN发送设定值 + DJIMotorInstance *motor; + Motor_Control_Setting_s *motor_setting; // 电机控制参数 + Motor_Controller_s *motor_controller; // 电机控制器 + DJI_Motor_Measure_s *measure; // 电机测量值 + float pid_measure, pid_ref; // 电机PID测量值和设定值 + + // 遍历所有电机实例,进行串级PID的计算并设置发送报文的值 + for (size_t i = 0; i < idx; ++i) + { // 减小访存开销,先保存指针引用 + motor = dji_motor_instance[i]; + motor_setting = &motor->motor_settings; + motor_controller = &motor->motor_controller; + measure = &motor->measure; + pid_ref = motor_controller->pid_ref; // 保存设定值,防止motor_controller->pid_ref在计算过程中被修改 + if (motor_setting->motor_reverse_flag == MOTOR_DIRECTION_REVERSE) + pid_ref *= -1; // 设置反转 + + // pid_ref会顺次通过被启用的闭环充当数据的载体 + // 计算位置环,只有启用位置环且外层闭环为位置时会计算速度环输出 + if ((motor_setting->close_loop_type & ANGLE_LOOP) && motor_setting->outer_loop_type == ANGLE_LOOP) + { + if (motor_setting->angle_feedback_source == OTHER_FEED) + pid_measure = *motor_controller->other_angle_feedback_ptr; + else + pid_measure = measure->total_angle; // MOTOR_FEED,对total angle闭环,防止在边界处出现突跃 + // 更新pid_ref进入下一个环 + pid_ref = PIDCalculate(&motor_controller->angle_PID, pid_measure, pid_ref); + } + + // 计算速度环,(外层闭环为速度或位置)且(启用速度环)时会计算速度环 + if ((motor_setting->close_loop_type & SPEED_LOOP) && (motor_setting->outer_loop_type & (ANGLE_LOOP | SPEED_LOOP))) + { + if (motor_setting->feedforward_flag & SPEED_FEEDFORWARD) + pid_ref += *motor_controller->speed_feedforward_ptr; + + if (motor_setting->speed_feedback_source == OTHER_FEED) + pid_measure = *motor_controller->other_speed_feedback_ptr; + else // MOTOR_FEED + pid_measure = measure->speed_aps; + // 更新pid_ref进入下一个环 + 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->feedback_reverse_flag == FEEDBACK_DIRECTION_REVERSE) + pid_ref *= -1; + + // 获取最终输出 + set = (int16_t)pid_ref; + + + // 分组填入发送数据 + group = motor->sender_group; + num = motor->message_num; + sender_assignment[group].tx_buff[num + 6] = (uint8_t)(set & 0x00ff); // 高八位 + sender_assignment[group].tx_buff[num + 7] = (uint8_t)(set >> 8); // 低八位 + + // 若该电机处于停止状态,直接将buff置零 + if (motor->stop_flag == MOTOR_STOP) + memset(sender_assignment[group].tx_buff + 2 * num, 0, 2u); + } + + // 遍历flag,检查是否要发送这一帧报文 + for (size_t i = 0; i < 6; ++i) + { + if (sender_enable_flag[i]) + { + CANTransmit(&sender_assignment[i], 1); + } + } +} diff --git a/modules/motor/DJImotor/dji_motor.h b/modules/motor/DJImotor/dji_motor.h index bd07c1a..1419199 100644 --- a/modules/motor/DJImotor/dji_motor.h +++ b/modules/motor/DJImotor/dji_motor.h @@ -105,7 +105,7 @@ void DJIMotorChangeFeed(DJIMotorInstance *motor, Closeloop_Type_e loop, Feedback * @brief 该函数被motor_task调用运行在rtos上,motor_stask内通过osDelay()确定控制频率 */ void DJIMotorControl(); - +void DM_MotorControl(); /** * @brief 停止电机,注意不是将设定值设为零,而是直接给电机发送的电流值置零 * diff --git a/modules/motor/motor_task.c b/modules/motor/motor_task.c index d751b4a..891cc57 100644 --- a/modules/motor/motor_task.c +++ b/modules/motor/motor_task.c @@ -13,6 +13,7 @@ void MotorControlTask() if(cnt%2==0) //500hz { DJIMotorControl(); + DM_MotorControl(); } /* 如果有对应的电机则取消注释,可以加入条件编译或者register对应的idx判断是否注册了电机 */