1、更新步科驱动

applications/kinco.c

@@ -6,6 +6,7 @@
 对外提供结构体查询
 在线计时
 底层 处理完毕
+先给工作模式，再给控制字86 ，再给F，再给转速
  * @version: 
  * @Author: Joe
  * @Date: 2021-11-13 21:55:10
@@ -13,7 +14,7 @@
  */
 
 #include "kinco.h"
-
+#include "chassis_bus.h"
 
 #define DBG_TAG                        "kinco"
 #define DBG_LVL                        DBG_INFO
@@ -104,18 +105,38 @@ uint8_t get_motor_mode(void)
 struct rt_can_msg send_motor_speed_mode(void)   
 {	
 	struct rt_can_msg tx_msg;
-	tx_msg.id = motor.id + 0x100;
+	tx_msg.id = motor.id + 0x600;
 	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
 	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
-	tx_msg.len = 8;                /* 数据长度为 8 */
-	tx_msg.data[0] = 0x00;		   /* 源地址 */
-	tx_msg.data[1] = 0x1e;		   /* 功能码 */
-	tx_msg.data[2] = 0x00;     	   /* 寄存器地址 */	
-	tx_msg.data[3] = 0x03;	       /* 寄存器地址 */	
-	tx_msg.data[4] = 0x00;	   /* 值 */
-	tx_msg.data[5] = mode_speed;		   /* 值 */
+	tx_msg.len = 6;                /* 数据长度为 8 */
+	tx_msg.data[0] = 0x2F;		   /* 源地址 */
+	tx_msg.data[1] = 0x60;		   /* 功能码 */
+	tx_msg.data[2] = 0x60;     	   /* 寄存器地址 */	
+	tx_msg.data[3] = 0x00;	       /* 寄存器地址 */	
+	tx_msg.data[4] = 0x03;	   /* 值 */
+	tx_msg.data[5] = 0x00;		   /* 值 */
 	return	tx_msg;
 }
+
+struct rt_can_msg send_motor_control_mode(void)   
+{	
+	struct rt_can_msg tx_msg;
+	tx_msg.id = motor.id + 0x600;
+	tx_msg.ide = RT_CAN_STDID;     /* 标准格式 */
+	tx_msg.rtr = RT_CAN_DTR;       /* 数据帧 */
+	tx_msg.len = 6;                /* 数据长度为 8 */
+	tx_msg.data[0] = 0x2B;		   /* 源地址 */
+	tx_msg.data[1] = 0x40;		   /* 功能码 */
+	tx_msg.data[2] = 0x60;     	   /* 寄存器地址 */	
+	tx_msg.data[3] = 0x00;	       /* 寄存器地址 */	
+	tx_msg.data[4] = 0x0F;	   /* 值 */
+	tx_msg.data[5] = 0x00;		   /* 值 */
+	return	tx_msg;
+}
+
+
+
+
 #endif /* SEND_MODE_EN */ 
 
 
@@ -132,10 +153,7 @@ void set_motor_rpm(int16_t rpm)
 {
 	 motor.rpm = rpm;		
 }
-int16_t get_motor_real_rpm(void)
-{
-	 return	motor.real_rpm;		
-}
+
 struct rt_can_msg send_motor_rpm(void)   
 {	
 	struct rt_can_msg tx_msg;
@@ -157,23 +175,47 @@ struct rt_can_msg send_motor_rpm(void)
 }
 #endif /* SEND_RPM_EN */ 
 
