测流调整，绕圈增加高度判断

ArduCopter/Copter.h

@@ -1076,6 +1076,7 @@ public:
     uint8_t mkdir_step;
     Mode::Number get_before_mode() const { return before_mode; };
     bool far_from_home;
+    bool alt_too_low;
     Proxy_Action avoid_action_step;
     void zr_set_battery_capacity();
     uint16_t mission_nav_index;

ArduCopter/Parameters.cpp

@@ -162,7 +162,6 @@ const AP_Param::Info Copter::var_info[] = {
     GSCALAR(zr_flow_type,  "ZR_FL_TP", 2),  // 0: 三合一 ,1: 单测流, 2:单测流指定水位
     GSCALAR(zr_flow_real_elevation,  "ZR_FL_RALT", 0),  //  直接设置水位高度，厘米
     GSCALAR(zr_flow_sens,  "ZR_FL_SENS", 0),  // 灵敏度 0:低，1：中，2：高 
-    GSCALAR(zr_flow_debug,  "ZR_FL_DEBG", 0),  // 灵敏度 0:低，1：中，2：高 
 
 #if MODE_RTL_ENABLED == ENABLED
     // @Param: RTL_ALT

ArduCopter/Parameters.h

@@ -396,7 +396,6 @@ public:
         k_param_zr_mission_type,
         k_param_zr_flow_type,
         k_param_zr_flow_sens,
-        k_param_zr_flow_debug,
         k_param_zr_flow_real_elevation,
         k_param_hardware_flag,
         k_param_zr_reg_date,
@@ -522,7 +521,6 @@ public:
     AP_Int8                 zr_mission_type;
     AP_Int8                 zr_flow_type;
     AP_Int8                 zr_flow_sens;
-    AP_Int8                 zr_flow_debug;
     
     AP_Int32                 zr_flow_real_elevation;
     AP_Int32                 zr_reg_date;

ArduCopter/UserCode.cpp

@@ -116,8 +116,10 @@ void Copter::userhook_SuperSlowLoop()
             }else{
                 far_from_home = true;
             }
+            alt_too_low = true;
         }else{
             far_from_home = true;    
+            alt_too_low = false;
         }
         avoid.set_enable_aviod(far_from_home); // 远离Home点才启用避障
         /// 手动避障开启提示
@@ -310,7 +312,6 @@ void Copter::zr_mission_flow_mode()
         int32_t real_alt;
         flow_measure.set_radar_mode(g.zr_flow_type);    //设置雷达模式，单测流或者三合一等
         flow_measure.set_radar_sens(g.zr_flow_sens);    //设置雷达灵敏度
-        // flow_measure.set_radar_debug(g.zr_flow_debug);    //设置雷达debug模式
         
         if(g.zr_flow_type == 2) // 单测流+指定水位高
             real_alt = g.zr_flow_real_elevation;  // 直接设置水位高度

ArduCopter/mode_circle.cpp

