增加绕8字功能

ArduCopter/Parameters.cpp

@@ -155,6 +155,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_circle", 1),    // 是否使用绕8字
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT
     // @DisplayName: RTL Altitude

ArduCopter/Parameters.h

@@ -395,6 +395,7 @@ public:
         k_param_zr_use_rc,
         k_param_hardware_flag,
         k_param_zr_reg_date,
+        k_param_zr_8_circle,
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
@@ -510,6 +511,7 @@ public:
     AP_Int16                 zr_rtl_delay;
     AP_Int8                  zr_use_rc;
     AP_Int32                 zr_reg_date;
+    AP_Int8                  zr_8_circle;
     
     // Note: keep initializers here in the same order as they are declared
     // above.

ArduCopter/UserCode.cpp

@@ -97,6 +97,26 @@ void Copter::userhook_auxSwitch1(uint8_t ch_flag)
     //         break;
     //     }
     // }
+    switch (ch_flag) {
+        case 2: {
+            // relay.on(2);
+            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 2!");
+            in_debug_mode = 1;
+            break;
+        }
+        // case 1: {
+        //     // relay.on(2);
+        //     gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode 1!");
+        //     in_debug_mode = 1;
+        //     break;
+        // }
+        case 0: {
+            // relay.off(2);
+            gcs().send_text(MAV_SEVERITY_INFO,"NOTICE: debug mode off!");
+            in_debug_mode = 0;
+            break;
+        }
+    }
 #if CAM_DEBUG      
     switch (ch_flag) {
         case 2: {

ArduCopter/mode_auto.cpp

@@ -794,7 +794,24 @@ void ModeAuto::takeoff_run()
             }     
         }   
     }
-    
+
+    if(g.zr_8_circle){  // 起飞到高度绕8字
+        const bool reached_wp_dest = copter.wp_nav->reached_wp_destination();
+        static uint8_t init_circle = false;
+        if(reached_wp_dest){
+            if(!init_circle){
+                copter.circle_nav->set_radius(5 * 100.0f);
+                copter.circle_nav->init();
+                init_circle = true;
+                gcs().send_text(MAV_SEVERITY_WARNING,"auto init_circle");
+            }else{
+                circle_run();
+                return;
+            }
+        }else{
+            init_circle = false;
+        }
+    }
     auto_takeoff_run();
 }
 
@@ -1573,7 +1590,11 @@ bool ModeAuto::verify_takeoff()
         copter.landinggear.set_position(AP_LandingGear::LandingGear_Retract);
     }
 
-    return reached_wp_dest;
+    if(g.zr_8_circle){  // 起飞结束判断，如果绕8字，则增加8字结束判断
+        return (reached_wp_dest && copter.circle_nav->get_circle_8_finish());
+    }else{
+        return reached_wp_dest;
+    }
 }
 
 // verify_land - returns true if landing has been completed

ArduCopter/mode_circle.cpp

@@ -57,7 +57,15 @@ void ModeCircle::run()
     motors->set_desired_spool_state(AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED);
 
     // run circle controller
-    copter.circle_nav->update();
+    if(g.zr_8_circle){
+        bool ret = copter.circle_nav->turn_2_circle();
+        if(ret){
+            set_mode(Mode::Number::LOITER, ModeReason::REQUEST_CMD);
+        }
+    }else{
+        copter.circle_nav->update();
+    }
+    
 
     // call attitude controller
     if (pilot_yaw_override) {

ArduCopter/mode_rtl.cpp

@@ -45,6 +45,45 @@ void ModeRTL::run(bool disarm_on_land)
         return;
     }
 
+    if(g.zr_8_circle){
+        static bool init_circle = false;
+        static bool finish_circle = false;
+        if(_state != RTL_LoiterAtHome){
+            init_circle = false;
+            finish_circle = false;
+        }else{
+            if(!init_circle){
+                copter.circle_nav->set_radius(5 * 100.0f);
+                copter.circle_nav->init();
+                init_circle = true;
+                gcs().send_text(MAV_SEVERITY_WARNING,"rtl init_circle,arm yaw:%ld",copter.initial_armed_bearing);
+                // auto_yaw.set_mode(AUTO_YAW_LOOK_AHEAD);
+                auto_yaw.set_mode(AUTO_YAW_HOLD);
+                return;
+            }else{
+                if(!finish_circle){
+                    bool ret = copter.circle_nav->turn_2_circle();
+                    
+                    // call z-axis position controller
+                    pos_control->update_z_controller();
+                    // roll & pitch from waypoint controller, yaw rate from pilot
+                    if(!ret){
+                        attitude_control->input_euler_angle_roll_pitch_yaw(copter.circle_nav->get_roll(), copter.circle_nav->get_pitch(), copter.circle_nav->get_yaw(), true);
+                    }else{
+                        // roll, pitch from waypoint controller, yaw heading from auto_heading()
+                        auto_yaw.set_mode(AUTO_YAW_RESETTOARMEDYAW);
+                        attitude_control->input_euler_angle_roll_pitch_yaw(wp_nav->get_roll(), wp_nav->get_pitch(), auto_yaw.yaw(),true);
+                    }
+                    // check if heading is within 2 degrees of heading when vehicle was armed
+                    if (ret && abs(wrap_180_cd(ahrs.yaw_sensor-copter.initial_armed_bearing)) <= 200) {
+                        finish_circle = true;
+                    }
+                    return;
+                }
+            }
+        }
+    }
+
     // check if we need to move to next state
     if (_state_complete) {
         switch (_state) {

ArduCopter/version.h

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

libraries/AC_WPNav/AC_Circle.cpp

@@ -67,6 +67,11 @@ void AC_Circle::init(const Vector3f& center)
 
     // set start angle from position
     init_start_angle(false);
+    // 绕8字初始化
+    stage = 0;
+    is_cw_turn = true;
+    _circle_8_finish =  false;
+    // gcs().send_text(MAV_SEVERITY_INFO, "--- AC_Circle x:%.2f,y:%.2f \n",_center.x,_center.y);
 }
 
 /// init - initialise circle controller setting center using stopping point and projecting out based on the copter's heading
@@ -95,6 +100,10 @@ void AC_Circle::init()
 
     // set starting angle from vehicle heading
     init_start_angle(true);
+    // 绕8字初始化
+    stage = 0;
+    is_cw_turn = true;
+    _circle_8_finish =  false;
 }
 
 /// set_circle_rate - set circle rate in degrees per second
