Merge branch 'zr-v4.0-dev' of zrzk.nzeg.top:zrzk/zr-v4 into zr-v4.0-dev

ArduCopter/AP_Arming.cpp

@@ -457,7 +457,7 @@ bool AP_Arming_Copter::mandatory_deadline_checks(bool display_failure)
 {
    const AP_GPS &gps   =  AP::gps();
     if(gps.status()<2)
-        return true;
+        return false;
     if (!copter.deadline_ok())
     {
         int32_t deadline = 0;
@@ -562,6 +562,11 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
 {
     const AP_AHRS_NavEKF &ahrs = AP::ahrs_navekf();
 
+   //arm if  Trial time is not arrived  add@bnsir
+    if( !mandatory_deadline_checks(false)){
+                return false;
+    }
+    
     // always check if inertial nav has started and is ready
     if (!ahrs.healthy()) {
         check_failed(true, "AHRS not healthy");
@@ -580,13 +585,6 @@ bool AP_Arming_Copter::arm_checks(AP_Arming::Method method)
     }
 #endif
 
-   //arm if  Trial time is not arrived  add@bnsir
-
-if( !mandatory_deadline_checks(false)){
-            return false;
-}
-
-
     Mode::Number control_mode = copter.control_mode;
 
     // always check if the current mode allows arming

ArduCopter/Attitude.cpp

@@ -156,9 +156,39 @@ void Copter::rotate_body_frame_to_NE(float &x, float &y)
 // It will return the PILOT_SPEED_DN value if non zero, otherwise if zero it returns the PILOT_SPEED_UP value.
 uint16_t Copter::get_pilot_speed_dn()
 {
-    if (g2.pilot_speed_dn == 0) {
+    int16_t speed_dn_now ;
+
+    if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
+        speed_dn_now = g2.pilot_speed_dn2;
+    else if(get_alt_above_ground_cm() < g2.land_alt_low)
+        speed_dn_now = g.land_speed;
+    else
+    {
+        speed_dn_now = g2.pilot_speed_dn;
+    }
+    
+   if (speed_dn_now == 0) {
         return abs(g.pilot_speed_up);
     } else {
-        return abs(g2.pilot_speed_dn);
+        return abs(speed_dn_now);
+    }
+    // if (g2.pilot_speed_dn == 0) {
+    //     return abs(g.pilot_speed_up);
+    // } else {
+    //     return abs(g2.pilot_speed_dn);
+    // }
+}
+
+int32_t Copter::get_alt_above_ground_cm()
+{
+    int32_t alt_above_ground;
+    if (rangefinder_alt_ok() && current_loc.alt < g.land_slow_2nd_alt) {
+        alt_above_ground = rangefinder_state.alt_cm_filt.get();
+    } else {
+        bool navigating = pos_control->is_active_xy();
+        if (!navigating || !current_loc.get_alt_cm(Location::AltFrame::ABOVE_TERRAIN, alt_above_ground)) {
+            alt_above_ground = current_loc.alt;
+        }
     }
+    return alt_above_ground;
 }

ArduCopter/Copter.h

@@ -685,6 +685,7 @@ private:
     void set_accel_throttle_I_from_pilot_throttle();
     void rotate_body_frame_to_NE(float &x, float &y);
     uint16_t get_pilot_speed_dn();
+    int32_t get_alt_above_ground_cm();
 
 #if ADSB_ENABLED == ENABLED
     // avoidance_adsb.cpp

ArduCopter/Parameters.cpp

@@ -43,24 +43,18 @@ const AP_Param::Info Copter::var_info[] = {
     // @ReadOnly: True
     GSCALAR(format_version, "SYSID_SW_MREV",   0),
 
-    // @Param: BOARD_ID
-    // @DisplayName: Board ID
-    // @Description: For mark board id
-    // @User: @Brown
-    GSCALAR(sysid_board_id,   "SYSID_BOARD_ID",     100),
-
     // @Param: SYSID_BD_NAME1
     // @DisplayName: Board Name 1st
     // @Description: For mark board name first
     // @Range: 1-4294967269
     // @User: @Binsir
-    GSCALAR(sysid_board_name_1st,   "SYSID_BDNAME_P1",   12848 ),
+    GSCALAR(sysid_board_name_1st,   "SYSID_BDNAME_P1",   2004 ),
 
     // @Param: SYSID_BD_NAME2
     // @DisplayName: Board Name 2nd
     // @Description: For mark board name last
   // @User: @Binsir
-    GSCALAR(sysid_board_name_2nd,   "SYSID_BDNAME_P2",     5329970),
+    GSCALAR(sysid_board_name_2nd,   "SYSID_BDNAME_P2",    1001),
 
     // @Param: SYSID_THISMAV
     // @DisplayName: MAVLink system ID of this vehicle
@@ -220,7 +214,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Description: Determines how the autopilot controls the yaw during missions and RTL
     // @Values: 0:Never change yaw, 1:Face next waypoint, 2:Face next waypoint except RTL, 3:Face along GPS course
     // @User: Standard
-    GSCALAR(wp_yaw_behavior,  "WP_YAW_BEHAVIOR",    WP_YAW_BEHAVIOR_DEFAULT),
+    GSCALAR(wp_yaw_behavior,  "WP_YAW_BEHAVIOR",    WP_YAW_BEHAVIOR_LOOK_AT_NEXT_WP),
 
     // @Param: LAND_SPEED
     // @DisplayName: Land speed
@@ -229,7 +223,12 @@ const AP_Param::Info Copter::var_info[] = {
     // @Range: 30 200
     // @Increment: 10
     // @User: Standard
-    GSCALAR(land_speed,             "LAND_SPEED",   LAND_SPEED),
+    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_lock_alt,             "LAND_LOCK_ALT",    100),
+    GSCALAR(land_lock_rpy,             "LAND_LOCK_RPY",    1),
+    
 
     // @Param: LAND_SPEED_HIGH
     // @DisplayName: Land speed high
@@ -263,7 +262,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @Description: The throttle failsafe allows you to configure a software failsafe activated by a setting on the throttle input channel
     // @Values:  0:Disabled,1:Enabled always RTL,2:Enabled Continue with Mission in Auto Mode (Deprecated in 4.0+),3:Enabled always Land,4:Enabled always SmartRTL or RTL,5:Enabled always SmartRTL or Land
     // @User: Standard
-    GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_ENABLED_ALWAYS_RTL),
+    GSCALAR(failsafe_throttle,  "FS_THR_ENABLE",   FS_THR_DISABLED),
 
     // @Param: FS_THR_VALUE
     // @DisplayName: Throttle Failsafe Value
@@ -910,7 +909,7 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
     // @Range: 100 10000
     // @Increment: 10
     // @User: Advanced
-    AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 1000),
+    AP_GROUPINFO("LAND_ALT_LOW", 25, ParametersG2, land_alt_low, 2000),
 
 #if !HAL_MINIMIZE_FEATURES && OPTFLOW == ENABLED
     // @Group: FHLD
@@ -986,6 +985,9 @@ const AP_Param::GroupInfo ParametersG2::var_info[] = {
 #endif
 
 
+    AP_GROUPINFO("PILOT_SPEED_DN2", 38, ParametersG2, pilot_speed_dn2, 100),
+    AP_GROUPINFO("PILOT_DN2_ALT", 39, ParametersG2, pilot_slow_dn2_alt, 100),
+
 
     AP_GROUPEND
 };
@@ -1629,39 +1631,11 @@ const char* Copter::get_sysid_board_id(void)
 
      int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;   
-     char  name[7] ;   
-     memset(name,0,7);
-     //nameValue2 = nameValue2 & 0xffffff;
-     name[5] = '.';
-     name[4] = nameValue2&0xFF;
-     name[3] = nameValue2>>8&0xFF; 
-     name[2] = nameValue2>>16& 0xFF;   
-     name[1] = nameValue1&0xFF; 
-     name[0] = nameValue1>>8 & 0xFF;  
-    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.1 ,Board ID: ZRZK.%s%d",name,(int)g.sysid_board_id);
+
+    // snprintf(buf, sizeof(buf),  "Version: zr-v4.0.1 ,Board ID: ZRZK.20QT2.%d",(int)nameValue2);
+    snprintf(buf, sizeof(buf),  "Version: zr-v4.0.2 ,ID: RS100%04d%05d",(int)nameValue1,(int)nameValue2);
     return buf;
 }
-// // modify by @Brown
-// const char* Copter::get_sysid_board_id(void)
-// {
-
-//     static char  buf[50];
-//     // snprintf(buf, sizeof(buf),  "Version: v3.5.6 ,Board ID: ZRZK.19QT3.%d",(int)g.sysid_board_id);
-
-//      int32_t nameValue1 =(int32_t) g.sysid_board_name_1st;
-//      int32_t nameValue2 = (int32_t)g.sysid_board_name_2nd;   
-//      char  name[7] = {' ',' ',' ',' ',' '};   
-//     //  memset(name,0,6);
-//      nameValue2 = nameValue2 & 0xffffff;
-//     //  name[5] = "a";
-//      name[4] = nameValue2&0xFF;
-//      name[3] = nameValue2>>8&0xFF; 
-//      name[2] = nameValue2>>16& 0xFF;   
-//      name[1] = nameValue1&0xFF; 
-//      name[0] = nameValue1>>8 & 0xFF;  
-//     snprintf(buf, sizeof(buf),  "Version: zr-v4.0.1,Board ID: ZRZK.%s.%d",name,(int)g.sysid_board_id);
-//     return buf;
-// }
 
 void Copter::get_deadline_params(int32_t &deadline)
 {

ArduCopter/Parameters.h

@@ -381,15 +381,20 @@ public:
        k_param_sysid_dl3,
        k_param_sysid_dl4,
 
+        k_param_land_speed_2nd,
+        k_param_land_slow_2nd_alt,
+        k_param_land_lock_alt,
+        k_param_land_lock_rpy,
+
         // the k_param_* space is 9-bits in size
         // 511: reserved
     };
 
     AP_Int16        format_version;
 
-    AP_Int16        sysid_board_id; // modify by @Brown
-    AP_Int32        sysid_board_name_1st;// modify by @Binsir
-    AP_Int32         sysid_board_name_2nd;// modify by @Binsir
+    // AP_Int32        sysid_board_id; // modify by @Brown
+    AP_Int16        sysid_board_name_1st;// modify by @Binsir
+    AP_Int16         sysid_board_name_2nd;// modify by @Binsir
 
     // Telemetry control
     //
@@ -433,6 +438,14 @@ public:
     AP_Int16        pilot_speed_up;    // maximum vertical ascending velocity the pilot may request
     AP_Int16        pilot_accel_z;               // vertical acceleration the pilot may request
 
+
+    AP_Int16        land_speed_2nd;
+    AP_Int16        land_slow_2nd_alt;
+    AP_Int8        land_lock_alt;
+    AP_Int8        land_lock_rpy;
+
+
+    
     // Throttle
     //
     AP_Int8         failsafe_throttle;
@@ -582,6 +595,8 @@ public:
 
     // Additional pilot velocity items
     AP_Int16    pilot_speed_dn;
+    AP_Int16    pilot_speed_dn2;
+    AP_Int16    pilot_slow_dn2_alt;
 
     // Land alt final stage
     AP_Int16 land_alt_low;

ArduCopter/config.h

@@ -410,22 +410,22 @@
 //
 
 #ifndef FLIGHT_MODE_1
- # define FLIGHT_MODE_1                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_1                  Mode::Number::ALT_HOLD
 #endif
 #ifndef FLIGHT_MODE_2
- # define FLIGHT_MODE_2                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_2                  Mode::Number::ALT_HOLD
 #endif
 #ifndef FLIGHT_MODE_3
- # define FLIGHT_MODE_3                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_3                  Mode::Number::LOITER
 #endif
 #ifndef FLIGHT_MODE_4
- # define FLIGHT_MODE_4                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_4                  Mode::Number::LOITER
 #endif
 #ifndef FLIGHT_MODE_5
- # define FLIGHT_MODE_5                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_5                  Mode::Number::AUTO
 #endif
 #ifndef FLIGHT_MODE_6
- # define FLIGHT_MODE_6                  Mode::Number::STABILIZE
+ # define FLIGHT_MODE_6                  Mode::Number::AUTO
 #endif
 
 
@@ -504,7 +504,7 @@
 #endif
 
 #ifndef ACRO_YAW_P
- # define ACRO_YAW_P                4.5f
+ # define ACRO_YAW_P                3.0f
 #endif
 
 #ifndef ACRO_LEVEL_MAX_ANGLE
@@ -641,7 +641,7 @@
 #endif
 
 #ifndef AUTO_DISARMING_DELAY
-# define AUTO_DISARMING_DELAY  10
+# define AUTO_DISARMING_DELAY  5
 #endif
 
 //////////////////////////////////////////////////////////////////////////////

ArduCopter/mode.cpp

@@ -509,7 +509,7 @@ void Mode::make_safe_spool_down()
 int32_t Mode::get_alt_above_ground_cm(void)
 {
     int32_t alt_above_ground;
-    if (copter.rangefinder_alt_ok()) {
+    if (copter.rangefinder_alt_ok() && copter.current_loc.alt < g.land_slow_2nd_alt) {
         alt_above_ground = copter.rangefinder_state.alt_cm_filt.get();
     } else {
         bool navigating = pos_control->is_active_xy();
@@ -533,18 +533,10 @@ void Mode::land_run_vertical_control(bool pause_descent)
     const float precland_acceptable_error = 15.0f;
     const float precland_min_descent_speed = 10.0f;
 
-
 // modify by @Brown , Staging land speed control
-        if(get_alt_above_ground_cm() < 100)
-            g.land_speed = 20;
-        else if(get_alt_above_ground_cm() < 500)
-            g.land_speed = 30;
-        else if(get_alt_above_ground_cm() < 1000)
-            g.land_speed = 50;
-        else if(get_alt_above_ground_cm() < 2000)
-            g.land_speed = 100;
-        else if(get_alt_above_ground_cm() < 3000)
-            g.land_speed = 150;
+    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;
 
     float cmb_rate = 0;
     if (!pause_descent) {
@@ -556,16 +548,19 @@ void Mode::land_run_vertical_control(bool pause_descent)
         }
 
         // Don't speed up for landing.
-        max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
+        // max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(g.land_speed));
+        max_land_descent_velocity = MIN(max_land_descent_velocity, -abs(land_speed_now));
 
         // Compute a vertical velocity demand such that the vehicle approaches g2.land_alt_low. Without the below constraint, this would cause the vehicle to hover at g2.land_alt_low.
         cmb_rate = AC_AttitudeControl::sqrt_controller(MAX(g2.land_alt_low,100)-get_alt_above_ground_cm(), pos_control->get_pos_z_p().kP(), pos_control->get_max_accel_z(), G_Dt);
 
         // Constrain the demanded vertical velocity so that it is between the configured maximum descent speed and the configured minimum descent speed.
-        cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
+        // cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(g.land_speed));
+        cmb_rate = constrain_float(cmb_rate, max_land_descent_velocity, -abs(land_speed_now));
 
         if (doing_precision_landing && copter.rangefinder_alt_ok() && copter.rangefinder_state.alt_cm > 35.0f && copter.rangefinder_state.alt_cm < 200.0f) {
-            float max_descent_speed = abs(g.land_speed)*0.5f;
+            // float max_descent_speed = abs(g.land_speed)*0.5f;
+            float max_descent_speed = abs(land_speed_now)*0.5f;
             float land_slowdown = MAX(0.0f, pos_control->get_horizontal_error()*(max_descent_speed/precland_acceptable_error));
             cmb_rate = MIN(-precland_min_descent_speed, -max_descent_speed+land_slowdown);
         }
@@ -637,6 +632,30 @@ void Mode::land_run_horizontal_control()
         pos_control->override_vehicle_velocity_xy(-target_vel_rel);
     }
 #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(land_lock_attitude)
+    {
+        if(prt_once == 0)
+        {
+            gcs().send_text(MAV_SEVERITY_INFO, "Land ready to enter attitude lock mode");
+            copter.Log_Write_Event(DATA_LAND_READY_LOCK);
+            prt_once = 1;
+        }
+        if(copter.rangefinder_state.alt_cm_filt.get() < g.land_lock_alt*2){
+            target_roll = 0;
+            target_pitch = 0;
+            target_yaw_rate = 0;
+            loiter_nav->soften_for_landing();
+            if(prt_once == 1)
+            {
+                gcs().send_text(MAV_SEVERITY_INFO, "Land Lock attitude!");
+                copter.Log_Write_Event(DATA_LAND_LOCK_ATT);
+                prt_once = 2;
+            }
+        }
+    }
 
     // process roll, pitch inputs
     loiter_nav->set_pilot_desired_acceleration(target_roll, target_pitch, G_Dt);

ArduCopter/mode.h

@@ -245,6 +245,7 @@ public:
     void set_throttle_takeoff(void);
     float get_avoidance_adjusted_climbrate(float target_rate);
     uint16_t get_pilot_speed_dn(void);
+    uint16_t get_pilot_speed_dn2(void);
 
     // end pass-through functions
 };

ArduCopter/mode_loiter.cpp

@@ -79,8 +79,24 @@ void ModeLoiter::run()
     float target_climb_rate = 0.0f;
     float takeoff_climb_rate = 0.0f;
 
+    // @Brown, loiter land auto slow down
+    int16_t  land_speed_now = get_pilot_speed_dn();
+    // static int16_t last_land_speed = land_speed_now;
+    // if(get_alt_above_ground_cm() < g2.pilot_slow_dn2_alt)
+    //     land_speed_now = g2.pilot_speed_dn2;
+    // else if(get_alt_above_ground_cm() < g2.land_alt_low)
+    //     land_speed_now = g.land_speed;
+
+    // if(last_land_speed != land_speed_now)
+    // {
+    //     // gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d,dn2:%4d,dn:%4d",land_speed_now,g2.pilot_speed_dn2,g.land_speed);
+    //     gcs().send_text(MAV_SEVERITY_INFO, "vz:%4d",land_speed_now);
+    //     last_land_speed = land_speed_now;
+    // }
+
     // initialize vertical speed and acceleration
-    pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    // pos_control->set_max_speed_z(-get_pilot_speed_dn(), g.pilot_speed_up);
+    pos_control->set_max_speed_z(-land_speed_now, g.pilot_speed_up);
     pos_control->set_max_accel_z(g.pilot_accel_z);
 
     // process pilot inputs unless we are in radio failsafe
@@ -99,7 +115,8 @@ void ModeLoiter::run()
 
         // 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);
+        // target_climb_rate = constrain_float(target_climb_rate, -get_pilot_speed_dn(), g.pilot_speed_up);
+        target_climb_rate = constrain_float(target_climb_rate, -land_speed_now, g.pilot_speed_up);
     } else {
         // clear out pilot desired acceleration in case radio failsafe event occurs and we do not switch to RTL for some reason
         loiter_nav->clear_pilot_desired_acceleration();

ArduCopter/system.cpp

@@ -339,9 +339,9 @@ bool Copter::deadline_ok()
     int32_t deadline =0;
     copter.get_deadline_params(deadline);
     int32_t gps_date =0;
-if((AP::gps().time_week() ==0)||(AP::gps().time_week_ms() ==0)){
+    if((AP::gps().time_week() ==0)||(AP::gps().time_week_ms() ==0)){
         return  false;
-}
+    }
     uint64_t timestamp  = AP::gps().time_epoch_convert(AP::gps().time_week(),AP::gps().time_week_ms())/1000;
       //tick  -timestamp - uint:s
     time_t tick = (time_t)timestamp;

libraries/AC_Avoidance/AC_Avoid.h

@@ -15,7 +15,7 @@
 #define AC_AVOID_DEFAULT                (AC_AVOID_STOP_AT_FENCE | AC_AVOID_USE_PROXIMITY_SENSOR)
 
 // definitions for non-GPS avoidance
-#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT    5.0f    // objects over 5m away are ignored (default value for DIST_MAX parameter)
+#define AC_AVOID_NONGPS_DIST_MAX_DEFAULT    7.0f    // objects over 5m away are ignored (default value for DIST_MAX parameter)
 #define AC_AVOID_ANGLE_MAX_PERCENT          0.75f   // object avoidance max lean angle as a percentage (expressed in 0 ~ 1 range) of total vehicle max lean angle
 
 /*

libraries/AC_WPNav/AC_Loiter.cpp

@@ -3,7 +3,7 @@
 
 extern const AP_HAL::HAL& hal;
 
-#define LOITER_SPEED_DEFAULT                1250.0f // default loiter speed in cm/s
+#define LOITER_SPEED_DEFAULT                1000.0f // default loiter speed in cm/s
 #define LOITER_SPEED_MIN                    20.0f   // minimum loiter speed in cm/s
 #define LOITER_ACCEL_MAX_DEFAULT            500.0f  // default acceleration in loiter mode
 #define LOITER_BRAKE_ACCEL_DEFAULT          250.0f  // minimum acceleration in loiter mode

libraries/AC_WPNav/AC_WPNav.h

@@ -14,13 +14,13 @@
 #define WPNAV_ACCELERATION              100.0f      // defines the default velocity vs distant curve.  maximum acceleration in cm/s/s that position controller asks for from acceleration controller
 #define WPNAV_ACCELERATION_MIN           50.0f      // minimum acceleration in cm/s/s - used for sanity checking _wp_accel parameter
 
-#define WPNAV_WP_SPEED                  500.0f      // default horizontal speed between waypoints in cm/s
+#define WPNAV_WP_SPEED                  800.0f      // default horizontal speed between waypoints in cm/s
 #define WPNAV_WP_SPEED_MIN               20.0f      // minimum horizontal speed between waypoints in cm/s
 #define WPNAV_WP_TRACK_SPEED_MIN         50.0f      // minimum speed along track of the target point the vehicle is chasing in cm/s (used as target slows down before reaching destination)
 #define WPNAV_WP_RADIUS                 200.0f      // default waypoint radius in cm
 #define WPNAV_WP_RADIUS_MIN               5.0f      // minimum waypoint radius in cm
 
-#define WPNAV_WP_SPEED_UP               250.0f      // default maximum climb velocity
+#define WPNAV_WP_SPEED_UP               350.0f      // default maximum climb velocity
 #define WPNAV_WP_SPEED_DOWN             150.0f      // default maximum descent velocity
 
 #define WPNAV_WP_ACCEL_Z_DEFAULT        100.0f      // default vertical acceleration between waypoints in cm/s/s

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -38,7 +38,7 @@
 #endif
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
-# define BOARD_SAFETY_ENABLE_DEFAULT 1
+# define BOARD_SAFETY_ENABLE_DEFAULT 0
 #ifndef BOARD_PWM_COUNT_DEFAULT
 # define BOARD_PWM_COUNT_DEFAULT 6
 #endif
@@ -62,7 +62,7 @@
 #endif
 
 #ifndef BOARD_PWM_COUNT_DEFAULT
-#define BOARD_PWM_COUNT_DEFAULT 8
+#define BOARD_PWM_COUNT_DEFAULT 4
 #endif
 
 #ifndef BOARD_CONFIG_BOARD_VOLTAGE_MIN

libraries/AP_Camera/AP_Camera.cpp

@@ -97,7 +97,7 @@ const AP_Param::GroupInfo AP_Camera::var_info[] = {
     // @Description: When enabled, trigging by distance is done in AUTO mode only.
     // @Values: 0:Always,1:Only when in AUTO
     // @User: Standard
-    AP_GROUPINFO("AUTO_ONLY",  10, AP_Camera, _auto_mode_only, 0),
+    AP_GROUPINFO("AUTO_ONLY",  10, AP_Camera, _auto_mode_only, 1),
 
     // @Param: TYPE
     // @DisplayName: Type of camera (0: None, 1: BMMCC)

libraries/AP_Camera/AP_Camera.h

@@ -8,9 +8,9 @@
 #define AP_CAMERA_TRIGGER_TYPE_SERVO                0
 #define AP_CAMERA_TRIGGER_TYPE_RELAY                1
 
-#define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE  AP_CAMERA_TRIGGER_TYPE_SERVO    // default is to use servo to trigger camera
+#define AP_CAMERA_TRIGGER_DEFAULT_TRIGGER_TYPE  AP_CAMERA_TRIGGER_TYPE_RELAY    // default is to use servo to trigger camera
 
-#define AP_CAMERA_TRIGGER_DEFAULT_DURATION  10      // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second)
+#define AP_CAMERA_TRIGGER_DEFAULT_DURATION  3      // default duration servo or relay is held open in 10ths of a second (i.e. 10 = 1 second)
 
 #define AP_CAMERA_SERVO_ON_PWM              1300    // default PWM value to move servo to when shutter is activated
 #define AP_CAMERA_SERVO_OFF_PWM             1100    // default PWM value to move servo to when shutter is deactivated

libraries/AP_Compass/AP_Compass.cpp

@@ -296,7 +296,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Description: Enable or disable the second compass for determining heading.
     // @Values: 0:Disabled,1:Enabled
     // @User: Advanced
-    AP_GROUPINFO("USE2",    18, Compass, _state[1].use_for_yaw, 1),
+    AP_GROUPINFO("USE2",    18, Compass, _state[1].use_for_yaw, 0),
 
     // @Param: ORIENT2
     // @DisplayName: Compass2 orientation
@@ -319,7 +319,7 @@ const AP_Param::GroupInfo Compass::var_info[] = {
     // @Description: Enable or disable the third compass for determining heading.
     // @Values: 0:Disabled,1:Enabled
     // @User: Advanced
-    AP_GROUPINFO("USE3",    21, Compass, _state[2].use_for_yaw, 1),
+    AP_GROUPINFO("USE3",    21, Compass, _state[2].use_for_yaw, 0),
 
     // @Param: ORIENT3
     // @DisplayName: Compass3 orientation

libraries/AP_Logger/AP_Logger.h

@@ -92,6 +92,9 @@ enum Log_Event : uint8_t {
     DATA_STANDBY_ENABLE = 74,
     DATA_STANDBY_DISABLE = 75,
 
+    DATA_LAND_READY_LOCK = 76,
+    DATA_LAND_LOCK_ATT = 77,
+
     DATA_SURFACED = 163,
     DATA_NOT_SURFACED = 164,
     DATA_BOTTOMED = 165,

libraries/AP_NavEKF2/AP_NavEKF2.cpp

@@ -107,7 +107,7 @@
 #define VEL_I_GATE_DEFAULT      500
 #define POS_I_GATE_DEFAULT      500
 #define HGT_I_GATE_DEFAULT      500
-#define MAG_I_GATE_DEFAULT      300
+#define MAG_I_GATE_DEFAULT      800
 #define MAG_CAL_DEFAULT         3
 #define GLITCH_RADIUS_DEFAULT   25
 #define FLOW_MEAS_DELAY         10
@@ -452,7 +452,7 @@ const AP_Param::GroupInfo NavEKF2::var_info[] = {
     // @Range: 100 1000
     // @Increment: 25
     // @User: Advanced
-    AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 300),
+    AP_GROUPINFO("YAW_I_GATE", 38, NavEKF2, _yawInnovGate, 800),
 
     // @Param: TAU_OUTPUT
     // @DisplayName: Output complementary filter time constant (centi-sec)

libraries/AP_RangeFinder/AP_RangeFinder_Benewake.cpp

@@ -120,6 +120,11 @@ bool AP_RangeFinder_Benewake::get_reading(uint16_t &reading_cm)
                 }
                 // if checksum matches extract contents
                 if (checksum == linebuf[BENEWAKE_FRAME_LENGTH-1]) {
+
+                    uint16_t strength = ((uint16_t)linebuf[5] << 8) | linebuf[4];
+                    if(strength < 100 || strength == 0xFFFF){
+                        return false;
+                    }
                     // calculate distance
                     uint16_t dist = ((uint16_t)linebuf[3] << 8) | linebuf[2];
                     if (dist >= BENEWAKE_DIST_MAX_CM) {

libraries/AP_Relay/AP_Relay.cpp

@@ -21,7 +21,7 @@
 #endif
 
 #ifndef RELAY1_PIN_DEFAULT
-  #define RELAY1_PIN_DEFAULT -1
+  #define RELAY1_PIN_DEFAULT 56
 #endif
 
 #ifndef RELAY2_PIN_DEFAULT