Loiter模式也加入下降姿态控制锁定

ArduCopter/mode.cpp

@@ -684,24 +684,24 @@ void Mode::land_run_horizontal_control()
 
 void Mode::land_lock_att_control(float *p_roll,float *p_pitch,float *p_yaw_rate)
 {
-    float *target_roll = p_roll;
-    float *target_pitch = p_pitch;
-    float *target_yaw_rate = p_yaw_rate;
     static uint8_t prt_once;
     #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
         bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
                                  g.land_lock_alt &&  \
-                                (copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
+                                (copter.control_mode == Mode::Number::LOITER || copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
         if(!prt_once && land_lock_attitude)
             gcs().send_text(MAV_SEVERITY_INFO, "land lock sitl mode");
     #else
 
     bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 2) && \
                                 copter.rangefinder_alt_ok() && g.land_lock_alt &&  \
-                                (copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
+                                (copter.control_mode == Mode::Number::LOITER || copter.control_mode == Mode::Number::LAND || copter.control_mode == Mode::Number::RTL || copter.control_mode == Mode::Number::AUTO);
     #endif
     if(land_lock_attitude)
     {
+        float *target_roll = p_roll;
+        float *target_pitch = p_pitch;
+        float *target_yaw_rate = p_yaw_rate;
         if(prt_once == 0)
         {
             gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode");

ArduCopter/mode_loiter.cpp

@@ -95,12 +95,12 @@ void ModeLoiter::run()
         // convert pilot input to lean angles
         get_pilot_desired_lean_angles(target_roll, target_pitch, loiter_nav->get_angle_max_cd(), attitude_control->get_althold_lean_angle_max());
 
+        // 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); //降落锁定判断
         // process pilot's roll and pitch input
         loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);
 
-        // get pilot's desired yaw rate
-        target_yaw_rate = get_pilot_desired_yaw_rate(channel_yaw->get_control_in());
-
         // get pilot desired climb rate
         target_climb_rate = get_pilot_desired_climb_rate(channel_throttle->get_control_in());
         // target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);

hexa-zrv4.sh

@@ -1,3 +1,3 @@
 ./waf configure --board zr-hexa
 ./waf copter
-cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.9-rc12.px4
+cp ./build/zr-hexa/bin/arducopter.apj /mnt/f/_01-work/100=data/固件/zr-v4/六轴_v4.0.16-rc2.px4