控制模式调整

ArduCopter/mode_auto.cpp

@@ -888,7 +888,7 @@ void ModeAuto::wp_run()
     }
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !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)) {
@@ -935,7 +935,7 @@ void ModeAuto::spline_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
         // get pilot's desired yaw rat
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
         if (!is_zero(target_yaw_rate)) {

ArduCopter/mode_guided.cpp

@@ -386,7 +386,7 @@ void Mode::auto_takeoff_run()
 
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !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());
     }
@@ -417,7 +417,7 @@ void ModeGuided::pos_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !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)) {
@@ -459,7 +459,7 @@ void ModeGuided::vel_control_run()
 {
     // process pilot's yaw input
     float target_yaw_rate = 0;
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !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)) {
@@ -512,7 +512,7 @@ void ModeGuided::posvel_control_run()
     // process pilot's yaw input
     float target_yaw_rate = 0;
 
-    if (!copter.failsafe.radio && copter.zr_serial_api.data_trans_init) {   //copter.zr_serial_api.data_trans_init 仿线模式启动，禁用遥控控制
+    if (!copter.failsafe.radio && !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/version.h

@@ -9,14 +9,14 @@
 #undef GIT_VERSION
 #define GIT_VERSION "Beta"
 #endif
-#define THISFIRMWARE "ZRUAV v4.3.1"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.3.2"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
-#define FIRMWARE_VERSION 4,3,1,FIRMWARE_VERSION_TYPE_OFFICIAL
+#define FIRMWARE_VERSION 4,3,2,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 3
-#define FW_PATCH 1
+#define FW_PATCH 2
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL
 /**
  * AVDv6: avoid.get_zr_mode() = 3时可以用Loiter模式测试自动避障

ArduCopter/zr_app.cpp

@@ -212,6 +212,24 @@ 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,lock yaw",target_vel.x,target_vel.y,target_vel.z);
+                            zr_serial_api.print_msg(buf);
+                            memset(buf,0,50);
+                        }
+                    }
+                    break;
                 case AC_ZR_Serial_API::ZR_Msg_Type::MSG_CONTROL_MODE:
                 {
 

libraries/AC_ZR_APP/AC_ZR_Serial_API.h

@@ -45,8 +45,9 @@ public:
         MSG_VEHICLE_STATUS,  //  飞机状态
         MSG_CONTROL_TKOFF , // 飞行控制：起飞
         MSG_CONTROL_POS ,   // 飞行控制：位置控制模式
-        MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL ,   // 飞行控制：速度控制模式，大地坐标系
         MSG_CONTROL_VEL_P,   // 飞行控制：速度控制模式
+        MSG_CONTROL_VEL_LOCK_YAW ,   // 悬停，锁定航向
         MSG_CONTROL_MODE,   // 飞行控制：速度控制模式
         
     };

rs100.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100
 ./waf copter
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100-sdk-v4.3.0-dev-FE.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/rs100f-sdk-v4.3.2-beta.px4