land lock fix

ArduCopter/Parameters.cpp

@@ -254,7 +254,9 @@ const AP_Param::Info Copter::var_info[] = {
     // @User: Standard
     GSCALAR(land_speed,             "LAND_SPEED",    100),
     GSCALAR(land_speed_2nd,                "LAND_SPEED_2ND",    50),
-    GSCALAR(land_slow_2nd_alt,             "LAND_SL_2ND_ALT",    1000),
+    GSCALAR(land_speed_3nd,                "LAND_SPEED_3ND",    100),
+    GSCALAR(land_slow_2nd_alt,             "LAND_SL_2ND_ALT",    1500),
+    GSCALAR(land_slow_3nd_alt,             "LAND_SL_3ND_ALT",    3000),
     GSCALAR(land_lock_alt,             "LAND_LOCK_ALT",    100),
     GSCALAR(land_lock_rpy,             "LAND_LOCK_RPY",    1),
     

ArduCopter/Parameters.h

@@ -383,6 +383,8 @@ public:
 
         k_param_land_speed_2nd,
         k_param_land_slow_2nd_alt,
+        k_param_land_speed_3nd,
+        k_param_land_slow_3nd_alt,
         k_param_land_lock_alt,
         k_param_land_lock_rpy,
 
@@ -443,7 +445,9 @@ public:
 
 
     AP_Int16        land_speed_2nd;
+    AP_Int16        land_speed_3nd;
     AP_Int16        land_slow_2nd_alt;
+    AP_Int16        land_slow_3nd_alt;
     AP_Int8        land_lock_alt;
     AP_Int8        land_lock_rpy;
 

ArduCopter/config.h

@@ -533,7 +533,7 @@
 
 
 #ifndef TAKEOFF_ALT_REQ
- # define TAKEOFF_ALT_REQ           0       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
+ # define TAKEOFF_ALT_REQ           600       // the altitude the vehicle will move to as the final stage of Returning to Launch.  Set to zero to land.
 #endif
 
 #ifndef UPDOWN_COUNTDOWN

ArduCopter/events.cpp

@@ -93,14 +93,17 @@ void Copter::handle_battery_failsafe(const char *type_str, const int8_t action)
         // should immediately disarm when we're on the ground
         arming.disarm();
         desired_action = Failsafe_Action_None;
-        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
+        // gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Disarming");
+        gcs().send_text(MAV_SEVERITY_WARNING, "低电压保护 - 锁定");
 
     } else if (flightmode->is_landing() && failsafe_option(FailsafeOption::CONTINUE_IF_LANDING) && desired_action != Failsafe_Action_None) {
         // Allow landing to continue when FS_OPTIONS is set to continue when landing
         desired_action = Failsafe_Action_Land;
-        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
+        // gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe - Continuing Landing");
+        gcs().send_text(MAV_SEVERITY_WARNING, "低电压保护 - 降落");
     } else {
-        gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
+        // gcs().send_text(MAV_SEVERITY_WARNING, "Battery Failsafe");
+        gcs().send_text(MAV_SEVERITY_WARNING, "低电压保护");
     }
         const AP_AHRS &_ahrs = AP::ahrs();
         float home = 0.0;
@@ -391,7 +394,8 @@ void Copter::do_failsafe_action(Failsafe_Action action, ModeReason reason){
         case Failsafe_Action_Terminate:
 #if ADVANCED_FAILSAFE == ENABLED
             char battery_type_str[17];
-            snprintf(battery_type_str, 17, "%s battery", type_str);
+            // snprintf(battery_type_str, 17, "%s battery", type_str);
+            snprintf(battery_type_str, 17, "%s 电池", type_str);
             g2.afs.gcs_terminate(true, battery_type_str);
 #else
             arming.disarm();

ArduCopter/mode.cpp

@@ -546,6 +546,9 @@ void Mode::land_run_vertical_control(bool pause_descent)
     int16_t  land_speed_now = g.land_speed;
     if(get_alt_above_ground_cm() < g.land_slow_2nd_alt)
         land_speed_now = g.land_speed_2nd;
+    else if(get_alt_above_ground_cm() < 3000)
+        land_speed_now = g.land_speed_3nd;
+
 
     float cmb_rate = 0;
     if (!pause_descent) {
@@ -642,8 +645,18 @@ void Mode::land_run_horizontal_control()
     }
 #endif
     static uint8_t prt_once;
-    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);
-    // bool land_lock_attitude = (get_alt_above_ground_cm() < g.land_lock_alt * 4) && g.land_lock_alt && (copter.control_mode == Mode::Number::LAND);
+    #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);
+        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);
+    #endif
     if(land_lock_attitude)
     {
         if(prt_once == 0)
@@ -652,7 +665,7 @@ void Mode::land_run_horizontal_control()
             copter.Log_Write_Event(DATA_LAND_READY_LOCK);
             prt_once = 1;
         }
-        if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt*2){
+        if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt){
             target_roll = 0;
             target_pitch = 0;
             target_yaw_rate = 0;

ArduCopter/version.h

@@ -6,12 +6,12 @@
 
 #include "ap_version.h"
 
-#define THISFIRMWARE "ZRUAV v4.0.5-rc2"  //"ArduCopter V4.0.0"
+#define THISFIRMWARE "ZRUAV v4.0.5-rc3"  //"ArduCopter V4.0.0"
 
 // the following line is parsed by the autotest scripts
 #define FIRMWARE_VERSION 4,0,5,FIRMWARE_VERSION_TYPE_OFFICIAL
 
 #define FW_MAJOR 4
 #define FW_MINOR 0
-#define FW_PATCH 0
+#define FW_PATCH 5
 #define FW_TYPE FIRMWARE_VERSION_TYPE_OFFICIAL

ArduCopter/zr_flight.cpp

@@ -22,7 +22,7 @@ bool Copter::zr_radio_valid(){
     if(!motors->armed()){
         return true;
     }
-    if (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND){
+    if (!rc().ignore_ctrl_channel() && (control_mode == Mode::Number::AUTO || control_mode == Mode::Number::RTL || control_mode == Mode::Number::LAND)){
         if(abs(channel_roll->get_control_in())>750 || \
             abs(channel_pitch->get_control_in())>750 || \
             abs(channel_yaw->get_control_in())>750 || \

libraries/AP_BattMonitor/AP_BattMonitor_Serial_BattGo.cpp

@@ -264,14 +264,14 @@ void AP_BattMonitor_Serial_BattGo::read()
         }else{            
             if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                       _interim_state.healthy = false;
-                      gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
+                      gcs().send_text(MAV_SEVERITY_WARNING,"%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
                 last_prt = tnow;
             }
         }
     }else{
             if(tnow-last_rev_batt_time>5000000 && tnow - last_prt > 20000000 ){
                       _interim_state.healthy = false;
-                      gcs().send_text(MAV_SEVERITY_WARNING,"BATT_ERR:已经%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
+                      gcs().send_text(MAV_SEVERITY_WARNING,"%d秒没有接收到智能电池的串口数据！",(tnow-last_rev_batt_time)/1000000);
                 last_prt = tnow;
             }
     }