增加模式控制，日志记录

ArduCopter/zr_app.cpp

@@ -212,6 +212,34 @@ void Copter::zr_app_50hz(){
                         }
                     }
                     break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE:
+                {
+
+                    if(control_mode != Mode::Number::GUIDED){
+                        set_mode(Mode::Number::GUIDED, ModeReason::SCRIPTING);
+                    }
+                    uint8_t user_mode = (data.x);
+                    if(user_mode == 3 || user_mode == 4 || user_mode == 6 || user_mode == 17){
+                        bool change_ok = set_mode(user_mode, ModeReason::SCRIPTING);
+    
+                        if(zr_serial_api.get_param_debug() && change_ok){
+                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
+                            snprintf(buf, sizeof(buf), "Change mode %d",user_mode);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }else{
+                        if(zr_serial_api.get_param_debug()){
+                            // zr_serial_api.print_data("do takeoff,alt %.2f",tk_alt);
+                            snprintf(buf, sizeof(buf), "Unsupported mode %d",user_mode);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }
+                    
+                }
+                    break;
+                    
             
                 default:
                     break;

libraries/AC_ZR_APP/AC_ZR_Serial_API.cpp

@@ -20,6 +20,7 @@
 #include <string.h>
 #include <stdio.h>
 #include <AP_Math/crc.h>
+#include <AP_Logger/AP_Logger.h>
 
 
 #define ZR_API_DEBUG 1
@@ -300,6 +301,15 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
     if(control_data_last_time == last_time){    // 数据没更新，直接退出
         return false;
     }
+    AP::logger().Write("DATR", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBB",
+                            AP_HAL::micros64(),
+                            mode,
+                            flight_control_parser.msg.type,
+                            flight_control_parser.msg.x,
+                            flight_control_parser.msg.y,
+                            flight_control_parser.msg.z,
+                            flight_control_parser.msg.yaw);
+
     last_time = control_data_last_time;
     WITH_SEMAPHORE(sem);
     if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
@@ -322,7 +332,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
         return false; // 校验失败
     }
     type = flight_control_parser.msg.type;
-    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_VEL_P ){
+    if(type < ZR_Msg_Type::MSG_CONTROL_TKOFF || type > ZR_Msg_Type::MSG_CONTROL_MODE ){
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRTYPE); // 应答控制类型错误
         if(get_param_debug()){
 
@@ -330,7 +340,7 @@ bool AC_ZR_Serial_API::get_control_data(uint8_t mode, ZR_Msg_Type &type,Vector3l
         }
         return false; // 控制类型错误
     }
-    if(type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4){
+    if(type != ZR_Msg_Type::MSG_CONTROL_MODE && type != ZR_Msg_Type::MSG_CONTROL_TKOFF && mode != 4 ){
         set_control_ask(flight_control_parser.msg.msg_id,flight_control_parser.msg.type,ZR_Msg_ASK::MSG_ASK_ERRMODE); 
         if(get_param_debug()){
             Debug("mode error: %d",mode);

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -46,7 +46,8 @@ public:
         MSG_CONTROL_TKOFF , // 飞行控制：起飞
         MSG_CONTROL_POS ,   // 飞行控制：位置控制模式
         MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式
-        MSG_CONTROL_VEL_P   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_P,   // 飞行控制：速度控制模式
+        MSG_CONTROL_MODE,   // 飞行控制：速度控制模式
         
     };