@@ -9,6 +9,11 @@
 // circle_init - initialise circle controller flight mode
 bool ModeCircle::init(bool ignore_checks)
 {
+    if(copter.alt_too_low){
+        gcs().send_text(MAV_SEVERITY_ERROR, "绕圈模式切换失败，飞行高度过低");
+        return false;
+    }
+    
     pilot_yaw_override = false;
     speed_changing = false;
 

ArduCopter/mode_rtl.cpp

@@ -44,7 +44,7 @@ void ModeRTL::run(bool disarm_on_land)
     if (!motors->armed()) {
         return;
     }
-if(g.zr_8_circle == 2 && copter.mission_nav_index > 1){
+if(g.zr_8_circle == 2 && copter.mission_nav_index > 1 && !copter.alt_too_low){
         static uint8_t flag = 0;
         if(_state == RTL_ReturnHome){
             // uint8_t flag = copter.circle_nav->run_rtl_8_circle(_state != RTL_LoiterAtHome);

ArduCopter/version.h

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

all.sh

@@ -4,32 +4,32 @@ starttime=`date +'%Y-%m-%d %H:%M:%S'`
 
 
 ./waf configure --board rs100 > build_log.txt
-./waf clean >> build_log.txt
+# ./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
-cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.0.17-RC4.px4
+cp ./build/rs100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.0.17-RC6.px4
 echo "finish build rs100 "
 ./waf configure --board rs100h >> build_log.txt
-./waf clean >> build_log.txt
+# ./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
-cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100h-v4.0.17-RC4.px4
+cp ./build/rs100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/致睿rs100h-v4.0.17-RC6.px4
 echo "finish build rs100h "
 
 ./waf configure --board d100 >> build_log.txt
-./waf clean >> build_log.txt
+# ./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
-cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.0.17-RC4.px4
+cp ./build/d100/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100-v4.0.17-RC6.px4
 echo "finish build d100 "
 
 ./waf configure --board d100h >> build_log.txt
-./waf clean >> build_log.txt
+# ./waf clean >> build_log.txt
 ./waf copter >> build_log.txt
-cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100h-v4.0.17-RC4.px4
+cp ./build/d100h/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/中海达d100h-v4.0.17-RC6.px4
 echo "finish build d100h "
 
 # ./waf configure --board zr-hexa >> build_log.txt
 # ./waf clean >> build_log.txt
 # ./waf copter >> build_log.txt
-# cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.0.17-RC4.px4
+# cp ./build/zr-hexa/bin/arducopter.apj  /mnt/f/_01-work/100\=data/固件/编译/六轴m66-v4.0.17-RC6.px4
 # echo "finish build zr-hexa "
 
 endtime=`date +'%Y-%m-%d %H:%M:%S'`

libraries/AC_Flow_Measure/AC_Flow_Measure.cpp

@@ -523,9 +523,6 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
             set_flow_date_flag(false);
         }
         
-        if(!index_sw || !start_sw){
-            data_to_mp.index = auto_index;
-        }
         switch (flow_update_state)
         {
 
@@ -564,7 +561,7 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                     flow_update_state = START_MEASURE;
                     // data_package_angle();
                     disable_data_ready();
-                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]设置灵敏度等级：%d",radar_sens + 1);
+                    gcs().send_text(MAV_SEVERITY_NOTICE, "[NOTE]设置灵敏度等级: %d",radar_sens + 1);
                     last_measure_sens = radar_sens;
                     repeat = 0;
                 }else if(repeat++ > 2){
@@ -623,7 +620,6 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                 {
                     if(index_sw){
                         manual_cnt++;
-                        set_wp_index(manual_cnt);
                     }
                     
                     if(last_measure_sens != radar_sens){
@@ -633,7 +629,11 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
                     }else{
                         flow_update_state = GET_ANGLE;
                     }
-
+                    if(!start_sw  && !index_sw){
+                        data_to_mp.index = auto_index;
+                    }else{
+                        data_to_mp.index = manual_cnt;
+                    }
                     get_gps_alt(alt + delt_alt);
                     if(radar_mode != 1){ // 非单测流雷达
                         data_package(); 
@@ -687,7 +687,6 @@ void Flow_Measure::flow_auto_measure(bool start_sw,bool index_sw,int32_t alt,uin
 
         if(!index_sw){
             manual_cnt = 0;
-            set_wp_index(manual_cnt);
         }
     }
 }

libraries/AC_Flow_Measure/AC_Flow_Measure.h

@@ -111,7 +111,6 @@ public:
     bool get_is_data_flag(){return is_data_flag;};
     bool get_radar_data(){return data_ready;};
     void disable_data_ready(){data_ready = false;};
-    void set_wp_index(uint16_t index){data_to_mp.index = index;};
     uint16_t get_wp_index(){return data_to_mp.index;};
     void data_package();
     uint8_t get_flow_update_state() {return flow_update_state;};

libraries/AP_Mission/AP_Mission.cpp

@@ -2073,7 +2073,7 @@ void AP_Mission::auto_resume_mission(uint8_t f_mode, bool is_arm,int32_t r_lat,i
         cmd_rtl.content.location.lng = r_lng;
         cmd_rtl.id = MAV_CMD_NAV_WAYPOINT;
         // cmd_rtl.content.location.options = 0;
-        cmd_rtl.p1 = 2;     // 到航点后延时2s
+        cmd_rtl.p1 = 1;     // 到航点后延时2s
         // cmd_rtl.content.location.flags.relative_alt = 1;
 
         replace_cmd(cmd_rtl_index, cmd_rtl);

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/rs100-v4.0.17-RC3.px4
+cp ./build/rs100/bin/arducopter.apj   /mnt/f/_01-work/100\=data/固件/编译/致睿rs100-v4.0.17-RC6.px4