Merge branch 'ArduPilot:master' into master

AntennaTracker/Parameters.cpp

@@ -23,6 +23,7 @@ const AP_Param::Info Tracker::var_info[] = {
     // @DisplayName: Ground station MAVLink system ID
     // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
     // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",    255),
 

ArduCopter/Parameters.cpp

@@ -46,6 +46,7 @@ const AP_Param::Info Copter::var_info[] = {
     // @DisplayName: My ground station number
     // @Description: Allows restricting radio overrides to only come from my ground station
     // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
 

ArduCopter/autoyaw.cpp

@@ -114,9 +114,9 @@ void Mode::AutoYaw::set_fixed_yaw(float angle_deg, float turn_rate_ds, int8_t di
     } else {
         // absolute angle
         _fixed_yaw_offset_cd = wrap_180_cd(angle_deg * 100.0 - _yaw_angle_cd);
-        if ( direction < 0 && is_positive(_fixed_yaw_offset_cd) ) {
+        if (direction < 0 && is_positive(_fixed_yaw_offset_cd)) {
             _fixed_yaw_offset_cd -= 36000.0;
-        } else if ( direction > 0 && is_negative(_fixed_yaw_offset_cd) ) {
+        } else if (direction > 0 && is_negative(_fixed_yaw_offset_cd)) {
             _fixed_yaw_offset_cd += 36000.0;
         }
     }

ArduCopter/mode_auto.cpp

@@ -1850,8 +1850,6 @@ void ModeAuto::do_yaw(const AP_Mission::Mission_Command& cmd)
 // Do (Now) commands
 /********************************************************************************/
 
-
-
 void ModeAuto::do_change_speed(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.content.speed.target_ms > 0) {

ArduPlane/Parameters.cpp

@@ -23,6 +23,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @DisplayName: Ground station MAVLink system ID
     // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
     // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",    255),
 

ArduPlane/mode.cpp

@@ -113,6 +113,9 @@ bool Mode::enter()
         plane.adsb.set_is_auto_mode(does_auto_navigation());
 #endif
 
+        // set the nav controller stale AFTER _enter() so that we can check if we're currently in a loiter during the mode change
+        plane.nav_controller->set_data_is_stale();
+
         // reset steering integrator on mode change
         plane.steerController.reset_I();
 

ArduPlane/mode_LoiterAltQLand.cpp

@@ -10,17 +10,13 @@ bool ModeLoiterAltQLand::_enter()
         return true;
     }
 
+    // If we were already in a loiter then use that waypoint. Else, use the current point
+    const bool already_in_a_loiter = plane.nav_controller->reached_loiter_target() && !plane.nav_controller->data_is_stale();
+    const Location loiter_wp = already_in_a_loiter ? plane.next_WP_loc : plane.current_loc;
+
     ModeLoiter::_enter();
 
-#if AP_TERRAIN_AVAILABLE
-    if (plane.terrain_enabled_in_mode(Mode::Number::QLAND)) {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_TERRAIN);
-    } else {
-        plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-    }
-#else
-    plane.next_WP_loc.set_alt_cm(quadplane.qrtl_alt*100UL, Location::AltFrame::ABOVE_HOME);
-#endif
+    handle_guided_request(loiter_wp);
 
     switch_qland();
 

ArduPlane/quadplane.cpp

@@ -1219,7 +1219,7 @@ bool QuadPlane::is_flying_vtol(void) const
     }
     if (plane.control_mode->is_vtol_man_mode()) {
         // in manual flight modes only consider aircraft landed when pilot demanded throttle is zero
-        return is_positive(plane.get_throttle_input());
+        return is_positive(get_throttle_input());
     }
     if (in_vtol_mode() && millis() - landing_detect.lower_limit_start_ms > 5000) {
         // use landing detector
@@ -1289,7 +1289,7 @@ float QuadPlane::get_pilot_input_yaw_rate_cds(void) const
     const auto rudder_in = plane.channel_rudder->get_control_in();
     bool manual_air_mode = plane.control_mode->is_vtol_man_throttle() && air_mode_active();
     if (!manual_air_mode &&
-        !is_positive(plane.get_throttle_input()) &&
+        !is_positive(get_throttle_input()) &&
         (!plane.control_mode->does_auto_throttle() || motors->limit.throttle_lower) &&
         plane.arming.get_rudder_arming_type() == AP_Arming::RudderArming::ARMDISARM &&
         rudder_in < 0 &&
@@ -1366,7 +1366,7 @@ float QuadPlane::get_pilot_desired_climb_rate_cms(void) const
  */
 void QuadPlane::init_throttle_wait(void)
 {
-    if (plane.get_throttle_input() >= 10 ||
+    if (get_throttle_input() >= 10 ||
         plane.is_flying()) {
         throttle_wait = false;
     } else {
@@ -1920,7 +1920,7 @@ void QuadPlane::update_throttle_suppression(void)
        consider non-zero throttle to mean that pilot is commanding
        takeoff unless in a manual thottle mode
     */
-    if (!is_zero(plane.get_throttle_input()) &&
+    if (!is_zero(get_throttle_input()) &&
         (rc().arming_check_throttle() ||
          plane.control_mode->is_vtol_man_throttle() ||
          plane.channel_throttle->norm_input_dz() > 0)) {
@@ -4061,7 +4061,7 @@ void QuadPlane::update_throttle_mix(void)
 
     if (plane.control_mode->is_vtol_man_throttle()) {
         // manual throttle
-        if (!is_positive(plane.get_throttle_input()) && !air_mode_active()) {
+        if (!is_positive(get_throttle_input()) && !air_mode_active()) {
             attitude_control->set_throttle_mix_min();
         } else {
             attitude_control->set_throttle_mix_man();
@@ -4729,4 +4729,18 @@ bool QuadPlane::should_disable_TECS() const
     return false;
 }
 
+// Get pilot throttle input with deadzone, this will return 50% throttle in failsafe!
+// This is a re-implmentation of Plane::get_throttle_input
+// Ignoring the no_deadzone case means we don't need to check for valid RC
+// This is handled by Plane::control_failsafe setting of control in
+float QuadPlane::get_throttle_input() const
+{
+    float ret = plane.channel_throttle->get_control_in();
+    if (plane.reversed_throttle) {
+        // RC option for reverse throttle has been set
+        ret = -ret;
+    }
+    return ret;
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -190,6 +190,9 @@ class QuadPlane
      */
     bool should_disable_TECS() const;
 
+    // Get pilot throttle input with deadzone, this will return 50% throttle in failsafe!
+    float get_throttle_input() const;
+
 private:
     AP_AHRS &ahrs;
 

ArduPlane/reverse_thrust.cpp

@@ -125,6 +125,10 @@ bool Plane::have_reverse_thrust(void) const
  */
 float Plane::get_throttle_input(bool no_deadzone) const
 {
+    if (!rc().has_valid_input()) {
+        // Return 0 if there is no valid input
+        return 0.0;
+    }
     float ret;
     if (no_deadzone) {
         ret = channel_throttle->get_control_in_zero_dz();
@@ -143,6 +147,10 @@ float Plane::get_throttle_input(bool no_deadzone) const
  */
 float Plane::get_adjusted_throttle_input(bool no_deadzone) const
 {
+    if (!rc().has_valid_input()) {
+        // Return 0 if there is no valid input
+        return 0.0;
+    }
     if ((plane.channel_throttle->get_type() != RC_Channel::ControlType::RANGE) ||
         (flight_option_enabled(FlightOptions::CENTER_THROTTLE_TRIM)) == 0) {
        return  get_throttle_input(no_deadzone);

ArduPlane/servos.cpp

@@ -551,9 +551,7 @@ void Plane::set_servos_controlled(void)
                control_mode == &mode_fbwa ||
                control_mode == &mode_autotune) {
         // a manual throttle mode
-        if (!rc().has_valid_input()) {
-            SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, g.throttle_passthru_stabilize ? 0.0 : MAX(min_throttle,0));
-        } else if (g.throttle_passthru_stabilize) {
+        if (g.throttle_passthru_stabilize) {
             // manual pass through of throttle while in FBWA or
             // STABILIZE mode with THR_PASS_STAB set
             SRV_Channels::set_output_scaled(SRV_Channel::k_throttle, get_throttle_input(true));

ArduSub/Parameters.cpp

@@ -47,6 +47,8 @@ const AP_Param::Info Sub::var_info[] = {
     // @Param: SYSID_MYGCS
     // @DisplayName: My ground station number
     // @Description: Allows restricting radio overrides to only come from my ground station
+    // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
 

ArduSub/Parameters.h

@@ -358,6 +358,7 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
                              AP_Arming::ARMING_CHECK_BATTERY},
     { "CIRCLE_RATE",         2.0f},
     { "ATC_ACCEL_Y_MAX",     110000.0f},
+    { "ATC_RATE_Y_MAX",      180.0f},
     { "RC3_TRIM",            1100},
     { "COMPASS_OFFS_MAX",    1000},
     { "INS_GYR_CAL",         0},
@@ -368,4 +369,24 @@ static const struct AP_Param::defaults_table_struct defaults_table[] = {
     { "RC8_OPTION",          213},   // MOUNT1_PITCH
     { "MOT_PWM_MIN",         1100},
     { "MOT_PWM_MAX",         1900},
+    { "PSC_JERK_Z",          50.0f},
+    { "WPNAV_SPEED",         100.0f},
+    { "PILOT_SPEED_UP",      100.0f},
+    { "PSC_VELXY_P",         6.0f},
+    { "EK3_SRC1_VELZ",       0},
+#if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_LINUX_NAVIGATOR
+    { "BARO_PROBE_EXT",      0},
+    { "BATT_MONITOR",        4},
+    { "BATT_CAPACITY",       0},
+    { "LEAK1_PIN",           27},
+    { "SCHED_LOOP_RATE",     200},
+    { "SERVO13_FUNCTION",    59},    // k_rcin9, lights 1
+    { "SERVO14_FUNCTION",    60},    // k_rcin10, lights 2
+    { "SERVO16_FUNCTION",    7},     // k_mount_tilt
+    { "SERVO16_REVERSED",    1},
+#else
+    { "BARO_PROBE_EXT",      768},
+    { "SERVO9_FUNCTION",     59},    // k_rcin9, lights 1
+    { "SERVO10_FUNCTION",    7},     // k_mount_tilt
+#endif
 };

Blimp/Parameters.cpp

@@ -41,6 +41,7 @@ const AP_Param::Info Blimp::var_info[] = {
     // @DisplayName: My ground station number
     // @Description: Allows restricting radio overrides to only come from my ground station
     // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,   "SYSID_MYGCS",     255),
 

Rover/Parameters.cpp

@@ -42,6 +42,7 @@ const AP_Param::Info Rover::var_info[] = {
     // @DisplayName: MAVLink ground station ID
     // @Description: The identifier of the ground station in the MAVLink protocol. Don't change this unless you also modify the ground station to match.
     // @Range: 1 255
+    // @Increment: 1
     // @User: Advanced
     GSCALAR(sysid_my_gcs,           "SYSID_MYGCS",      255),
 

Rover/RC_Channel.cpp

@@ -33,6 +33,7 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw
     // the following functions do not need initialising:
     case AUX_FUNC::ACRO:
     case AUX_FUNC::AUTO:
+    case AUX_FUNC::CIRCLE:
     case AUX_FUNC::FOLLOW:
     case AUX_FUNC::GUIDED:
     case AUX_FUNC::HOLD:
@@ -226,6 +227,10 @@ bool RC_Channel_Rover::do_aux_function(const aux_func_t ch_option, const AuxSwit
         do_aux_function_change_mode(rover.mode_simple, ch_flag);
         break;
 
+    case AUX_FUNC::CIRCLE:
+        do_aux_function_change_mode(rover.g2.mode_circle, ch_flag);
+        break;
+
     // trigger sailboat tack
     case AUX_FUNC::SAILBOAT_TACK:
         // any switch movement interpreted as request to tack

Rover/mode_auto.cpp

@@ -82,21 +82,15 @@ void ModeAuto::update()
     switch (_submode) {
         case SubMode::WP:
         {
-            // check if we've reached the destination
-            if (!g2.wp_nav.reached_destination() || g2.wp_nav.is_fast_waypoint()) {
-                // update navigation controller
+            // boats loiter once the waypoint is reached
+            bool keep_navigating = true;
+            if (rover.is_boat() && g2.wp_nav.reached_destination() && !g2.wp_nav.is_fast_waypoint()) {
+                keep_navigating = !start_loiter();
+            }
+
+            // update navigation controller
+            if (keep_navigating) {
                 navigate_to_waypoint();
-            } else {
-                // we have reached the destination so stay here
-                if (rover.is_boat()) {
-                    if (!start_loiter()) {
-                        start_stop();
-                    }
-                } else {
-                    start_stop();
-                }
-                // update distance to destination
-                _distance_to_destination = rover.current_loc.get_distance(g2.wp_nav.get_destination());
             }
             break;
         }

Tools/AP_Periph/AP_Periph.h

@@ -26,7 +26,6 @@
 #include <AP_Scripting/AP_Scripting.h>
 #include <AP_HAL/CANIface.h>
 #include <AP_Stats/AP_Stats.h>
-#include <AP_Networking/AP_Networking.h>
 #include <AP_RPM/AP_RPM.h>
 #include <AP_SerialManager/AP_SerialManager.h>
 #include <AP_ESC_Telem/AP_ESC_Telem_config.h>
@@ -77,6 +76,15 @@
 #define HAL_PERIPH_CAN_MIRROR 0
 #endif
 
+#if defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT) && !defined(HAL_DEBUG_BUILD) && !defined(HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG)
+/* this checking for reboot can lose bytes on GPS modules and other
+ * serial devices. It is really only relevent on a debug build if you
+ * really want it for non-debug build then define
+ * HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_NON_DEBUG in hwdef.dat
+ */
+#undef HAL_PERIPH_LISTEN_FOR_SERIAL_UART_REBOOT_CMD_PORT
+#endif
+
 #include "Parameters.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL

Tools/AP_Periph/networking.h

@@ -1,9 +1,11 @@
 #pragma once
 
-#include "AP_Periph.h"
+#include <AP_HAL/AP_HAL_Boards.h>
 
 #ifdef HAL_PERIPH_ENABLE_NETWORKING
 
+#include <AP_Networking/AP_Networking.h>
+
 #ifndef HAL_PERIPH_NETWORK_NUM_PASSTHRU
 #define HAL_PERIPH_NETWORK_NUM_PASSTHRU 2
 #endif

Tools/AP_Periph/networking_passthru.cpp

@@ -13,7 +13,7 @@
    along with this program.  If not, see <http://www.gnu.org/licenses/>.
  */
 
-#include "networking.h"
+#include "AP_Periph.h"
 
 #if defined(HAL_PERIPH_ENABLE_NETWORKING) && HAL_PERIPH_NETWORK_NUM_PASSTHRU > 0