loiter降落锁定调整,v4.0.16 - rc8

ArduCopter/Parameters.cpp

@@ -156,6 +156,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Standard
     GSCALAR(zr_rtl_delay,  "ZR_RTL_DELAY", ZR_RTL_DELAY),
     GSCALAR(zr_8_circle,  "ZR_8_CICLE", 0), // 是否启用绕8字
+    GSCALAR(zr_land_lock_att,  "ZR_ATT_LOCK", 1), // 是否启用锁定
 
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT

ArduCopter/Parameters.h

@@ -398,6 +398,7 @@ public:
         k_param_zr_avd_wait,
         k_param_zr_8_circle,
         k_param_zr_bat_cycled,
+        k_param_zr_land_lock_att,
 
         // the k_param_* space is 9-bits in size
         // 511: reserved
@@ -517,6 +518,7 @@ public:
     AP_Int16                 zr_avd_wait;
     AP_Int8                  zr_8_circle;
     AP_Int16                 zr_bat_cycled;
+    AP_Int8                  zr_land_lock_att;
     
     // Note: keep initializers here in the same order as they are declared
     // above.

ArduCopter/mode.cpp

@@ -682,7 +682,7 @@ void Mode::land_run_horizontal_control()
     }
 }
 
-void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
+bool Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
 {
     static uint8_t prt_once;
     #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
@@ -713,10 +713,13 @@ void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
                 copter.Log_Write_Event(DATA_LAND_LOCK_ATT);
                 prt_once = 1;
             }
+            
+            return true;
         }
     }else{
         prt_once = 0;
     }
+    return false;
 }
 
 

ArduCopter/mode.h

@@ -101,7 +101,7 @@ protected:
     // functions to control landing
     // in modes that support landing
     void land_run_horizontal_control();
-    void land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate);
+    bool land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate);
     void land_run_vertical_control(bool pause_descent = false);
 
     // return expected input throttle setting to hover:

ArduCopter/mode_loiter.cpp

@@ -78,6 +78,7 @@ void ModeLoiter::run()
     float target_yaw_rate = 0.0f;
     float target_climb_rate = 0.0f;
     float takeoff_climb_rate = 0.0f;
+    bool lock_att = false;
 
     // @Brown, loiter land auto slow down
     int16_t  land_speed_now = get_speed_dn_slow();
@@ -97,7 +98,9 @@ void ModeLoiter::run()
 
         // get pilot's desired yaw rate
         target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in()); // 提前到降落锁定判断前
-        land_lock_att_control(&target_roll,&target_pitch,&target_yaw_rate); //降落锁定判断
+        if(g.zr_land_lock_att){
+            lock_att = land_lock_att_control(&target_roll,&target_pitch,&target_yaw_rate); //降落锁定判断
+        }
         // process pilot's roll and pitch input
         loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
@@ -182,6 +185,10 @@ void ModeLoiter::run()
         // call attitude controller
         attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw(loiter_nav->get_roll(), loiter_nav->get_pitch(), target_yaw_rate);
 
+        if(g.zr_land_lock_att && lock_att && channel_throttle->get_control_in() < 300){
+            attitude_control->set_attitude_target_to_current_attitude();
+            attitude_control->reset_rate_controller_I_terms();
+        }
         // adjust climb rate using rangefinder
         target_climb_rate = copter.surface_tracking.adjust_climb_rate(target_climb_rate);
 

ArduCopter/mode_rtl.cpp

@@ -56,6 +56,7 @@ if(g.zr_8_circle == 2){
             }else if(flag == 3){
                 auto_yaw.set_mode(AUTO_YAW_LOOK_AT_NEXT_WP);
                 gcs().send_text(MAV_SEVERITY_INFO, "RTL circle finish");
+                flag = 4;
             }
         }else{
             flag = 0;

ArduCopter/version.h

@@ -6,7 +6,7 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.16-rc7"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.16-rc8"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,16,FIRMWARE_VERSION_TYPE_OFFICIAL

all.sh

@@ -1,20 +1,20 @@
 ./waf configure --board rs100
 ./waf clean
 ./waf copter
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC6.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100-v4.0.16-RC8.px4
 ./waf configure --board rs100h
 ./waf clean
 ./waf copter
-cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC6.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/致睿rs100h-v4.0.16-RC8.px4
 ./waf configure --board d100
 ./waf clean
 ./waf copter
-cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC6.px4
+cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100-v4.0.16-RC8.px4
 ./waf configure --board d100h
 ./waf clean
 ./waf copter
-cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC6.px4
+cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/中海达d100h-v4.0.16-RC8.px4
 ./waf configure --board zr-hexa
 ./waf clean
 ./waf copter
-cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC6.px4
+cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/六轴m66-v4.0.16-RC8.px4