新增导航通信测试

application/cmd/robot_cmd.c

@@ -129,10 +129,10 @@ static void RemoteControlSet()
     }
 
     // 云台参数,确定云台控制数据
-    if (switch_is_mid(rc_data[TEMP].rc.switch_left) ||
+    if (switch_is_mid(rc_data[TEMP].rc.switch_left) )
-        (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+//        (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
-         && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
+//         && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 &&
-         vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
+//         vision_recv_data->vz == 0)) // 左侧开关状态为[中],或视觉未识别到目标,纯遥控器拨杆控制
     {
         // 待添加,视觉会发来和目标的误差,同样将其转化为total angle的增量进行控制
         // ...
@@ -145,44 +145,51 @@ static void RemoteControlSet()
 
     }
     // 左侧开关状态为[下],视觉模式
-    if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
+//    if (switch_is_down(rc_data[TEMP].rc.switch_left)) {
-        trajectory_cal.v0 = 30; //弹速30
+//        trajectory_cal.v0 = 30; //弹速30
-        if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
+//        if (vision_recv_data->x == 0 && vision_recv_data->y == 0 && vision_recv_data->z == 0
-            && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
+//            && vision_recv_data->vx == 0 && vision_recv_data->vy == 0 && vision_recv_data->vz == 0) {
-            aim_select.suggest_fire = 0;
+//            aim_select.suggest_fire = 0;
-            //未发现目标
+//            //未发现目标
-            no_find_cnt++;
+//            no_find_cnt++;
+//
+//            if (no_find_cnt >= 2000) {
+//                //gimbal_scan_flag = 1;
+//                //auto_aim_flag = 0;
+//            }
+//            //else
+//            //auto_aim_flag = 1;
+//        } else {
+//            //弹道解算
+//            no_find_cnt = 0;
+//            auto_aim_flag = 1;
+//
+//            auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
+//
+//            gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
+//
+//            gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
+//
+//            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
+//            if (yaw_err <= 0.008) //3度
+//                aim_select.suggest_fire = 1;
+//            else
+//                aim_select.suggest_fire = 0;
+//        }
+//    }
 
-            if (no_find_cnt >= 2000) {
-                //gimbal_scan_flag = 1;
-                //auto_aim_flag = 0;
-            }
-            //else
-            //auto_aim_flag = 1;
-        } else {
-            //弹道解算
-            no_find_cnt = 0;
-            auto_aim_flag = 1;
 
-            auto_aim(&aim_select, &trajectory_cal, vision_recv_data);
-
-            gimbal_cmd_send.yaw = trajectory_cal.cmd_yaw * 180 / PI;
-
-            gimbal_cmd_send.pitch = trajectory_cal.cmd_pitch * 180 / PI;
-
-            float yaw_err = fabsf(trajectory_cal.cmd_yaw - gimbal_cmd_send.yaw);
-            if (yaw_err <= 0.008) //3度
-                aim_select.suggest_fire = 1;
-            else
-                aim_select.suggest_fire = 0;
-        }
-    }
     // 云台软件限位
 
     // 底盘参数,目前没有加入小陀螺(调试似乎暂时没有必要),系数需要调整
     chassis_cmd_send.vx = 0.001f * (float)rc_data[TEMP].rc.rocker_r1; // _水平方向
     chassis_cmd_send.vy = -0.001f * (float)rc_data[TEMP].rc.rocker_r_; // 1数值方向
 
+    if (vision_recv_data->nav_vx != 0 || vision_recv_data->nav_vy != 0 || vision_recv_data->nav_wz != 0) {
+        chassis_cmd_send.vx = vision_recv_data->nav_vx;
+        chassis_cmd_send.vy = vision_recv_data->nav_vy;
+        chassis_cmd_send.wz = vision_recv_data->nav_wz;
+    }
 
     // 发射参数
     if (switch_is_up(rc_data[TEMP].rc.switch_right)) // 右侧开关状态[上],弹舱打开

application/gimbal/gimbal.c

@@ -82,17 +82,17 @@ void GimbalInit()
         },
         .controller_param_init_config = {
             .angle_PID = {
-                .Kp = 2.0f,
+                .Kp = 2.0f,//4.0f,//2.0f
-                .Ki = 0,
+                .Ki = 0.0f,//1,//0
-                .Kd = 0.0f,
+                .Kd = 0.0f,//0.0f
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 100,
                 .MaxOut = 500,
             },
             .speed_PID = {
-                .Kp = 800,
+                .Kp = 800,//6000,//800
-                .Ki = 100,
+                .Ki = 100,//500,//100
-                .Kd = 0,
+                .Kd = 0,//0
                 .Improve = PID_Trapezoid_Intergral | PID_Integral_Limit | PID_Derivative_On_Measurement,
                 .IntegralLimit = 5000,
                 .MaxOut = 10000,

modules/master_machine/master_process.c

@@ -62,6 +62,8 @@ static void VisionOfflineCallback(void *id)
     USARTServiceInit(vision_usart_instance);
 #endif // !VISION_USE_UART
     LOGWARNING("[vision] vision offline, restart communication.");
+
+    memset(&recv_data,0,sizeof(recv_data));
 }
 
 #ifdef VISION_USE_UART
@@ -141,6 +143,7 @@ static void DecodeVision(uint16_t recv_len)
     {
         if(VerifyCRC16CheckSum(vis_recv_buff,sizeof(recv_data)))
         {
+            DaemonReload(vision_daemon_instance);
             memcpy(&recv_data,vis_recv_buff,sizeof(recv_data));
         }
     }

modules/master_machine/master_process.h

@@ -92,17 +92,20 @@ typedef struct {
     uint8_t id: 3;          // 0-outpost 6-guard 7-base
     uint8_t armors_num: 3;  // 2-balance 3-outpost 4-normal
     uint8_t reserved: 1;
-    float x;
+//    float x;
-    float y;
+//    float y;
-    float z;
+//    float z;
-    float yaw;
+//    float yaw;
-    float vx;
+//    float vx;
-    float vy;
+//    float vy;
-    float vz;
+//    float vz;
-    float v_yaw;
+//    float v_yaw;
-    float r1;
+//    float r1;
-    float r2;
+//    float r2;
-    float dz;
+//    float dz;
+    float nav_vx;
+    float nav_vy;
+    float nav_wz;
     uint16_t checksum;
 } RecievePacket_t;
 #pragma pack()

sentry_left.jdebug

@@ -22,12 +22,12 @@ void OnProjectLoad (void) {
   //
   // Dialog-generated settings
   //
-  Project.AddPathSubstitute ("D:/CLion/Project/sentry_left", "$(ProjectDir)");
-  Project.AddPathSubstitute ("d:/clion/project/sentry_left", "$(ProjectDir)");
   Project.SetDevice ("STM32F407IG");
   Project.SetHostIF ("USB", "17935099");
   Project.SetTargetIF ("SWD");
   Project.SetTIFSpeed ("4 MHz");
+  Project.AddPathSubstitute ("D:/CLion/Project/sentry_left", "$(ProjectDir)");
+  Project.AddPathSubstitute ("d:/clion/project/sentry_left", "$(ProjectDir)");
   Project.AddSvdFile ("$(InstallDir)/Config/CPU/Cortex-M4F.svd");
   Project.AddSvdFile ("$(InstallDir)/Config/Peripherals/STM32F407IG.svd");
   //