@@ -106,6 +115,127 @@ void AC_Circle::set_rate(float deg_per_sec)
     }
 }
 
+/**
+ * @brief 执行绕8字
+ * 
+ * @return true 
+ * @return false 
+ */
+bool AC_Circle::turn_2_circle()
+{
+    // calculate dt
+    bool ret = false;
+    _circle_8_finish =  false;
+    float dt = _pos_control.time_since_last_xy_update();//位置更新时间，单位是s
+    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;
+    float _fabs_total; // circle count,float
+    // calculate target position
+    Vector3f target;
+    const Vector3f curr_pos = _inav.get_position();
+    Vector3f new_pos;
+    float bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
+    static float  last_bearing_rad ;  
+    const Vector3f& stopping_point = _pos_control.get_pos_target();
+    switch (stage)
+    {
+    case 0:
+        // _angle = wrap_PI(_ahrs.yaw-M_PI);
+        _pos_control.set_desired_accel_xy(0.0f,0.0f);
+        _pos_control.set_desired_velocity_xy(0.0f,0.0f);
+        _pos_control.init_xy_controller();
+        // set initial position target to reasonable stopping point
+        _pos_control.set_target_to_stopping_point_xy();
+        _pos_control.set_target_to_stopping_point_z();
+        // get stopping point
+        // set circle center to circle_radius ahead of stopping point
+        _center.x = stopping_point.x + _radius * _ahrs.cos_yaw();
+        _center.y = stopping_point.y + _radius * _ahrs.sin_yaw();
+        _center.z = stopping_point.z;
+        // calculate velocities
+        calc_velocities(true);
+        // set starting angle from vehicle heading
+        init_start_angle(true);
+        last_bearing_rad = bearing_rad;
+        stage = 1;
+        break;
+    case 1:
+        {
+            _angle += angle_change;
+            _angle = wrap_PI(_angle);
+            _angle_total += angle_change;
+            _fabs_total = fabsf(get_angle_total()/M_2PI);
+            target.x = _center.x + _radius * cosf(_angle);
+            target.y = _center.y - _radius * sinf(_angle);
+            if(fabs(last_bearing_rad - bearing_rad) < 0.01 && _fabs_total > 0.9){
+                _center.x = _center.x + 2 * _radius* cosf(bearing_rad);
+                _center.y = _center.y + 2 * _radius* sinf(bearing_rad);
+                last_bearing_rad = atan2f(curr_pos.y-_center.y,curr_pos.x-_center.x);
+                is_cw_turn = false;
+                // calculate velocities
+                calc_velocities(false);
+                init_start_angle(false);
+                _angle_total = 0;
+                stage = 2;
+            }
+        }
+        break;
+    case 2:
+        {
+            _angle += angle_change;
+            _angle = wrap_PI(_angle);
+            _angle_total += angle_change;
+            _fabs_total = fabsf(get_angle_total()/M_2PI);
+            target.x = _center.x + _radius * cosf(-_angle);
+            target.y = _center.y - _radius * sinf(-_angle);
+            if(fabs(last_bearing_rad - bearing_rad) < 0.01  && _fabs_total > 0.9){
+                _angle_total = 0;
+                stage = 3;
+            }
+        }
+        break;
+    case 3:
+        // _angle = wrap_PI(_ahrs.yaw-M_PI);
+        // gcs().send_text(MAV_SEVERITY_INFO, "[c]finish angle::%.2f,y:%.2f \n",_angle,_ahrs.yaw);
+        stage = 4;
+        break;
+    case 4:
+        ret = true;
+        _circle_8_finish = true;
+        break;
+    
+    default:
+        break;
+    }
+
+        target.z = _pos_control.get_alt_target();
+        // update position controller target
+        _pos_control.set_xy_target(target.x, target.y);
+        // heading is from vehicle to center of circle
+        _yaw = get_bearing_cd(_inav.get_position(), _center);
+        if(is_cw_turn){
+            _yaw += 9000;
+        }else{
+            _yaw += 27000;
+        }
+    // update position controller更新位置控制器
+    _pos_control.update_xy_controller();
+    return ret;
+}
+
 /// update - update circle controller
 void AC_Circle::update()
 {

libraries/AC_WPNav/AC_Circle.h

@@ -45,6 +45,8 @@ public:
 
     /// update - update circle controller
     void update();
+    bool turn_2_circle();   // 一键8字用
+    bool get_circle_8_finish() const { return _circle_8_finish; }   // 返回是否结束绕8字
 
     /// get desired roll, pitch which should be fed into stabilize controllers
     float get_roll() const { return _pos_control.get_roll(); }
@@ -101,4 +103,8 @@ private:
     float       _angular_vel;   // angular velocity in radians/sec
     float       _angular_vel_max;   // maximum velocity in radians/sec
     float       _angular_accel; // angular acceleration in radians/sec/sec
+
+    bool is_cw_turn;        // 绕8字方向
+    bool _circle_8_finish;  // 绕8字完成
+    uint8_t stage;          // 绕8字阶段
 };