数字绿土，增加停下功能

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,51 @@ void Copter::zr_app_50hz(){
                         }
                     }
                     break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_VEL_LOCK_YAW:
+                    if(motors->armed()){
+                        Vector3f target_vel;
+                        target_vel.x = 0;
+                        target_vel.y = 0;
+                        target_vel.z = 0;
+                        // target_loc.alt = (data.z + 500)/100.0;
+                        // set_target_velocity_NED(target_vel);
+                        mode_guided.set_velocity(target_vel,false,ahrs_yaw_cd,false,0.0f,false,true);
+                        if(zr_serial_api.get_param_debug()){
+
+                            // zr_serial_api.print_data("set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            snprintf(buf, sizeof(buf), "set vel %.2f,%.2f,%.2f",target_vel.x,target_vel.y,target_vel.z);
+                            gcs().send_text(MAV_SEVERITY_INFO,"%s",buf);
+                            memset(buf,0,50);
+                        }
+                    }
+                    break;
+                case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE:
+                {
+
+                    if(flightmode->mode_number() != 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,9 @@ public:
         MSG_CONTROL_TKOFF , // 飞行控制：起飞
         MSG_CONTROL_POS ,   // 飞行控制：位置控制模式
         MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式
-        MSG_CONTROL_VEL_P   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_P,   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_LOCK_YAW ,   // 飞行控制：速度控制模式，大地坐标系，锁定航向
+        MSG_CONTROL_MODE,   // 飞行控制：速度控制模式
         
     };
 

sitl.sh

@@ -1 +1 @@
- ./Tools/autotest/sim_vehicle.py  -j4 -L CYDS2 --console --out 192.168.0.51:14552 --map -A "--serial5=uart:/dev/pts/1:230400" -v ArduCopter
+ ./Tools/autotest/sim_vehicle.py  -j4 -L CYDS2 --console --out 192.168.0.51:14552 --map -A "--serial5=uart:/dev/ttyS1:230400" -v ArduCopter