将视觉通信转移到INStask之后，频率改为1kHz，cmd仅保留颜色和模式设置

modules/imu/ins_task.c

@@ -18,6 +18,8 @@
 #include "spi.h"
 #include "user_lib.h"
 #include "general_def.h"
+#include "master_process.h"
+
 static INS_t INS;
 static IMU_Param_t IMU_Param;
 static PIDInstance TempCtrl = {0};
@@ -152,6 +154,8 @@ void INS_Task(void)
         INS.Pitch = QEKF_INS.Pitch;
         INS.Roll = QEKF_INS.Roll;
         INS.YawTotalAngle = QEKF_INS.YawTotalAngle;
+        
+        VisionSetAltitude(INS.Yaw,INS.Pitch);
     }
 
     // temperature control