+uint8_t motor_init_ok(void)
+{
+	struct rt_can_msg msg;	
+	if(motor.mode != 3)
+	{
+		msg = send_motor_speed_mode();
+		chassis_bus_write(&msg, sizeof(msg));
+		return 1;
+	}
+	else
+	{
+		if(motor.control != 0X0F)
+		{
+			msg = send_motor_control_mode();
+			chassis_bus_write(&msg, sizeof(msg));
+			return 1;
+		}
+		else
+		return 0;		
+	}
+}
+
+
 
 uint8_t motor_msg_parse(struct rt_can_msg msg)
 {
     uint8_t temp = 1;
+	uint16_t svd;
 	if(msg.ide!=RT_CAN_STDID)
 	return temp;
-	if(msg.id == motor.id + 0x600)	/* 定时上传 */
+	if(msg.id == motor.id + 0x580)	/* 定时上传 */
 	{
-		motor.miss_cnt = 0;
 		temp = 0;
-		switch(msg.data[1])/* 功能码 */
+		svd = (msg.data[1]<<8) + msg.data[2];
+		switch(svd)/* 功能码 */
 		{
-			case 0xca:
-				motor.real_rpm = (msg.data[4]<<8) + msg.data[5];	//转速
+			case 0x6060:	//工作模式
+				motor.mode = msg.data[4];	//
 			break;
-            case 0xcb: 
-				motor.err = (msg.data[2]<<8) + msg.data[3];	//故障码
+            case 0x4060: //控制字
+				motor.control = msg.data[4];	//
 			break;
 			default:
 			break;
@@ -182,7 +224,6 @@ uint8_t motor_msg_parse(struct rt_can_msg msg)
 	else
 	if(msg.id == 0x0700)	/*即发即回:主站发送，从站回复，以0x700为id,功能码+1，进行状态和参数信息交换*/
 	{
-		motor.miss_cnt = 0;
 		temp = 0;
 		if(msg.data[0] == motor.id) /* 源地址 */
 		{
@@ -219,10 +260,6 @@ uint8_t get_motor_err(void)
 {
 	return motor.err;
 }
-uint8_t get_motor_miss_err(void)
-{
-	return	motor.miss_err;
-}
 struct rt_can_msg send_motor_reset(void)   
 {	
 	struct rt_can_msg tx_msg;
@@ -240,35 +277,13 @@ struct rt_can_msg send_motor_reset(void)
 	tx_msg.data[7] = 0x01;		  
 	return	tx_msg;
 }
-/****************************************
- *        检查失联    
- *函数功能 : 
- *参数描述 : 无
- *返回值   : 无
- ****************************************/
-void check_motor_miss(void)
-{
-    if(motor.enable)
-    {
-        motor.miss_cnt ++;
-        if(motor.miss_cnt > 10)
-        {
-            motor.miss_cnt = 0;
-            motor.miss_err = 1;	  
-        }			
-    }			
-}
 
 static void motor_param_init(void)
 {  
-    motor.enable = 0;
-    motor.miss_cnt = 0;
-    motor.miss_err = 0;
 	motor.mode = 0;
 	motor.id = 0x7a;   
     motor.err = 0;
     motor.rpm = 0;
-	motor.speed =0;
 }
 
 /****************************************

applications/kinco.h

@@ -20,15 +20,12 @@
 
 typedef struct
 {       
-	uint8_t  miss_cnt;    /*失联时间*/	
 	uint8_t  mode;  		/*电机模式*/    			
     uint16_t err;      		/*故障*/
 	int16_t rpm;      		/*转速*/
-	int16_t real_rpm;      	/*转速*/	
-	int32_t speed;      	/*速度*/
 	uint32_t id;	 		/*电机id*/
-	uint8_t  enable    	:1;  /*电机使能*/ 
-	uint8_t  miss_err  	:1;  /*失联错误*/
+	uint8_t  control;  		/*电机控制字*/   
+	
 	uint8_t  acc  	    :1;  /*设置好加减速*/
 	uint8_t  dcc  		:1;  /*设置好加减速*/
 	uint8_t 		   	:4;
@@ -47,7 +44,8 @@ struct rt_can_msg send_motor_dcc(void);
 #define mode_pos		0x06
 #define mode_speed		0x02
 uint8_t get_motor_mode(void);
-struct rt_can_msg send_motor_speed_mode(void); 
+struct rt_can_msg send_motor_speed_mode(void);
+struct rt_can_msg send_motor_control_mode(void) ;
 #endif /* SEND_MODE_EN */
 
 #if defined SEND_RPM_EN
@@ -61,5 +59,5 @@ uint8_t motor_msg_parse(struct rt_can_msg msg);
 void check_motor_miss(void);
 uint8_t get_motor_err(void);
 uint8_t get_motor_miss_err(void);
-
+uint8_t motor_init_ok(void);
 #endif

applications/nav/dl-dwd.c

@@ -483,9 +483,12 @@ static void DRProcess(void *param)	//驱动器进程
 		
 //        McWalkProcess();	
 		//步科电机
-		set_motor_rpm(Set.FWlkRpm);	//设置电机转速
-		msg = send_motor_rpm();
-		chassis_bus_write(&msg, sizeof(msg));
+		if(motor_init_ok()==0)
+		{
+			set_motor_rpm(Set.FWlkRpm);	//设置电机转速
+			msg = send_motor_rpm();
+			chassis_bus_write(&msg, sizeof(msg));		
+		}
 		bms_cnt++;
 		if(bms_cnt>150)	//增加奥冠的bms协议发送，每1200ms获取一次容量
 		{