增加模式控制，日志记录

仿线模式禁用遥控偏航控制

ArduCopter/mode_guided.cpp

@@ -169,7 +169,7 @@ void ModeGuided::wp_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
+    if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -648,7 +648,7 @@ void ModeGuided::pos_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
+    if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -707,7 +707,7 @@ void ModeGuided::accel_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
+    if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -771,7 +771,7 @@ void ModeGuided::velaccel_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
+    if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {
@@ -847,7 +847,7 @@ void ModeGuided::posvelaccel_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!copter.failsafe.radio && use_pilot_yaw()) {
+    if (!copter.failsafe.radio && use_pilot_yaw() && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {

ArduCopter/zr_app.cpp

@@ -186,6 +186,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
@@ -294,6 +295,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("APID", "TimeUS,Mode,Type,X,Y,Z,Yaw", "QBBiiif",
+                            AP_HAL::micros64(), //Q
+                            mode, //B
+                            flight_control_parser.msg.type, //B
+                            flight_control_parser.msg.x,    //i
+                            flight_control_parser.msg.y,    //i
+                            flight_control_parser.msg.z,    //i
+                            flight_control_parser.msg.yaw); //f
+
     last_time = control_data_last_time;
     WITH_SEMAPHORE(sem);
     if(flight_control_parser.msg.head != MSG_HEAD){ // 帧头
@@ -316,7 +326,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()){
 
@@ -324,7 +334,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,   // 飞行控制：速度控制模式
         
     };