起飞优化，绕8存在问题

ArduCopter/mode_auto.cpp

@@ -954,12 +954,27 @@ void ModeAuto::circle_run()
     if(g.zr_8_circle > 0){  // 起飞到高度绕8字
 
         int32_t initial_armed_bearing = wp_yaw_cd;
+        static autopilot_yaw_mode last_yaw_mode;
         if(circle_init_yaw == false){   // 如果需要转向到下一个航点
             if (abs(wrap_180_cd(ahrs.yaw_sensor-initial_armed_bearing)) <= 200) {   //转到角度
                 circle_init_yaw = true;
+                auto_yaw.set_mode(last_yaw_mode);
                 gcs().send_text(MAV_SEVERITY_INFO, "circ init: y: %d -- %d ",initial_armed_bearing,ahrs.yaw_sensor);
             }else{
-                attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing ,true);
+                
+                if(wp_nav->set_wp_destination(last_loc)) {
+                    if (auto_yaw.mode() != AUTO_YAW_FIXED) {
+                        last_yaw_mode = auto_yaw.mode();
+                        gcs().send_text(MAV_SEVERITY_INFO, "auto_yaw: y: %d ",auto_yaw.mode());
+                        auto_yaw.set_mode(AUTO_YAW_FIXED);
+                    }
+                    attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), initial_armed_bearing,true);
+                }else{
+                    
+                    circle_init_yaw = true;
+                    gcs().send_text(MAV_SEVERITY_INFO, "--- circle turn false ---");
+                }
+                
                 return;
             }
         }else{ // 转向完成后，开始绕8字

ArduCopter/version.h

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

hrs100h.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100h
 ./waf copter
-cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100h-v4.0.16.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/rs100h-v4.0.17-RC3.px4

libraries/AC_WPNav/AC_Circle.cpp

@@ -365,6 +365,50 @@ void AC_Circle::update()
     _pos_control.update_xy_controller();
 }
 
+
+/// update - update circle controller
+void AC_Circle::turn_yaw(const Vector3f& center)
+{
+    // calculate dt
+    float dt = _pos_control.time_since_last_xy_update();
+    if (dt >= 0.2f) {
+        dt = 0.0f;
+    }
+
+    // ramp angular velocity to maximum
+    if (_angular_vel < _angular_vel_max) {
+        _angular_vel += fabsf(_angular_accel) * dt;
+        _angular_vel = MIN(_angular_vel, _angular_vel_max);
+    }
+    if (_angular_vel > _angular_vel_max) {
+        _angular_vel -= fabsf(_angular_accel) * dt;
+        _angular_vel = MAX(_angular_vel, _angular_vel_max);
+    }
+
+    // update the target angle and total angle traveled
+    float angle_change = _angular_vel * dt;
+    _angle += angle_change;
+    _angle = wrap_PI(_angle);
+    _angle_total += angle_change;
+
+    // set target position to center
+    Vector3f target;
+    target.x = center.x;
+    target.y = center.y;
+    target.z = _pos_control.get_alt_target();
+
+    // update position controller target
+    _pos_control.set_xy_target(target.x, target.y);
+
+    // heading is same as _angle but converted to centi-degrees
+    _yaw = _angle * DEGX100;
+    
+
+    // update position controller
+    _pos_control.update_xy_controller();
+}
+
+
 // get_closest_point_on_circle - returns closest point on the circle
 //  circle's center should already have been set
 //  closest point on the circle will be placed in result

libraries/AC_WPNav/AC_Circle.h

@@ -80,6 +80,7 @@ public:
 
     bool run_8_circle(bool reached);
     uint8_t run_rtl_8_circle(uint8_t *step);
+    void turn_yaw(const Vector3f& center);
     static const struct AP_Param::GroupInfo var_info[];
     
 private:

rs100.sh

@@ -1,3 +1,3 @@
 ./waf configure --board rs100
 ./waf copter
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/wsl/rs100-v4.0.16.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/zr-v4/rs100-v4.0.17-RC3.px4