diff --git a/ArduCopter/mode_auto.cpp b/ArduCopter/mode_auto.cpp
index 08f41f06c7397b..8a009901a63cf9 100644
--- a/ArduCopter/mode_auto.cpp
+++ b/ArduCopter/mode_auto.cpp
@@ -2330,7 +2330,7 @@ bool ModeAuto::resume()
 
 bool ModeAuto::paused() const
 {
-    return wp_nav->paused();
+    return (wp_nav != nullptr) && wp_nav->paused();
 }
 
 #endif
diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp
index 788fe97dbada6d..17cc78bc173975 100644
--- a/ArduPlane/Attitude.cpp
+++ b/ArduPlane/Attitude.cpp
@@ -204,9 +204,9 @@ float Plane::stabilize_pitch_get_pitch_out()
 #endif
     // if LANDING_FLARE RCx_OPTION switch is set and in FW mode, manual throttle,throttle idle then set pitch to LAND_PITCH_DEG if flight option FORCE_FLARE_ATTITUDE is set
 #if HAL_QUADPLANE_ENABLED
-    const bool quadplane_in_transition = quadplane.in_transition();
+    const bool quadplane_in_frwd_transition = quadplane.in_frwd_transition();
 #else
-    const bool quadplane_in_transition = false;
+    const bool quadplane_in_frwd_transition = false;
 #endif
 
     int32_t demanded_pitch = nav_pitch_cd + int32_t(g.pitch_trim * 100.0) + SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) * g.kff_throttle_to_pitch;
@@ -219,7 +219,7 @@ float Plane::stabilize_pitch_get_pitch_out()
        - throttle stick at zero thrust
        - in fixed wing non auto-throttle mode
     */
-    if (!quadplane_in_transition &&
+    if (!quadplane_in_frwd_transition &&
         !control_mode->is_vtol_mode() &&
         !control_mode->does_auto_throttle() &&
         flare_mode == FlareMode::ENABLED_PITCH_TARGET &&
diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp
index 2c4d9cfed6c561..bbb8d234486eaf 100644
--- a/ArduPlane/Parameters.cpp
+++ b/ArduPlane/Parameters.cpp
@@ -156,7 +156,7 @@ const AP_Param::Info Plane::var_info[] = {
     // @Range: 0 100
     // @Increment: 1
     // @User: Standard
-    ASCALAR(takeoff_throttle_min,       "TKOFF_THR_MIN",    60),
+    ASCALAR(takeoff_throttle_min,       "TKOFF_THR_MIN",    0),
 
     // @Param: TKOFF_OPTIONS
     // @DisplayName: Takeoff options
diff --git a/ArduPlane/RC_Channel.cpp b/ArduPlane/RC_Channel.cpp
index 424c861c7ffc99..d674c55806f07a 100644
--- a/ArduPlane/RC_Channel.cpp
+++ b/ArduPlane/RC_Channel.cpp
@@ -169,6 +169,9 @@ void RC_Channel_Plane::init_aux_function(const RC_Channel::AUX_FUNC ch_option,
     case AUX_FUNC::FW_AUTOTUNE:
     case AUX_FUNC::VFWD_THR_OVERRIDE:
     case AUX_FUNC::PRECISION_LOITER:
+#if AP_ICENGINE_ENABLED
+    case AUX_FUNC::ICE_START_STOP:
+#endif
 #if QAUTOTUNE_ENABLED
     case AUX_FUNC::AUTOTUNE_TEST_GAINS:
 #endif
@@ -287,6 +290,9 @@ bool RC_Channel_Plane::do_aux_function(const AUX_FUNC ch_option, const AuxSwitch
     case AUX_FUNC::FLAP:
     case AUX_FUNC::FBWA_TAILDRAGGER:
     case AUX_FUNC::AIRBRAKE:
+#if AP_ICENGINE_ENABLED
+    case AUX_FUNC::ICE_START_STOP:
+#endif
         break; // input labels, nothing to do
 
 #if HAL_QUADPLANE_ENABLED
diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index fb0755492684c3..0a41b5551b6e6e 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -813,6 +813,10 @@ bool QuadPlane::setup(void)
     pilot_speed_z_max_dn.convert_centi_parameter(AP_PARAM_INT16);
     pilot_accel_z.convert_centi_parameter(AP_PARAM_INT16);
 
+    // Provisionally assign the SLT thrust type.
+    // It will be overwritten by tailsitter or tiltorotor setups.
+    thrust_type = ThrustType::SLT;
+
     tailsitter.setup();
 
     tiltrotor.setup();
@@ -3970,9 +3974,9 @@ bool QuadPlane::is_vtol_land(uint16_t id) const
 /*
   return true if we are in a transition to fwd flight from hover
  */
-bool QuadPlane::in_transition(void) const
+bool QuadPlane::in_frwd_transition(void) const
 {
-    return available() && transition->active();
+    return available() && transition->active_frwd();
 }
 
 /*
@@ -4351,7 +4355,7 @@ bool SLT_Transition::allow_update_throttle_mix() const
     return !(quadplane.assisted_flight && (transition_state == TRANSITION_AIRSPEED_WAIT || transition_state == TRANSITION_TIMER));
 }
 
-bool SLT_Transition::active() const
+bool SLT_Transition::active_frwd() const
 {
     return quadplane.assisted_flight && ((transition_state == TRANSITION_AIRSPEED_WAIT) || (transition_state == TRANSITION_TIMER));
 }
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 2ced92498b423f..ea3bba87791321 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -76,6 +76,13 @@ class QuadPlane
     void control_auto(void);
     bool setup(void);
 
+    enum class ThrustType : uint8_t {
+        SLT=0, // Traditional quadplane, with a pusher motor and independent multicopter lift.
+        TAILSITTER,
+        TILTROTOR,
+    };
+    ThrustType get_thrust_type(void) {return thrust_type;}
+
     void vtol_position_controller(void);
     void setup_target_position(void);
     void takeoff_controller(void);
@@ -103,10 +110,7 @@ class QuadPlane
     // abort landing, only valid when in a VTOL landing descent
     bool abort_landing(void);
 
-    /*
-      return true if we are in a transition to fwd flight from hover
-    */
-    bool in_transition(void) const;
+    bool in_frwd_transition(void) const;
 
     bool handle_do_vtol_transition(enum MAV_VTOL_STATE state) const;
 
@@ -198,6 +202,9 @@ class QuadPlane
     AP_Enum<AP_Motors::motor_frame_class> frame_class;
     AP_Enum<AP_Motors::motor_frame_type> frame_type;
 
+    // Types of different "quadplane" configurations.
+    ThrustType thrust_type;
+
     // Initialise motors to allow passing it to tailsitter in its constructor
     AP_MotorsMulticopter *motors = nullptr;
     const struct AP_Param::GroupInfo *motors_var_info;
diff --git a/ArduPlane/servos.cpp b/ArduPlane/servos.cpp
index aa2c7ce3737b7d..496255a75a54c1 100644
--- a/ArduPlane/servos.cpp
+++ b/ArduPlane/servos.cpp
@@ -51,7 +51,7 @@ void Plane::throttle_slew_limit(SRV_Channel::Aux_servo_function_t func)
         slewrate = g.takeoff_throttle_slewrate;
     }
 #if HAL_QUADPLANE_ENABLED
-    if (g.takeoff_throttle_slewrate != 0 && quadplane.in_transition()) {
+    if (g.takeoff_throttle_slewrate != 0 && quadplane.in_frwd_transition()) {
         slewrate = g.takeoff_throttle_slewrate;
     }
 #endif
@@ -555,10 +555,22 @@ float Plane::apply_throttle_limits(float throttle_in)
 
     // Handle throttle limits for transition conditions.
 #if HAL_QUADPLANE_ENABLED
-    if (quadplane.in_transition()) {
+    if (quadplane.in_frwd_transition()) {
         if (aparm.takeoff_throttle_max != 0) {
             max_throttle = aparm.takeoff_throttle_max.get();
         }
+
+        // Apply minimum throttle limits only for SLT thrust types.
+        // The other types don't support it well.
+        if (quadplane.get_thrust_type() == QuadPlane::ThrustType::SLT
+            && control_mode->does_auto_throttle()
+        ) {
+            if (aparm.takeoff_throttle_min.get() != 0) {
+                min_throttle = MAX(min_throttle, aparm.takeoff_throttle_min.get());
+            } else {
+                min_throttle = MAX(min_throttle, aparm.throttle_cruise.get());
+            }
+        }
     }
 #endif
 
@@ -793,7 +805,7 @@ void Plane::servos_twin_engine_mix(void)
 void Plane::force_flare(void)
 {
 #if HAL_QUADPLANE_ENABLED
-    if (quadplane.in_transition() && plane.arming.is_armed()) { //allows for ground checking of flare tilts
+    if (quadplane.in_frwd_transition() && plane.arming.is_armed()) { //allows for ground checking of flare tilts
         return;
     }
     if (control_mode->is_vtol_mode()) {
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index d27491ff21aae2..a38e00b9a222a6 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -164,6 +164,11 @@ void Plane::init_ardupilot()
 #if AC_PRECLAND_ENABLED
     g2.precland.init(scheduler.get_loop_rate_hz());
 #endif
+
+#if AP_ICENGINE_ENABLED
+    g2.ice_control.init();
+#endif
+
 }
 
 #if AP_FENCE_ENABLED
diff --git a/ArduPlane/tailsitter.cpp b/ArduPlane/tailsitter.cpp
index b530dacce55947..33d111db3d0667 100644
--- a/ArduPlane/tailsitter.cpp
+++ b/ArduPlane/tailsitter.cpp
@@ -208,6 +208,8 @@ void Tailsitter::setup()
         return;
     }
 
+    quadplane.thrust_type = QuadPlane::ThrustType::TAILSITTER;
+
     // Set tailsitter transition rate to match old calculation
     if (!transition_rate_fw.configured()) {
         transition_rate_fw.set_and_save(transition_angle_fw / (quadplane.transition_time_ms/2000.0f));
diff --git a/ArduPlane/tailsitter.h b/ArduPlane/tailsitter.h
index fbf26702f8ca9f..143c26cb9beaaf 100644
--- a/ArduPlane/tailsitter.h
+++ b/ArduPlane/tailsitter.h
@@ -175,7 +175,7 @@ friend class Tailsitter;
 
     uint8_t get_log_transition_state() const override { return static_cast<uint8_t>(transition_state); }
 
-    bool active() const override { return transition_state != TRANSITION_DONE; }
+    bool active_frwd() const override { return transition_state == TRANSITION_ANGLE_WAIT_FW; }
 
     bool show_vtol_view() const override;
 
diff --git a/ArduPlane/tiltrotor.cpp b/ArduPlane/tiltrotor.cpp
index 9fd1f8a2cbd1ab..fe893b01cd730d 100644
--- a/ArduPlane/tiltrotor.cpp
+++ b/ArduPlane/tiltrotor.cpp
@@ -106,6 +106,8 @@ void Tiltrotor::setup()
         return;
     }
 
+    quadplane.thrust_type = QuadPlane::ThrustType::TILTROTOR;
+
     _is_vectored = tilt_mask != 0 && type == TILT_TYPE_VECTORED_YAW;
 
     // true if a fixed forward motor is configured, either throttle, throttle left  or throttle right.
diff --git a/ArduPlane/transition.h b/ArduPlane/transition.h
index b31e45ff040ece..bf0e0dc5029d8b 100644
--- a/ArduPlane/transition.h
+++ b/ArduPlane/transition.h
@@ -36,7 +36,7 @@ class Transition
 
     virtual uint8_t get_log_transition_state() const = 0;
 
-    virtual bool active() const = 0;
+    virtual bool active_frwd() const = 0;
 
     virtual bool show_vtol_view() const = 0;
 
@@ -85,7 +85,7 @@ class SLT_Transition : public Transition
 
     uint8_t get_log_transition_state() const override { return static_cast<uint8_t>(transition_state); }
 
-    bool active() const override;
+    bool active_frwd() const override;
 
     bool show_vtol_view() const override;
 
diff --git a/Tools/AP_Bootloader/board_types.txt b/Tools/AP_Bootloader/board_types.txt
index 3887710b9ae2b9..bf801d779604d8 100644
--- a/Tools/AP_Bootloader/board_types.txt
+++ b/Tools/AP_Bootloader/board_types.txt
@@ -321,6 +321,8 @@ AP_HW_MountainEagleH743              1444
 AP_HW_StellarF4                      1500
 AP_HW_GEPRCF745BTHD                  1501
 
+AP_HW_HGLRCF405V4                    1524
+
 AP_HW_MFT-SEMA100                    2000
 
 AP_HW_SakuraRC-H743                  2714
@@ -374,6 +376,10 @@ AP_HW_ZeroOne_X6                     5600
 AP_HW_ZeroOne_PMU                    5601
 AP_HW_ZeroOne_GNSS                   5602
 
+#IDs 5700-5710 reserved for DroneBuild  
+AP_HW_DroneBuild_G1                  5700
+AP_HW_DroneBuild_G2                  5701
+
 # IDs 6000-6099 reserved for SpektreWorks
 
 # IDs 6600-6699 reserved for Eagle Eye Drones
diff --git a/Tools/AP_Bootloader/network.cpp b/Tools/AP_Bootloader/network.cpp
index eb47c09ef80672..64270187a8e66d 100644
--- a/Tools/AP_Bootloader/network.cpp
+++ b/Tools/AP_Bootloader/network.cpp
@@ -484,10 +484,14 @@ void BL_Network::handle_request(SocketAPM *sock)
     sock->send(header, strlen(header));
 
     if (strncmp(headers, "POST / ", 7) == 0) {
-        const char *clen = "\r\nContent-Length:";
-        const char *p = strstr(headers, clen);
+        const char *clen1 = "\r\nContent-Length:";
+        const char *clen2 = "\r\ncontent-length:";
+        const char *p = strstr(headers, clen1);
+        if (p == nullptr) {
+            p = strstr(headers, clen2);
+        }
         if (p != nullptr) {
-            p += strlen(clen);
+            p += strlen(clen1);
             const uint32_t content_length = atoi(p);
             handle_post(sock, content_length);
             delete headers;
diff --git a/Tools/ardupilotwaf/ardupilotwaf.py b/Tools/ardupilotwaf/ardupilotwaf.py
index 1c807ccd6b0f18..36a352bfa72a8b 100644
--- a/Tools/ardupilotwaf/ardupilotwaf.py
+++ b/Tools/ardupilotwaf/ardupilotwaf.py
@@ -6,6 +6,7 @@
 from waflib.Scripting import run_command
 from waflib.TaskGen import before_method, after_method, feature
 import os.path, os
+from pathlib import Path
 from collections import OrderedDict
 import subprocess
 
@@ -261,6 +262,30 @@ def ap_common_vehicle_libraries(bld):
 
 _grouped_programs = {}
 
+
+class upload_fw_blueos(Task.Task):
+    def run(self):
+        # this is rarely used, so we import requests here to avoid the overhead
+        import requests
+        binary_path = self.inputs[0].abspath()
+        # check if .apj file exists for chibios builds
+        if Path(binary_path + ".apj").exists():
+            binary_path = binary_path + ".apj"
+        bld = self.generator.bld
+        board = bld.bldnode.name.capitalize()
+        print(f"Uploading {binary_path} to BlueOS at {bld.options.upload_blueos} for board {board}")
+        url = f'{bld.options.upload_blueos}/ardupilot-manager/v1.0/install_firmware_from_file?board_name={board}'
+        files = {
+          'binary': open(binary_path, 'rb')
+        }
+        response = requests.post(url, files=files, verify=False)
+        if response.status_code != 200:
+            raise Errors.WafError(f"Failed to upload firmware to BlueOS: {response.status_code}: {response.text}")
+        print("Upload complete")
+
+    def keyword(self):
+          return "Uploading to BlueOS"
+
 class check_elf_symbols(Task.Task):
     color='CYAN'
     always_run = True
@@ -309,7 +334,10 @@ def post_link(self):
 
     check_elf_task = self.create_task('check_elf_symbols', src=link_output)
     check_elf_task.set_run_after(self.link_task)
-    
+    if self.bld.options.upload_blueos and self.env["BOARD_CLASS"] == "LINUX":
+        _upload_task = self.create_task('upload_fw_blueos', src=link_output)
+        _upload_task.set_run_after(self.link_task)
+
 @conf
 def ap_program(bld,
                program_groups='bin',
@@ -641,6 +669,13 @@ def options(opt):
         help='''Specify the port to be used with the --upload option. For example a port of /dev/ttyS10 indicates that serial port 10 shuld be used.
 ''')
 
+    g.add_option('--upload-blueos',
+        action='store',
+        dest='upload_blueos',
+        default=None,
+        help='''Automatically upload to a BlueOS device. The argument is the url for the device. http://blueos.local for example.
+''')
+
     g.add_option('--upload-force',
         action='store_true',
         help='''Override board type check and continue loading. Same as using uploader.py --force.
diff --git a/Tools/ardupilotwaf/chibios.py b/Tools/ardupilotwaf/chibios.py
index 7e20f40fc58b26..60fd1ef4ee2418 100644
--- a/Tools/ardupilotwaf/chibios.py
+++ b/Tools/ardupilotwaf/chibios.py
@@ -452,6 +452,10 @@ def chibios_firmware(self):
         _upload_task = self.create_task('upload_fw', src=apj_target)
         _upload_task.set_run_after(generate_apj_task)
 
+    if self.bld.options.upload_blueos:
+        _upload_task = self.create_task('upload_fw_blueos', src=link_output)
+        _upload_task.set_run_after(generate_apj_task)
+
 def setup_canmgr_build(cfg):
     '''enable CANManager build. By doing this here we can auto-enable CAN in
     the build based on the presence of CAN pins in hwdef.dat except for AP_Periph builds'''
diff --git a/Tools/autotest/arducopter.py b/Tools/autotest/arducopter.py
index 24db4cec1f07e9..d0fbc73cee7f0b 100644
--- a/Tools/autotest/arducopter.py
+++ b/Tools/autotest/arducopter.py
@@ -9142,20 +9142,90 @@ def GPSWeightedBlending(self):
             if m.TimeUS != current_ts:
                 current_ts = None
                 continue
-            measurements[m.I] = (m.Lat, m.Lng)
+            measurements[m.I] = (m.Lat, m.Lng, m.Alt)
             if len(measurements) == 3:
                 # check lat:
-                for n in 0, 1:
+                for n in 0, 1, 2:
                     expected_blended = 0.8*measurements[0][n] + 0.2*measurements[1][n]
-                    epsilon = 0.0000002
+                    axis_epsilons = [0.0000002, 0.0000002, 0.2]
+                    epsilon = axis_epsilons[n]
                     error = abs(measurements[2][n] - expected_blended)
                     if error > epsilon:
-                        raise NotAchievedException(f"Blended diverged {measurements[0][n]=} {measurements[1][n]=}")
+                        raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=}")
                 current_ts = None
 
         self.context_pop()
         self.reboot_sitl()
 
+    def GPSBlendingAffinity(self):
+        '''test blending when affinity in use'''
+        # configure:
+        self.set_parameters({
+            "WP_YAW_BEHAVIOR": 0,  # do not yaw
+            "GPS2_TYPE": 1,
+            "SIM_GPS2_TYPE": 1,
+            "SIM_GPS2_DISABLE": 0,
+            "SIM_GPS_POS_X": 1.0,
+            "SIM_GPS_POS_Y": -1.0,
+            "SIM_GPS2_POS_X": -1.0,
+            "SIM_GPS2_POS_Y": 1.0,
+            "GPS_AUTO_SWITCH": 2,
+
+            "EK3_AFFINITY" : 1,
+            "EK3_IMU_MASK": 7,
+            "SIM_IMU_COUNT": 3,
+            "INS_ACC3OFFS_X": 0.001,
+            "INS_ACC3OFFS_Y": 0.001,
+            "INS_ACC3OFFS_Z": 0.001,
+        })
+        # force-calibration of accel:
+        self.run_cmd(mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION, p5=76)
+        self.reboot_sitl()
+
+        alt = 10
+        self.takeoff(alt, mode='GUIDED')
+        self.fly_guided_move_local(30, 0, alt)
+        self.fly_guided_move_local(30, 30, alt)
+        self.fly_guided_move_local(0, 30, alt)
+        self.fly_guided_move_local(0, 0, alt)
+        self.change_mode('LAND')
+
+        current_log_file = self.dfreader_for_current_onboard_log()
+
+        self.wait_disarmed()
+
+        # ensure that the blended solution is always about half-way
+        # between the two GPSs:
+        current_ts = None
+        max_errors = [0, 0, 0]
+        while True:
+            m = current_log_file.recv_match(type='XKF1')
+            if m is None:
+                break
+            if current_ts is None:
+                if m.C != 0:  # noqa
+                    continue
+                current_ts = m.TimeUS
+                measurements = {}
+            if m.TimeUS != current_ts:
+                current_ts = None
+                continue
+            measurements[m.C] = (m.PN, m.PE, m.PD)
+            if len(measurements) == 3:
+                # check lat:
+                for n in 0, 1, 2:
+                    expected_blended = 0.5*measurements[0][n] + 0.5*measurements[1][n]
+                    axis_epsilons = [0.02, 0.02, 0.03]
+                    epsilon = axis_epsilons[n]
+                    error = abs(measurements[2][n] - expected_blended)
+                    # self.progress(f"{n=} {error=}")
+                    if error > max_errors[n]:
+                        max_errors[n] = error
+                    if error > epsilon:
+                        raise NotAchievedException(f"Blended diverged {n=} {measurements[0][n]=} {measurements[1][n]=} {measurements[2][n]=} {error=}")  # noqa:E501
+                current_ts = None
+        self.progress(f"{max_errors=}")
+
     def Callisto(self):
         '''Test Callisto'''
         self.customise_SITL_commandline(
@@ -11791,6 +11861,7 @@ def tests2b(self):  # this block currently around 9.5mins here
             self.GPSBlending,
             self.GPSWeightedBlending,
             self.GPSBlendingLog,
+            self.GPSBlendingAffinity,
             self.DataFlash,
             Test(self.DataFlashErase, attempts=8),
             self.Callisto,
diff --git a/Tools/autotest/default_params/blimp.parm b/Tools/autotest/default_params/blimp.parm
index a790eec33454f5..0eeca63cef7890 100644
--- a/Tools/autotest/default_params/blimp.parm
+++ b/Tools/autotest/default_params/blimp.parm
@@ -1,7 +1,4 @@
-ARSPD_PIN 1
-ARSPD_BUS 2
 EK2_ENABLE      1
-FRAME_TYPE	0
 FS_THR_ENABLE   1
 BATT_MONITOR    4
 COMPASS_OFS_X   5
diff --git a/Tools/autotest/default_params/plane-ice.parm b/Tools/autotest/default_params/plane-ice.parm
index 28b135b98e2b99..e6cb59cad0102c 100644
--- a/Tools/autotest/default_params/plane-ice.parm
+++ b/Tools/autotest/default_params/plane-ice.parm
@@ -2,6 +2,6 @@ ICE_ENABLE 1
 ICE_RPM_CHAN 1
 SERVO13_FUNCTION 67
 SERVO14_FUNCTION 69
-ICE_START_CHAN 7
+RC11_OPTION 179
 RPM1_TYPE 10
 SERVO3_REVERSED 1
diff --git a/Tools/autotest/default_params/quadplane-cl84.parm b/Tools/autotest/default_params/quadplane-cl84.parm
index fee738bef83beb..500a34e5b4d5b1 100644
--- a/Tools/autotest/default_params/quadplane-cl84.parm
+++ b/Tools/autotest/default_params/quadplane-cl84.parm
@@ -1,7 +1,6 @@
 Q_FRAME_CLASS    7
 Q_TILT_ENABLE    1
 Q_TILT_MASK      3
-Q_TILT_RATE      13
 Q_TILT_TYPE      1
 
 # 7 seconds to move servo
diff --git a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm
index 6cafdcb50c220c..acd8243d109cc7 100644
--- a/Tools/autotest/default_params/quadplane-copter_tailsitter.parm
+++ b/Tools/autotest/default_params/quadplane-copter_tailsitter.parm
@@ -17,21 +17,17 @@ Q_A_RATE_R_MAX   0.000000
 Q_A_RATE_Y_MAX   0.000000
 Q_A_RAT_PIT_D    0.004973
 Q_A_RAT_PIT_FF   0.000000
-Q_A_RAT_PIT_FILT 10.000000
 Q_A_RAT_PIT_I    0.528246
 Q_A_RAT_PIT_IMAX 0.500000
 Q_A_RAT_PIT_P    0.528246
 Q_A_RAT_RLL_D    0.001000
 Q_A_RAT_RLL_FF   0.000000
-Q_A_RAT_RLL_FILT 10.000000
 Q_A_RAT_RLL_I    0.541613
 Q_A_RAT_RLL_IMAX 0.500000
 Q_A_RAT_RLL_P    0.541613
 Q_A_RAT_YAW_D    0.000000
 Q_A_RAT_YAW_FF   0.000000
-Q_A_RAT_YAW_FILT 1.073194
 Q_A_RAT_YAW_I    0.039192
 Q_A_RAT_YAW_IMAX 0.500000
 Q_A_RAT_YAW_P    0.391919
-Q_TAILSIT_THSCMX 1.5
 Q_TRANS_DECEL 8
diff --git a/Tools/autotest/quadplane.py b/Tools/autotest/quadplane.py
index 1395e245c22d89..1282f9213c0625 100644
--- a/Tools/autotest/quadplane.py
+++ b/Tools/autotest/quadplane.py
@@ -1049,12 +1049,8 @@ def Tailsitter(self):
                 raise NotAchievedException("Changed throttle output on mode change to QHOVER")
         self.disarm_vehicle()
 
-    def setup_ICEngine_vehicle(self, start_chan):
+    def setup_ICEngine_vehicle(self):
         '''restarts SITL with an IC Engine setup'''
-        self.set_parameters({
-            'ICE_START_CHAN': start_chan,
-        })
-
         model = "quadplane-ice"
         self.customise_SITL_commandline(
             [],
@@ -1066,7 +1062,7 @@ def setup_ICEngine_vehicle(self, start_chan):
     def ICEngine(self):
         '''Test ICE Engine support'''
         rc_engine_start_chan = 11
-        self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
+        self.setup_ICEngine_vehicle()
 
         self.wait_ready_to_arm()
         self.wait_rpm(1, 0, 0, minimum_duration=1)
@@ -1105,7 +1101,7 @@ def ICEngine(self):
     def ICEngineMission(self):
         '''Test ICE Engine Mission support'''
         rc_engine_start_chan = 11
-        self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
+        self.setup_ICEngine_vehicle()
 
         self.load_mission("mission.txt")
         self.wait_ready_to_arm()
@@ -1123,7 +1119,7 @@ def MAV_CMD_DO_ENGINE_CONTROL(self):
         expected_starter_rpm_max = 355
 
         rc_engine_start_chan = 11
-        self.setup_ICEngine_vehicle(start_chan=rc_engine_start_chan)
+        self.setup_ICEngine_vehicle()
 
         self.wait_ready_to_arm()
 
@@ -1560,6 +1556,29 @@ def mavlink_MAV_CMD_DO_VTOL_TRANSITION(self):
 
         self.fly_home_land_and_disarm()
 
+    def TransitionMinThrottle(self):
+        '''Ensure that TKOFF_THR_MIN is applied during the forward transition'''
+        wps = self.create_simple_relhome_mission([
+            (mavutil.mavlink.MAV_CMD_NAV_TAKEOFF, 0, 0, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2000, 0, 30),
+            (mavutil.mavlink.MAV_CMD_NAV_RETURN_TO_LAUNCH, 0, 0, 0),
+        ])
+        self.check_mission_upload_download(wps)
+        self.set_parameter('TKOFF_THR_MIN', 80)
+
+        self.change_mode('AUTO')
+        self.wait_ready_to_arm()
+
+        self.arm_vehicle()
+        self.wait_current_waypoint(2)
+        # Wait for 5 seconds into the transition.
+        self.delay_sim_time(5)
+        # Ensure TKOFF_THR_MIN is still respected.
+        thr_min = self.get_parameter('TKOFF_THR_MIN')
+        self.wait_servo_channel_value(3, 1000+thr_min*10, comparator=operator.eq)
+
+        self.fly_home_land_and_disarm()
+
     def MAV_CMD_NAV_TAKEOFF(self):
         '''test issuing takeoff command via mavlink'''
         self.change_mode('GUIDED')
@@ -1815,6 +1834,7 @@ def tests(self):
             self.RCDisableAirspeedUse,
             self.mission_MAV_CMD_DO_VTOL_TRANSITION,
             self.mavlink_MAV_CMD_DO_VTOL_TRANSITION,
+            self.TransitionMinThrottle,
             self.MAV_CMD_NAV_TAKEOFF,
             self.Q_GUIDED_MODE,
             self.DCMClimbRate,
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
index 65041324a94330..eb7c8dbf5940ee 100644
--- a/Tools/autotest/rover.py
+++ b/Tools/autotest/rover.py
@@ -6803,6 +6803,48 @@ def RCDuplicateOptionsExist(self):
         })
         self.assert_arm_failure("Duplicate Aux Switch Options")
 
+    def JammingSimulation(self):
+        '''Test jamming simulation works'''
+        self.wait_ready_to_arm()
+        start_loc = self.assert_receive_message('GPS_RAW_INT')
+        self.set_parameter("SIM_GPS_JAM", 1)
+
+        class Requirement():
+            def __init__(self, field, min_value):
+                self.field = field
+                self.min_value = min_value
+
+            def met(self, m):
+                return getattr(m, self.field) > self.min_value
+
+        requirements = set([
+            Requirement('v_acc', 50000),
+            Requirement('h_acc', 50000),
+            Requirement('vel_acc', 1000),
+            Requirement('vel', 10000),
+        ])
+        low_sats_seen = False
+        seen_bad_loc = False
+        tstart = self.get_sim_time()
+
+        while True:
+            if self.get_sim_time() - tstart > 120:
+                raise NotAchievedException("Did not see all jamming")
+            m = self.assert_receive_message('GPS_RAW_INT')
+            new_requirements = copy.copy(requirements)
+            for requirement in requirements:
+                if requirement.met(m):
+                    new_requirements.remove(requirement)
+            requirements = new_requirements
+            if m.satellites_visible < 6:
+                low_sats_seen = True
+            here = self.assert_receive_message('GPS_RAW_INT')
+            if self.get_distance_int(start_loc, here) > 100:
+                seen_bad_loc = True
+
+            if len(requirements) == 0 and low_sats_seen and seen_bad_loc:
+                break
+
     def tests(self):
         '''return list of all tests'''
         ret = super(AutoTestRover, self).tests()
@@ -6898,6 +6940,7 @@ def tests(self):
             self.OpticalFlow,
             self.RCDuplicateOptionsExist,
             self.ClearMission,
+            self.JammingSimulation,
         ])
         return ret
 
diff --git a/Tools/bootloaders/CubePilot-PPPGW_bl.bin b/Tools/bootloaders/CubePilot-PPPGW_bl.bin
index 7d19a35945cbd8..328f581b23c404 100755
Binary files a/Tools/bootloaders/CubePilot-PPPGW_bl.bin and b/Tools/bootloaders/CubePilot-PPPGW_bl.bin differ
diff --git a/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin b/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin
index 825898a87278da..4254a87d1b0e2f 100755
Binary files a/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin and b/Tools/bootloaders/CubeRedPrimary-PPPGW_bl.bin differ
diff --git a/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin b/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin
index 9776ee4968e0a6..d51d5080a7eb17 100755
Binary files a/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin and b/Tools/bootloaders/Pixhawk6X-PPPGW_bl.bin differ
diff --git a/Tools/environment_install/install-prereqs-ubuntu.sh b/Tools/environment_install/install-prereqs-ubuntu.sh
index 5dece4504fb242..2a125faa42b1fd 100755
--- a/Tools/environment_install/install-prereqs-ubuntu.sh
+++ b/Tools/environment_install/install-prereqs-ubuntu.sh
@@ -62,6 +62,9 @@ RELEASE_CODENAME=$(lsb_release -c -s)
 
 # translate Mint-codenames to Ubuntu-codenames based on https://www.linuxmint.com/download_all.php
 case ${RELEASE_CODENAME} in
+    wilma)
+        RELEASE_CODENAME='noble'
+        ;;
     vanessa)
         RELEASE_CODENAME='jammy'
         ;;
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index db2f9dde5656e9..3d3039f25be098 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -35,7 +35,6 @@ def __init__(self,
     Feature('AHRS', 'MicroStrain7', 'AP_EXTERNAL_AHRS_MICROSTRAIN7_ENABLED', 'Enable MICROSTRAIN 7-series External AHRS', 0, "AHRS_EXT"),  # noqa: E501
     Feature('AHRS', 'AHRS_EXT_VECTORNAV', 'AP_EXTERNAL_AHRS_VECTORNAV_ENABLED', 'Enable VectorNav External AHRS', 0, "AHRS_EXT"),  # noqa
     Feature('AHRS', 'InertialLabs', 'AP_EXTERNAL_AHRS_INERTIALLABS_ENABLED', 'Enable InertialLabs External AHRS', 0, "AHRS_EXT"),  # noqa
-    Feature('AHRS', 'TEMPCAL', 'HAL_INS_TEMPERATURE_CAL_ENABLE', 'Enable IMU Temperature Calibration', 0, None),
     Feature('AHRS', 'VISUALODOM', 'HAL_VISUALODOM_ENABLED', 'Enable Visual Odometry', 0, 'EKF3_EXTNAV'),
     Feature('AHRS', 'EKF3_EXTNAV', 'EK3_FEATURE_EXTERNAL_NAV', 'Enable External Navigation for EKF3', 0, 'EKF3'),
     Feature('AHRS', 'EKF3_WINDEST', 'EK3_FEATURE_DRAG_FUSION', 'Enable Wind Estimation for EKF3', 0, 'EKF3'),
@@ -324,7 +323,10 @@ def __init__(self,
     Feature('Sensors', 'GPS_MOVING_BASELINE', 'GPS_MOVING_BASELINE', 'Enable GPS Moving Baseline', 0, None),
     Feature('Sensors', 'IMU_ON_UART', 'AP_SERIALMANAGER_IMUOUT_ENABLED', 'Enable sending raw IMU data on a serial port', 0, None), # NOQA: E501
 
-    Feature('Other', 'HarmonicNotches', 'AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', 'Enable InertialSensor Harmonic Notches', 0, None),  # noqa
+    Feature('IMU', 'TEMPCAL', 'HAL_INS_TEMPERATURE_CAL_ENABLE', 'Enable IMU Temperature Calibration', 0, None),
+    Feature('IMU', 'HarmonicNotches', 'AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', 'Enable InertialSensor Harmonic Notches', 0, None),  # noqa
+    Feature('IMU', 'BatchSampler', 'AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', 'Enable Batch Sampler', 0, None),  # noqa
+
     Feature('Other', 'GyroFFT', 'HAL_GYROFFT_ENABLED', 'Enable In-Flight Gyro FFT calculations', 0, None),
     Feature('Other', 'NMEA_OUTPUT', 'HAL_NMEA_OUTPUT_ENABLED', 'Enable NMEA Output', 0, None),
     Feature('Other', 'SDCARD_FORMATTING', 'AP_FILESYSTEM_FORMAT_ENABLED', 'Enable formatting of microSD cards', 0, None),
diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index 454e57391ca6e5..0454b5cb614614 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -202,6 +202,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
 
             ('HAL_WITH_DSP', r'AP_HAL::DSP::find_peaks\b',),
             ('AP_INERTIALSENSOR_HARMONICNOTCH_ENABLED', r'AP_InertialSensor::HarmonicNotch::update_params\b',),
+            ('AP_INERTIALSENSOR_BATCHSAMPLER_ENABLED', r'AP_InertialSensor::BatchSampler::init'),
             ('HAL_GYROFFT_ENABLED', r'AP_GyroFFT::AP_GyroFFT\b',),
             ('HAL_DISPLAY_ENABLED', r'Display::init\b',),
             ('HAL_NMEA_OUTPUT_ENABLED', r'AP_NMEA_Output::update\b',),
diff --git a/libraries/AP_Arming/AP_Arming.cpp b/libraries/AP_Arming/AP_Arming.cpp
index 41ec47659d650f..76f8159804ec45 100644
--- a/libraries/AP_Arming/AP_Arming.cpp
+++ b/libraries/AP_Arming/AP_Arming.cpp
@@ -649,12 +649,6 @@ bool AP_Arming::gps_checks(bool report)
                          (double)distance_m);
             return false;
         }
-#if AP_GPS_BLENDED_ENABLED
-        if (!gps.blend_health_check()) {
-            check_failed(ARMING_CHECK_GPS, report, "GPS blending unhealthy");
-            return false;
-        }
-#endif
 
         // check AHRS and GPS are within 10m of each other
         if (gps.num_sensors() > 0) {
diff --git a/libraries/AP_Beacon/AP_Beacon.cpp b/libraries/AP_Beacon/AP_Beacon.cpp
index 1b9de318d44767..c2044ad7585e52 100644
--- a/libraries/AP_Beacon/AP_Beacon.cpp
+++ b/libraries/AP_Beacon/AP_Beacon.cpp
@@ -28,6 +28,10 @@
 
 extern const AP_HAL::HAL &hal;
 
+#ifndef AP_BEACON_MINIMUM_FENCE_BEACONS
+#define AP_BEACON_MINIMUM_FENCE_BEACONS 3
+#endif
+
 // table of user settable parameters
 const AP_Param::GroupInfo AP_Beacon::var_info[] = {
 
diff --git a/libraries/AP_Beacon/AP_Beacon.h b/libraries/AP_Beacon/AP_Beacon.h
index 98f7c377750028..aaebfa86ad7c33 100644
--- a/libraries/AP_Beacon/AP_Beacon.h
+++ b/libraries/AP_Beacon/AP_Beacon.h
@@ -25,10 +25,6 @@
 
 class AP_Beacon_Backend;
 
-#define AP_BEACON_MAX_BEACONS 4
-#define AP_BEACON_TIMEOUT_MS 300
-#define AP_BEACON_MINIMUM_FENCE_BEACONS 3
-
 class AP_Beacon
 {
 public:
diff --git a/libraries/AP_Beacon/AP_Beacon_config.h b/libraries/AP_Beacon/AP_Beacon_config.h
index 38d35331421df9..d3d866015f7357 100644
--- a/libraries/AP_Beacon/AP_Beacon_config.h
+++ b/libraries/AP_Beacon/AP_Beacon_config.h
@@ -6,6 +6,14 @@
 #define AP_BEACON_ENABLED 1
 #endif
 
+#ifndef AP_BEACON_MAX_BEACONS
+#define AP_BEACON_MAX_BEACONS 4
+#endif
+
+#ifndef AP_BEACON_TIMEOUT_MS
+#define AP_BEACON_TIMEOUT_MS 300
+#endif
+
 #ifndef AP_BEACON_BACKEND_DEFAULT_ENABLED
 #define AP_BEACON_BACKEND_DEFAULT_ENABLED AP_BEACON_ENABLED
 #endif
diff --git a/libraries/AP_Common/float16.cpp b/libraries/AP_Common/float16.cpp
index e4ac982f48308c..c5def290149e94 100644
--- a/libraries/AP_Common/float16.cpp
+++ b/libraries/AP_Common/float16.cpp
@@ -2,6 +2,8 @@
 /*
   float16 implementation
 
+  Note that this is IEEE half-precision 16-bit float, *not* bfloat16
+
   algorithm with thanks to libcanard:
   https://github.com/dronecan/libcanard
 */
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.cpp b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
index ee2dfe2eba70a2..5e5637fee17799 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.cpp
@@ -80,10 +80,6 @@ extern const AP_HAL::HAL& hal;
 #define DRONECAN_STACK_SIZE     4096
 #endif
 
-#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
-#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0
-#endif
-
 #ifndef AP_DRONECAN_DEFAULT_NODE
 #define AP_DRONECAN_DEFAULT_NODE 10
 #endif
@@ -1424,7 +1420,7 @@ void AP_DroneCAN::handle_actuator_status_Volz(const CanardRxTransfer& transfer,
         ToDeg(msg.actual_position),
         msg.current * 0.025f,
         msg.voltage * 0.2f,
-        msg.motor_pwm * (100.0/255.0),
+        uint8_t(msg.motor_pwm * (100.0/255.0)),
         int16_t(msg.motor_temperature) - 50);
 #endif
 }
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN.h b/libraries/AP_DroneCAN/AP_DroneCAN.h
index b5527fb5947cc6..e1b713d33f1ad5 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN.h
@@ -64,6 +64,10 @@
 #define AP_DRONECAN_SERIAL_ENABLED AP_SERIALMANAGER_REGISTER_ENABLED && (BOARD_FLASH_SIZE>1024)
 #endif
 
+#ifndef AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
+#define AP_DRONECAN_VOLZ_FEEDBACK_ENABLED 0
+#endif
+
 #if AP_DRONECAN_SERIAL_ENABLED
 #include "AP_DroneCAN_serial.h"
 #endif
@@ -334,6 +338,11 @@ class AP_DroneCAN : public AP_CANDriver, public AP_ESC_Telem_Backend {
     Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_debug_LogMessage> debug_cb{this, &AP_DroneCAN::handle_debug};
     Canard::Subscriber<uavcan_protocol_debug_LogMessage> debug_listener{debug_cb, _driver_index};
 
+#if AP_DRONECAN_VOLZ_FEEDBACK_ENABLED
+    Canard::ObjCallback<AP_DroneCAN, com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_cb{this, &AP_DroneCAN::handle_actuator_status_Volz};
+    Canard::Subscriber<com_volz_servo_ActuatorStatus> volz_servo_ActuatorStatus_listener{volz_servo_ActuatorStatus_cb, _driver_index};
+#endif
+
     // param client
     Canard::ObjCallback<AP_DroneCAN, uavcan_protocol_param_GetSetResponse> param_get_set_res_cb{this, &AP_DroneCAN::handle_param_get_set_response};
     Canard::Client<uavcan_protocol_param_GetSetResponse> param_get_set_client{canard_iface, param_get_set_res_cb};
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index 2492e8f7370951..6ca80fccc82fee 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -29,6 +29,7 @@
 #include <AP_SerialManager/AP_SerialManager.h>
 
 #include "AP_GPS_NOVA.h"
+#include "AP_GPS_Blended.h"
 #include "AP_GPS_ERB.h"
 #include "AP_GPS_GSOF.h"
 #include "AP_GPS_NMEA.h"
@@ -59,6 +60,12 @@
 #include "RTCM3_Parser.h"
 #endif
 
+#if !AP_GPS_BLENDED_ENABLED
+#if defined(GPS_BLENDED_INSTANCE)
+#error GPS_BLENDED_INSTANCE should not be defined when AP_GPS_BLENDED_ENABLED is false
+#endif
+#endif
+
 #define GPS_RTK_INJECT_TO_ALL 127
 #ifndef GPS_MAX_RATE_MS
 #define GPS_MAX_RATE_MS 200 // maximum value of rate_ms (i.e. slowest update rate) is 5hz or 200ms
@@ -66,8 +73,6 @@
 #define GPS_BAUD_TIME_MS 1200
 #define GPS_TIMEOUT_MS 4000u
 
-#define BLEND_COUNTER_FAILURE_INCREMENT 10
-
 extern const AP_HAL::HAL &hal;
 
 // baudrates to try to detect GPSes with
@@ -347,6 +352,11 @@ void AP_GPS::init()
             rate_ms.set(GPS_MAX_RATE_MS);
         }
     }
+
+    // create the blended instance if appropriate:
+#if AP_GPS_BLENDED_ENABLED
+    drivers[GPS_BLENDED_INSTANCE] = NEW_NOTHROW AP_GPS_Blended(*this, params[GPS_BLENDED_INSTANCE], state[GPS_BLENDED_INSTANCE], timing[GPS_BLENDED_INSTANCE]);
+#endif
 }
 
 void AP_GPS::convert_parameters()
@@ -1087,25 +1097,15 @@ void AP_GPS::update_primary(void)
     */
     const bool using_moving_base = is_rtk_base(0) || is_rtk_base(1);
     if ((GPSAutoSwitch)_auto_switch.get() == GPSAutoSwitch::BLEND && !using_moving_base) {
-        _output_is_blended = calc_blend_weights();
-        // adjust blend health counter
-        if (!_output_is_blended) {
-            _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
-        } else if (_blend_health_counter > 0) {
-            _blend_health_counter--;
-        }
-        // stop blending if unhealthy
-        if (_blend_health_counter >= 50) {
-            _output_is_blended = false;
-        }
+        _output_is_blended = ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_weights();
     } else {
         _output_is_blended = false;
-        _blend_health_counter = 0;
+        ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->zero_health_counter();
     }
 
     if (_output_is_blended) {
         // Use the weighting to calculate blended GPS states
-        calc_blended_state();
+        ((AP_GPS_Blended*)drivers[GPS_BLENDED_INSTANCE])->calc_state();
         // set primary to the virtual instance
         primary_instance = GPS_BLENDED_INSTANCE;
         return;
@@ -1698,10 +1698,7 @@ bool AP_GPS::get_lag(uint8_t instance, float &lag_sec) const
 #if AP_GPS_BLENDED_ENABLED
     // return lag of blended GPS
     if (instance == GPS_BLENDED_INSTANCE) {
-        lag_sec = _blended_lag_sec;
-        // auto switching uses all GPS receivers, so all must be configured
-        uint8_t inst; // we don't actually care what the number is, but must pass it
-        return first_unconfigured_gps(inst);
+        return drivers[instance]->get_lag(lag_sec);
     }
 #endif
 
@@ -1735,7 +1732,7 @@ const Vector3f &AP_GPS::get_antenna_offset(uint8_t instance) const
 #if AP_GPS_BLENDED_ENABLED
     if (instance == GPS_BLENDED_INSTANCE) {
         // return an offset for the blended GPS solution
-        return _blended_antenna_offset;
+        return ((AP_GPS_Blended*)drivers[instance])->get_antenna_offset();
     }
 #endif
 
@@ -1791,12 +1788,6 @@ bool AP_GPS::is_healthy(uint8_t instance) const
     }
 #endif // HAL_BUILD_AP_PERIPH
 
-#if AP_GPS_BLENDED_ENABLED
-    if (instance == GPS_BLENDED_INSTANCE) {
-        return blend_health_check();
-    }
-#endif
-
     return drivers[instance] != nullptr &&
            drivers[instance]->is_healthy();
 }
@@ -1829,6 +1820,14 @@ bool AP_GPS::pre_arm_checks(char failure_msg[], uint16_t failure_msg_len)
             }
         }
     }
+
+#if AP_GPS_BLENDED_ENABLED
+    if (!drivers[GPS_BLENDED_INSTANCE]->is_healthy()) {
+        hal.util->snprintf(failure_msg, failure_msg_len, "GPS blending unhealthy");
+        return false;
+    }
+#endif
+
     return true;
 }
 
@@ -1999,31 +1998,31 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _TYPE
     // @DisplayName: 1st GPS type
-    // @Description: GPS type of 1st GPS
+    // @Description: GPS type of 1st GPS.Renamed in 4.6 and later to GPS1_TYPE
     // @Values: 0:None,1:AUTO,2:uBlox,5:NMEA,6:SiRF,7:HIL,8:SwiftNav,9:DroneCAN,10:SBF,11:GSOF,13:ERB,14:MAV,15:NOVA,16:HemisphereNMEA,17:uBlox-MovingBaseline-Base,18:uBlox-MovingBaseline-Rover,19:MSP,20:AllyStar,21:ExternalAHRS,22:DroneCAN-MovingBaseline-Base,23:DroneCAN-MovingBaseline-Rover,24:UnicoreNMEA,25:UnicoreMovingBaselineNMEA,26:SBF-DualAntenna
     // @RebootRequired: True
     // @User: Advanced
 
     // @Param: _TYPE2
     // @CopyFieldsFrom: GPS_TYPE
-    // @DisplayName: 2nd GPS type
+    // @DisplayName: 2nd GPS type.Renamed in 4.6 to GPS2_TYPE
     // @Description: GPS type of 2nd GPS
 
     // @Param: _GNSS_MODE
     // @DisplayName: GNSS system configuration
-    // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured)
+    // @Description: Bitmask for what GNSS system to use on the first GPS (all unchecked or zero to leave GPS as configured).Renamed in 4.6 and later to GPS1_GNSS_MODE.
     // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
     // @User: Advanced
 
     // @Param: _GNSS_MODE2
-    // @DisplayName: GNSS system configuration
-    // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured)
+    // @DisplayName: GNSS system configuration.
+    // @Description: Bitmask for what GNSS system to use on the second GPS (all unchecked or zero to leave GPS as configured). Renamed in 4.6 and later to GPS2_GNSS_MODE
     // @Bitmask: 0:GPS,1:SBAS,2:Galileo,3:Beidou,4:IMES,5:QZSS,6:GLONASS
     // @User: Advanced
 
     // @Param: _RATE_MS
     // @DisplayName: GPS update rate in milliseconds
-    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.
+    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.Renamed in 4.6 and later to GPS1_RATE_MS
     // @Units: ms
     // @Values: 100:10Hz,125:8Hz,200:5Hz
     // @Range: 50 200
@@ -2031,7 +2030,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _RATE_MS2
     // @DisplayName: GPS 2 update rate in milliseconds
-    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.
+    // @Description: Controls how often the GPS should provide a position update. Lowering below 5Hz(default) is not allowed. Raising the rate above 5Hz usually provides little benefit and for some GPS (eg Ublox M9N) can severely impact performance.Renamed in 4.6 and later to GPS2_RATE_MS
     // @Units: ms
     // @Values: 100:10Hz,125:8Hz,200:5Hz
     // @Range: 50 200
@@ -2039,7 +2038,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS1_X
     // @DisplayName: Antenna X position offset
-    // @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: X position of the first GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_X.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2047,7 +2046,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS1_Y
     // @DisplayName: Antenna Y position offset
-    // @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Y position of the first GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_Y.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2055,7 +2054,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS1_Z
     // @DisplayName: Antenna Z position offset
-    // @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Z position of the first GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS1_POS_Z.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2063,7 +2062,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS2_X
     // @DisplayName: Antenna X position offset
-    // @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: X position of the second GPS antenna in body frame. Positive X is forward of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_X.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2071,7 +2070,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS2_Y
     // @DisplayName: Antenna Y position offset
-    // @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Y position of the second GPS antenna in body frame. Positive Y is to the right of the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_Y.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2079,7 +2078,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _POS2_Z
     // @DisplayName: Antenna Z position offset
-    // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Z position of the second GPS antenna in body frame. Positive Z is down from the origin. Use antenna phase centroid location if provided by the manufacturer.Renamed in 4.6 and later to GPS2_POS_Z.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -2087,7 +2086,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _DELAY_MS
     // @DisplayName: GPS delay in milliseconds
-    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.Renamed in 4.6 and later to GPS1_DELAY_MS.
     // @Units: ms
     // @Range: 0 250
     // @User: Advanced
@@ -2095,7 +2094,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _DELAY_MS2
     // @DisplayName: GPS 2 delay in milliseconds
-    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.
+    // @Description: Controls the amount of GPS  measurement delay that the autopilot compensates for. Set to zero to use the default delay for the detected GPS type.Renamed in 4.6 and later to GPS2_DELAY_MS.
     // @Units: ms
     // @Range: 0 250
     // @User: Advanced
@@ -2103,7 +2102,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _COM_PORT
     // @DisplayName: GPS physical COM port
-    // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS
+    // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS,Renamed in 4.6 and later to GPS1_COM_PORT.
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
@@ -2112,7 +2111,7 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _COM_PORT2
     // @DisplayName: GPS physical COM port
-    // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS
+    // @Description: The physical COM port on the connected device, currently only applies to SBF and GSOF GPS.Renamed in 4.6 and later to GPS1_COM_PORT.
     // @Range: 0 10
     // @Increment: 1
     // @User: Advanced
@@ -2126,13 +2125,13 @@ bool AP_GPS::gps_yaw_deg(uint8_t instance, float &yaw_deg, float &accuracy_deg,
 
     // @Param: _CAN_NODEID1
     // @DisplayName: GPS Node ID 1
-    // @Description: GPS Node id for first-discovered GPS.
+    // @Description: GPS Node id for first-discovered GPS.Renamed in 4.6 and later to GPS1_CAN_NODEID.
     // @ReadOnly: True
     // @User: Advanced
 
     // @Param: _CAN_NODEID2
     // @DisplayName: GPS Node ID 2
-    // @Description: GPS Node id for second-discovered GPS.
+    // @Description: GPS Node id for second-discovered GPS.Renamed in 4.6 and later to GPS2_CAN_NODEID.
     // @ReadOnly: True
     // @User: Advanced
 
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index 73bf98ab252974..a3c814ffc241f8 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -52,6 +52,7 @@ class RTCM3_Parser;
 /// GPS driver main class
 class AP_GPS
 {
+    friend class AP_GPS_Blended;
     friend class AP_GPS_ERB;
     friend class AP_GPS_GSOF;
     friend class AP_GPS_MAV;
@@ -512,9 +513,6 @@ class AP_GPS
     // pre-arm check that all GPSs are close to each other.  farthest distance between GPSs (in meters) is returned
     bool all_consistent(float &distance) const;
 
-    // pre-arm check of GPS blending.  False if blending is unhealthy, True if healthy or blending is not being used
-    bool blend_health_check() const;
-
     // handle sending of initialisation strings to the GPS - only used by backends
     void send_blob_start(uint8_t instance);
     void send_blob_start(uint8_t instance, const char *_blob, uint16_t size);
@@ -607,7 +605,7 @@ class AP_GPS
 protected:
 
     // configuration parameters
-    Params params[GPS_MAX_RECEIVERS];
+    Params params[GPS_MAX_INSTANCES];
     AP_Int8 _navfilter;
     AP_Int8 _auto_switch;
     AP_Int16 _sbp_logmask;
@@ -669,7 +667,7 @@ class AP_GPS
     // Note allowance for an additional instance to contain blended data
     GPS_timing timing[GPS_MAX_INSTANCES];
     GPS_State state[GPS_MAX_INSTANCES];
-    AP_GPS_Backend *drivers[GPS_MAX_RECEIVERS];
+    AP_GPS_Backend *drivers[GPS_MAX_INSTANCES];
     AP_HAL::UARTDriver *_port[GPS_MAX_RECEIVERS];
 
     /// primary GPS instance
@@ -757,18 +755,7 @@ class AP_GPS
     void inject_data(uint8_t instance, const uint8_t *data, uint16_t len);
 
 #if AP_GPS_BLENDED_ENABLED
-    // GPS blending and switching
-    Vector3f _blended_antenna_offset; // blended antenna offset
-    float _blended_lag_sec; // blended receiver lag in seconds
-    float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
     bool _output_is_blended; // true when a blended GPS solution being output
-    uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
-
-    // calculate the blend weight.  Returns true if blend could be calculated, false if not
-    bool calc_blend_weights(void);
-
-    // calculate the blended state
-    void calc_blended_state(void);
 #endif
 
     bool should_log() const;
diff --git a/libraries/AP_GPS/AP_GPS_Blended.cpp b/libraries/AP_GPS/AP_GPS_Blended.cpp
index 071b377817cc52..47deea9f4b3292 100644
--- a/libraries/AP_GPS/AP_GPS_Blended.cpp
+++ b/libraries/AP_GPS/AP_GPS_Blended.cpp
@@ -1,22 +1,20 @@
-#include "AP_GPS.h"
+#include "AP_GPS_config.h"
 
 #if AP_GPS_BLENDED_ENABLED
 
+#include "AP_GPS_Blended.h"
+
 // defines used to specify the mask position for use of different accuracy metrics in the blending algorithm
 #define BLEND_MASK_USE_HPOS_ACC     1
 #define BLEND_MASK_USE_VPOS_ACC     2
 #define BLEND_MASK_USE_SPD_ACC      4
 
-// pre-arm check of GPS blending.  True means healthy or that blending is not being used
-bool AP_GPS::blend_health_check() const
-{
-    return (_blend_health_counter < 50);
-}
+#define BLEND_COUNTER_FAILURE_INCREMENT 10
 
 /*
  calculate the weightings used to blend GPSs location and velocity data
 */
-bool AP_GPS::calc_blend_weights(void)
+bool AP_GPS_Blended::_calc_weights(void)
 {
     // zero the blend weights
     memset(&_blend_weights, 0, sizeof(_blend_weights));
@@ -27,7 +25,7 @@ bool AP_GPS::calc_blend_weights(void)
     // The time delta calculations below also rely upon every instance being currently detected and being parsed
 
     // exit immediately if not enough receivers to do blending
-    if (state[0].status <= NO_FIX || state[1].status <= NO_FIX) {
+    if (gps.state[0].status <= AP_GPS::NO_FIX || gps.state[1].status <= AP_GPS::NO_FIX) {
         return false;
     }
 
@@ -37,22 +35,22 @@ bool AP_GPS::calc_blend_weights(void)
     uint32_t max_rate_ms = 0; // largest update interval of a GPS receiver
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         // Find largest and smallest times
-        if (state[i].last_gps_time_ms > max_ms) {
-            max_ms = state[i].last_gps_time_ms;
+        if (gps.state[i].last_gps_time_ms > max_ms) {
+            max_ms = gps.state[i].last_gps_time_ms;
         }
-        if ((state[i].last_gps_time_ms < min_ms) && (state[i].last_gps_time_ms > 0)) {
-            min_ms = state[i].last_gps_time_ms;
+        if ((gps.state[i].last_gps_time_ms < min_ms) && (gps.state[i].last_gps_time_ms > 0)) {
+            min_ms = gps.state[i].last_gps_time_ms;
         }
-        max_rate_ms = MAX(get_rate_ms(i), max_rate_ms);
-        if (isinf(state[i].speed_accuracy) ||
-            isinf(state[i].horizontal_accuracy) ||
-            isinf(state[i].vertical_accuracy)) {
+        max_rate_ms = MAX(gps.get_rate_ms(i), max_rate_ms);
+        if (isinf(gps.state[i].speed_accuracy) ||
+            isinf(gps.state[i].horizontal_accuracy) ||
+            isinf(gps.state[i].vertical_accuracy)) {
             return false;
         }
     }
     if ((max_ms - min_ms) < (2 * max_rate_ms)) {
         // data is not too delayed so use the oldest time_stamp to give a chance for data from that receiver to be updated
-        state[GPS_BLENDED_INSTANCE].last_gps_time_ms = min_ms;
+        state.last_gps_time_ms = min_ms;
     } else {
         // receiver data has timed out so fail out of blending
         return false;
@@ -60,11 +58,11 @@ bool AP_GPS::calc_blend_weights(void)
 
     // calculate the sum squared speed accuracy across all GPS sensors
     float speed_accuracy_sum_sq = 0.0f;
-    if (_blend_mask & BLEND_MASK_USE_SPD_ACC) {
+    if (gps._blend_mask & BLEND_MASK_USE_SPD_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_3D) {
-                if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f) {
-                    speed_accuracy_sum_sq += sq(state[i].speed_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D) {
+                if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f) {
+                    speed_accuracy_sum_sq += sq(gps.state[i].speed_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     speed_accuracy_sum_sq = 0.0f;
@@ -76,11 +74,11 @@ bool AP_GPS::calc_blend_weights(void)
 
     // calculate the sum squared horizontal position accuracy across all GPS sensors
     float horizontal_accuracy_sum_sq = 0.0f;
-    if (_blend_mask & BLEND_MASK_USE_HPOS_ACC) {
+    if (gps._blend_mask & BLEND_MASK_USE_HPOS_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_2D) {
-                if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f) {
-                    horizontal_accuracy_sum_sq += sq(state[i].horizontal_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_2D) {
+                if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f) {
+                    horizontal_accuracy_sum_sq += sq(gps.state[i].horizontal_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     horizontal_accuracy_sum_sq = 0.0f;
@@ -92,11 +90,11 @@ bool AP_GPS::calc_blend_weights(void)
 
     // calculate the sum squared vertical position accuracy across all GPS sensors
     float vertical_accuracy_sum_sq = 0.0f;
-    if (_blend_mask & BLEND_MASK_USE_VPOS_ACC) {
+    if (gps._blend_mask & BLEND_MASK_USE_VPOS_ACC) {
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_3D) {
-                if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f) {
-                    vertical_accuracy_sum_sq += sq(state[i].vertical_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D) {
+                if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f) {
+                    vertical_accuracy_sum_sq += sq(gps.state[i].vertical_accuracy);
                 } else {
                     // not all receivers support this metric so set it to zero and don't use it
                     vertical_accuracy_sum_sq = 0.0f;
@@ -121,8 +119,8 @@ bool AP_GPS::calc_blend_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_hpos_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_2D && state[i].horizontal_accuracy >= 0.001f) {
-                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(state[i].horizontal_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_2D && gps.state[i].horizontal_accuracy >= 0.001f) {
+                hpos_blend_weights[i] = horizontal_accuracy_sum_sq / sq(gps.state[i].horizontal_accuracy);
                 sum_of_hpos_weights += hpos_blend_weights[i];
             }
         }
@@ -141,8 +139,8 @@ bool AP_GPS::calc_blend_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_vpos_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_3D && state[i].vertical_accuracy >= 0.001f) {
-                vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(state[i].vertical_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D && gps.state[i].vertical_accuracy >= 0.001f) {
+                vpos_blend_weights[i] = vertical_accuracy_sum_sq / sq(gps.state[i].vertical_accuracy);
                 sum_of_vpos_weights += vpos_blend_weights[i];
             }
         }
@@ -161,8 +159,8 @@ bool AP_GPS::calc_blend_weights(void)
         // calculate the weights using the inverse of the variances
         float sum_of_spd_weights = 0.0f;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
-            if (state[i].status >= GPS_OK_FIX_3D && state[i].speed_accuracy >= 0.001f) {
-                spd_blend_weights[i] = speed_accuracy_sum_sq / sq(state[i].speed_accuracy);
+            if (gps.state[i].status >= AP_GPS::GPS_OK_FIX_3D && gps.state[i].speed_accuracy >= 0.001f) {
+                spd_blend_weights[i] = speed_accuracy_sum_sq / sq(gps.state[i].speed_accuracy);
                 sum_of_spd_weights += spd_blend_weights[i];
             }
         }
@@ -187,104 +185,116 @@ bool AP_GPS::calc_blend_weights(void)
     return true;
 }
 
+bool AP_GPS_Blended::calc_weights()
+{
+    // adjust blend health counter
+    if (!_calc_weights()) {
+        _blend_health_counter = MIN(_blend_health_counter+BLEND_COUNTER_FAILURE_INCREMENT, 100);
+    } else if (_blend_health_counter > 0) {
+        _blend_health_counter--;
+    }
+    // stop blending if unhealthy
+    return _blend_health_counter < 50;
+}
+
 /*
  calculate a blended GPS state
 */
-void AP_GPS::calc_blended_state(void)
+void AP_GPS_Blended::calc_state(void)
 {
     // initialise the blended states so we can accumulate the results using the weightings for each GPS receiver
-    state[GPS_BLENDED_INSTANCE].instance = GPS_BLENDED_INSTANCE;
-    state[GPS_BLENDED_INSTANCE].status = NO_FIX;
-    state[GPS_BLENDED_INSTANCE].time_week_ms = 0;
-    state[GPS_BLENDED_INSTANCE].time_week = 0;
-    state[GPS_BLENDED_INSTANCE].ground_speed = 0.0f;
-    state[GPS_BLENDED_INSTANCE].ground_course = 0.0f;
-    state[GPS_BLENDED_INSTANCE].hdop = GPS_UNKNOWN_DOP;
-    state[GPS_BLENDED_INSTANCE].vdop = GPS_UNKNOWN_DOP;
-    state[GPS_BLENDED_INSTANCE].num_sats = 0;
-    state[GPS_BLENDED_INSTANCE].velocity.zero();
-    state[GPS_BLENDED_INSTANCE].speed_accuracy = 1e6f;
-    state[GPS_BLENDED_INSTANCE].horizontal_accuracy = 1e6f;
-    state[GPS_BLENDED_INSTANCE].vertical_accuracy = 1e6f;
-    state[GPS_BLENDED_INSTANCE].have_vertical_velocity = false;
-    state[GPS_BLENDED_INSTANCE].have_speed_accuracy = false;
-    state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = false;
-    state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = false;
-    state[GPS_BLENDED_INSTANCE].location = {};
+    state.instance = GPS_BLENDED_INSTANCE;
+    state.status = AP_GPS::NO_FIX;
+    state.time_week_ms = 0;
+    state.time_week = 0;
+    state.ground_speed = 0.0f;
+    state.ground_course = 0.0f;
+    state.hdop = GPS_UNKNOWN_DOP;
+    state.vdop = GPS_UNKNOWN_DOP;
+    state.num_sats = 0;
+    state.velocity.zero();
+    state.speed_accuracy = 1e6f;
+    state.horizontal_accuracy = 1e6f;
+    state.vertical_accuracy = 1e6f;
+    state.have_vertical_velocity = false;
+    state.have_speed_accuracy = false;
+    state.have_horizontal_accuracy = false;
+    state.have_vertical_accuracy = false;
+    state.location = {};
 
     _blended_antenna_offset.zero();
     _blended_lag_sec = 0;
 
 #if HAL_LOGGING_ENABLED
-    const uint32_t last_blended_message_time_ms = timing[GPS_BLENDED_INSTANCE].last_message_time_ms;
+    const uint32_t last_blended_message_time_ms = timing.last_message_time_ms;
 #endif
-    timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = 0;
-    timing[GPS_BLENDED_INSTANCE].last_message_time_ms = 0;
-
-    if (state[0].have_undulation) {
-        state[GPS_BLENDED_INSTANCE].have_undulation = true;
-        state[GPS_BLENDED_INSTANCE].undulation = state[0].undulation;
-    } else if (state[1].have_undulation) {
-        state[GPS_BLENDED_INSTANCE].have_undulation = true;
-        state[GPS_BLENDED_INSTANCE].undulation = state[1].undulation;
+    timing.last_fix_time_ms = 0;
+    timing.last_message_time_ms = 0;
+
+    if (gps.state[0].have_undulation) {
+        state.have_undulation = true;
+        state.undulation = gps.state[0].undulation;
+    } else if (gps.state[1].have_undulation) {
+        state.have_undulation = true;
+        state.undulation = gps.state[1].undulation;
     } else {
-        state[GPS_BLENDED_INSTANCE].have_undulation = false;
+        state.have_undulation = false;
     }
 
     // combine the states into a blended solution
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         // use the highest status
-        if (state[i].status > state[GPS_BLENDED_INSTANCE].status) {
-            state[GPS_BLENDED_INSTANCE].status = state[i].status;
+        if (gps.state[i].status > state.status) {
+            state.status = gps.state[i].status;
         }
 
         // calculate a blended average velocity
-        state[GPS_BLENDED_INSTANCE].velocity += state[i].velocity * _blend_weights[i];
+        state.velocity += gps.state[i].velocity * _blend_weights[i];
 
         // report the best valid accuracies and DOP metrics
 
-        if (state[i].have_horizontal_accuracy && state[i].horizontal_accuracy > 0.0f && state[i].horizontal_accuracy < state[GPS_BLENDED_INSTANCE].horizontal_accuracy) {
-            state[GPS_BLENDED_INSTANCE].have_horizontal_accuracy = true;
-            state[GPS_BLENDED_INSTANCE].horizontal_accuracy = state[i].horizontal_accuracy;
+        if (gps.state[i].have_horizontal_accuracy && gps.state[i].horizontal_accuracy > 0.0f && gps.state[i].horizontal_accuracy < state.horizontal_accuracy) {
+            state.have_horizontal_accuracy = true;
+            state.horizontal_accuracy = gps.state[i].horizontal_accuracy;
         }
 
-        if (state[i].have_vertical_accuracy && state[i].vertical_accuracy > 0.0f && state[i].vertical_accuracy < state[GPS_BLENDED_INSTANCE].vertical_accuracy) {
-            state[GPS_BLENDED_INSTANCE].have_vertical_accuracy = true;
-            state[GPS_BLENDED_INSTANCE].vertical_accuracy = state[i].vertical_accuracy;
+        if (gps.state[i].have_vertical_accuracy && gps.state[i].vertical_accuracy > 0.0f && gps.state[i].vertical_accuracy < state.vertical_accuracy) {
+            state.have_vertical_accuracy = true;
+            state.vertical_accuracy = gps.state[i].vertical_accuracy;
         }
 
-        if (state[i].have_vertical_velocity) {
-            state[GPS_BLENDED_INSTANCE].have_vertical_velocity = true;
+        if (gps.state[i].have_vertical_velocity) {
+            state.have_vertical_velocity = true;
         }
 
-        if (state[i].have_speed_accuracy && state[i].speed_accuracy > 0.0f && state[i].speed_accuracy < state[GPS_BLENDED_INSTANCE].speed_accuracy) {
-            state[GPS_BLENDED_INSTANCE].have_speed_accuracy = true;
-            state[GPS_BLENDED_INSTANCE].speed_accuracy = state[i].speed_accuracy;
+        if (gps.state[i].have_speed_accuracy && gps.state[i].speed_accuracy > 0.0f && gps.state[i].speed_accuracy < state.speed_accuracy) {
+            state.have_speed_accuracy = true;
+            state.speed_accuracy = gps.state[i].speed_accuracy;
         }
 
-        if (state[i].hdop > 0 && state[i].hdop < state[GPS_BLENDED_INSTANCE].hdop) {
-            state[GPS_BLENDED_INSTANCE].hdop = state[i].hdop;
+        if (gps.state[i].hdop > 0 && gps.state[i].hdop < state.hdop) {
+            state.hdop = gps.state[i].hdop;
         }
 
-        if (state[i].vdop > 0 && state[i].vdop < state[GPS_BLENDED_INSTANCE].vdop) {
-            state[GPS_BLENDED_INSTANCE].vdop = state[i].vdop;
+        if (gps.state[i].vdop > 0 && gps.state[i].vdop < state.vdop) {
+            state.vdop = gps.state[i].vdop;
         }
 
-        if (state[i].num_sats > 0 && state[i].num_sats > state[GPS_BLENDED_INSTANCE].num_sats) {
-            state[GPS_BLENDED_INSTANCE].num_sats = state[i].num_sats;
+        if (gps.state[i].num_sats > 0 && gps.state[i].num_sats > state.num_sats) {
+            state.num_sats = gps.state[i].num_sats;
         }
 
         // report a blended average GPS antenna position
-        Vector3f temp_antenna_offset = params[i].antenna_offset;
+        Vector3f temp_antenna_offset = gps.params[i].antenna_offset;
         temp_antenna_offset *= _blend_weights[i];
         _blended_antenna_offset += temp_antenna_offset;
 
         // blend the timing data
-        if (timing[i].last_fix_time_ms > timing[GPS_BLENDED_INSTANCE].last_fix_time_ms) {
-            timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = timing[i].last_fix_time_ms;
+        if (gps.timing[i].last_fix_time_ms > timing.last_fix_time_ms) {
+            timing.last_fix_time_ms = gps.timing[i].last_fix_time_ms;
         }
-        if (timing[i].last_message_time_ms > timing[GPS_BLENDED_INSTANCE].last_message_time_ms) {
-            timing[GPS_BLENDED_INSTANCE].last_message_time_ms = timing[i].last_message_time_ms;
+        if (gps.timing[i].last_message_time_ms > timing.last_message_time_ms) {
+            timing.last_message_time_ms = gps.timing[i].last_message_time_ms;
         }
     }
 
@@ -300,7 +310,7 @@ void AP_GPS::calc_blended_state(void)
         if (_blend_weights[i] > best_weight) {
             best_weight = _blend_weights[i];
             best_index = i;
-            state[GPS_BLENDED_INSTANCE].location = state[i].location;
+            state.location = gps.state[i].location;
         }
     }
 
@@ -310,18 +320,18 @@ void AP_GPS::calc_blended_state(void)
     blended_NE_offset_m.zero();
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         if (_blend_weights[i] > 0.0f && i != best_index) {
-            blended_NE_offset_m += state[GPS_BLENDED_INSTANCE].location.get_distance_NE(state[i].location) * _blend_weights[i];
-            blended_alt_offset_cm += (float)(state[i].location.alt - state[GPS_BLENDED_INSTANCE].location.alt) * _blend_weights[i];
+            blended_NE_offset_m += state.location.get_distance_NE(gps.state[i].location) * _blend_weights[i];
+            blended_alt_offset_cm += (float)(gps.state[i].location.alt - state.location.alt) * _blend_weights[i];
         }
     }
 
     // Add the sum of weighted offsets to the reference location to obtain the blended location
-    state[GPS_BLENDED_INSTANCE].location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y);
-    state[GPS_BLENDED_INSTANCE].location.alt += (int)blended_alt_offset_cm;
+    state.location.offset(blended_NE_offset_m.x, blended_NE_offset_m.y);
+    state.location.alt += (int)blended_alt_offset_cm;
 
     // Calculate ground speed and course from blended velocity vector
-    state[GPS_BLENDED_INSTANCE].ground_speed = state[GPS_BLENDED_INSTANCE].velocity.xy().length();
-    state[GPS_BLENDED_INSTANCE].ground_course = wrap_360(degrees(atan2f(state[GPS_BLENDED_INSTANCE].velocity.y, state[GPS_BLENDED_INSTANCE].velocity.x)));
+    state.ground_speed = state.velocity.xy().length();
+    state.ground_course = wrap_360(degrees(atan2f(state.velocity.y, state.velocity.x)));
 
     // If the GPS week is the same then use a blended time_week_ms
     // If week is different, then use time stamp from GPS with largest weighting
@@ -331,8 +341,8 @@ void AP_GPS::calc_blended_state(void)
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         if (last_week_instance == 0 && _blend_weights[i] > 0) {
             // this is our first valid sensor week data
-            last_week_instance = state[i].time_week;
-        } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != state[i].time_week) {
+            last_week_instance = gps.state[i].time_week;
+        } else if (last_week_instance != 0 && _blend_weights[i] > 0 && last_week_instance != gps.state[i].time_week) {
             // there is valid sensor week data that is inconsistent
             weeks_consistent = false;
         }
@@ -340,19 +350,19 @@ void AP_GPS::calc_blended_state(void)
     // calculate output
     if (!weeks_consistent) {
         // use data from highest weighted sensor
-        state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
-        state[GPS_BLENDED_INSTANCE].time_week_ms = state[best_index].time_week_ms;
+        state.time_week = gps.state[best_index].time_week;
+        state.time_week_ms = gps.state[best_index].time_week_ms;
     } else {
         // use week number from highest weighting GPS (they should all have the same week number)
-        state[GPS_BLENDED_INSTANCE].time_week = state[best_index].time_week;
+        state.time_week = gps.state[best_index].time_week;
         // calculate a blended value for the number of ms lapsed in the week
         double temp_time_0 = 0.0;
         for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
             if (_blend_weights[i] > 0.0f) {
-                temp_time_0 += (double)state[i].time_week_ms * (double)_blend_weights[i];
+                temp_time_0 += (double)gps.state[i].time_week_ms * (double)_blend_weights[i];
             }
         }
-        state[GPS_BLENDED_INSTANCE].time_week_ms = (uint32_t)temp_time_0;
+        state.time_week_ms = (uint32_t)temp_time_0;
     }
 
     // calculate a blended value for the timing data and lag
@@ -360,21 +370,30 @@ void AP_GPS::calc_blended_state(void)
     double temp_time_2 = 0.0;
     for (uint8_t i=0; i<GPS_MAX_RECEIVERS; i++) {
         if (_blend_weights[i] > 0.0f) {
-            temp_time_1 += (double)timing[i].last_fix_time_ms * (double) _blend_weights[i];
-            temp_time_2 += (double)timing[i].last_message_time_ms * (double)_blend_weights[i];
+            temp_time_1 += (double)gps.timing[i].last_fix_time_ms * (double) _blend_weights[i];
+            temp_time_2 += (double)gps.timing[i].last_message_time_ms * (double)_blend_weights[i];
             float gps_lag_sec = 0;
-            get_lag(i, gps_lag_sec);
+            gps.get_lag(i, gps_lag_sec);
             _blended_lag_sec += gps_lag_sec * _blend_weights[i];
         }
     }
-    timing[GPS_BLENDED_INSTANCE].last_fix_time_ms = (uint32_t)temp_time_1;
-    timing[GPS_BLENDED_INSTANCE].last_message_time_ms = (uint32_t)temp_time_2;
+    timing.last_fix_time_ms = (uint32_t)temp_time_1;
+    timing.last_message_time_ms = (uint32_t)temp_time_2;
 
 #if HAL_LOGGING_ENABLED
-    if (timing[GPS_BLENDED_INSTANCE].last_message_time_ms > last_blended_message_time_ms &&
+    if (timing.last_message_time_ms > last_blended_message_time_ms &&
         should_log()) {
-        Write_GPS(GPS_BLENDED_INSTANCE);
+        gps.Write_GPS(GPS_BLENDED_INSTANCE);
     }
 #endif
 }
+
+bool AP_GPS_Blended::get_lag(float &lag_sec) const
+{
+        lag_sec = _blended_lag_sec;
+        // auto switching uses all GPS receivers, so all must be configured
+        uint8_t inst; // we don't actually care what the number is, but must pass it
+        return gps.first_unconfigured_gps(inst);
+}
+
 #endif  // AP_GPS_BLENDED_ENABLED
diff --git a/libraries/AP_GPS/AP_GPS_Blended.h b/libraries/AP_GPS/AP_GPS_Blended.h
new file mode 100644
index 00000000000000..b68eb0f9660ed7
--- /dev/null
+++ b/libraries/AP_GPS/AP_GPS_Blended.h
@@ -0,0 +1,74 @@
+#pragma once
+
+/*
+   This program is free software: you can redistribute it and/or modify
+   it under the terms of the GNU General Public License as published by
+   the Free Software Foundation, either version 3 of the License, or
+   (at your option) any later version.
+
+   This program is distributed in the hope that it will be useful,
+   but WITHOUT ANY WARRANTY; without even the implied warranty of
+   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+   GNU General Public License for more details.
+
+   You should have received a copy of the GNU General Public License
+   along with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+#pragma once
+
+#include "AP_GPS_config.h"
+
+#if AP_GPS_BLENDED_ENABLED
+
+#include "AP_GPS.h"
+#include "GPS_Backend.h"
+
+class AP_GPS_Blended : public AP_GPS_Backend
+{
+public:
+
+    AP_GPS_Blended(AP_GPS &_gps, AP_GPS::Params &_params, AP_GPS::GPS_State &_state, class AP_GPS::GPS_timing &_timing) :
+        AP_GPS_Backend(_gps, _params, _state, nullptr),
+        timing{_timing}
+        { }
+
+    // pre-arm check of GPS blending.  False if blending is unhealthy,
+    // True if healthy or blending is not being used
+    bool is_healthy() const override {
+        return (_blend_health_counter < 50);
+    }
+
+    bool read() override { return true; }
+
+    const char *name() const override { return "Blended"; }
+
+    bool get_lag(float &lag_sec) const override;
+    const Vector3f &get_antenna_offset() const {
+        return _blended_antenna_offset;
+    }
+
+    // calculate the blend weight.  Returns true if blend could be
+    // calculated, false if not
+    bool calc_weights(void);
+    // calculate the blended state
+    void calc_state(void);
+
+    void zero_health_counter() {
+        _blend_health_counter = 0;
+    }
+
+private:
+
+    // GPS blending and switching
+    Vector3f _blended_antenna_offset; // blended antenna offset
+    float _blended_lag_sec; // blended receiver lag in seconds
+    float _blend_weights[GPS_MAX_RECEIVERS]; // blend weight for each GPS. The blend weights must sum to 1.0 across all instances.
+    bool _output_is_blended; // true when a blended GPS solution being output
+    uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
+
+    AP_GPS::GPS_timing &timing;
+    bool _calc_weights(void);
+};
+
+#endif  // AP_GPS_BLENDED_ENABLED
diff --git a/libraries/AP_GPS/AP_GPS_config.h b/libraries/AP_GPS/AP_GPS_config.h
index 9aab43a02b88b2..6b114f156a15b5 100644
--- a/libraries/AP_GPS/AP_GPS_config.h
+++ b/libraries/AP_GPS/AP_GPS_config.h
@@ -15,6 +15,7 @@
 #ifndef GPS_MAX_RECEIVERS
 #define GPS_MAX_RECEIVERS 2 // maximum number of physical GPS sensors allowed - does not include virtual GPS created by blending receiver data
 #endif
+
 #if !defined(GPS_MAX_INSTANCES)
 #if GPS_MAX_RECEIVERS > 1
 #define GPS_MAX_INSTANCES  (GPS_MAX_RECEIVERS + 1) // maximum number of GPS instances including the 'virtual' GPS created by blending receiver data
@@ -26,18 +27,21 @@
 #if GPS_MAX_RECEIVERS <= 1 && GPS_MAX_INSTANCES > 1
 #error "GPS_MAX_INSTANCES should be 1 for GPS_MAX_RECEIVERS <= 1"
 #endif
-
-#if GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
-#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS  // the virtual blended GPS is always the highest instance (2)
-#endif
 #endif
 
 #ifndef AP_GPS_BACKEND_DEFAULT_ENABLED
 #define AP_GPS_BACKEND_DEFAULT_ENABLED AP_GPS_ENABLED
 #endif
 
+#if !defined(AP_GPS_BLENDED_ENABLED) && defined(GPS_MAX_INSTANCES)
+#define AP_GPS_BLENDED_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && GPS_MAX_INSTANCES > GPS_MAX_RECEIVERS
+#if AP_GPS_BLENDED_ENABLED
+#define GPS_BLENDED_INSTANCE GPS_MAX_RECEIVERS  // the virtual blended GPS is always the highest instance (2)
+#endif
+#endif
+
 #ifndef AP_GPS_BLENDED_ENABLED
-#define AP_GPS_BLENDED_ENABLED AP_GPS_BACKEND_DEFAULT_ENABLED && defined(GPS_BLENDED_INSTANCE)
+#define AP_GPS_BLENDED_ENABLED 0
 #endif
 
 #ifndef AP_GPS_DRONECAN_ENABLED
diff --git a/libraries/AP_GPS/MovingBase.cpp b/libraries/AP_GPS/MovingBase.cpp
index 3ce55c6d82ad7d..9a2143bfff952c 100644
--- a/libraries/AP_GPS/MovingBase.cpp
+++ b/libraries/AP_GPS/MovingBase.cpp
@@ -3,7 +3,7 @@
 const AP_Param::GroupInfo MovingBase::var_info[] = {
     // @Param: TYPE
     // @DisplayName: Moving base type
-    // @Description: Controls the type of moving base used if using moving base.
+    // @Description: Controls the type of moving base used if using moving base.This is renamed in 4.6 and later to GPSx_MB_TYPE.
     // @Values: 0:Relative to alternate GPS instance,1:RelativeToCustomBase
     // @User: Advanced
     // @RebootRequired: True
@@ -11,7 +11,7 @@ const AP_Param::GroupInfo MovingBase::var_info[] = {
 
     // @Param: OFS_X
     // @DisplayName: Base antenna X position offset
-    // @Description: X position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive X is forward of the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: X position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive X is forward of the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.This is renamed in 4.6 and later to GPSx_MB_OFS_X.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -19,7 +19,7 @@ const AP_Param::GroupInfo MovingBase::var_info[] = {
     
     // @Param: OFS_Y
     // @DisplayName: Base antenna Y position offset    
-    // @Description: Y position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive Y is to the right of the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Y position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive Y is to the right of the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.This is renamed in 4.6 and later to GPSx_MB_OFS_Y.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
@@ -27,7 +27,7 @@ const AP_Param::GroupInfo MovingBase::var_info[] = {
     
     // @Param: OFS_Z
     // @DisplayName: Base antenna Z position offset
-    // @Description: Z position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive Z is down from the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.
+    // @Description: Z position of the base (primary) GPS antenna in body frame from the position of the 2nd antenna. Positive Z is down from the 2nd antenna. Use antenna phase centroid location if provided by the manufacturer.This is renamed in 4.6 and later to GPSx_MB_OFS_Z.
     // @Units: m
     // @Range: -5 5
     // @Increment: 0.01
diff --git a/libraries/AP_GSOF/AP_GSOF.cpp b/libraries/AP_GSOF/AP_GSOF.cpp
index 92353e6502b7bc..f60fb37207e7d3 100644
--- a/libraries/AP_GSOF/AP_GSOF.cpp
+++ b/libraries/AP_GSOF/AP_GSOF.cpp
@@ -83,56 +83,6 @@ AP_GSOF::parse(const uint8_t temp, const uint8_t n_expected)
     return NO_GSOF_DATA;
 }
 
-double
-AP_GSOF::SwapDouble(const uint8_t* src, const uint32_t pos) const
-{
-    union {
-        double d;
-        char bytes[sizeof(double)];
-    } doubleu;
-    doubleu.bytes[0] = src[pos + 7];
-    doubleu.bytes[1] = src[pos + 6];
-    doubleu.bytes[2] = src[pos + 5];
-    doubleu.bytes[3] = src[pos + 4];
-    doubleu.bytes[4] = src[pos + 3];
-    doubleu.bytes[5] = src[pos + 2];
-    doubleu.bytes[6] = src[pos + 1];
-    doubleu.bytes[7] = src[pos + 0];
-
-    return doubleu.d;
-}
-
-float
-AP_GSOF::SwapFloat(const uint8_t* src, const uint32_t pos) const
-{
-    union {
-        float f;
-        char bytes[sizeof(float)];
-    } floatu;
-    floatu.bytes[0] = src[pos + 3];
-    floatu.bytes[1] = src[pos + 2];
-    floatu.bytes[2] = src[pos + 1];
-    floatu.bytes[3] = src[pos + 0];
-
-    return floatu.f;
-}
-
-uint32_t
-AP_GSOF::SwapUint32(const uint8_t* src, const uint32_t pos) const
-{
-    uint32_t u;
-    memcpy(&u, &src[pos], sizeof(u));
-    return be32toh(u);
-}
-
-uint16_t
-AP_GSOF::SwapUint16(const uint8_t* src, const uint32_t pos) const
-{
-    uint16_t u;
-    memcpy(&u, &src[pos], sizeof(u));
-    return be16toh(u);
-}
-
 int
 AP_GSOF::process_message(void)
 {
@@ -196,8 +146,8 @@ AP_GSOF::process_message(void)
 void AP_GSOF::parse_pos_time(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_TIME.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____25
-    pos_time.time_week_ms = SwapUint32(msg.data, a);
-    pos_time.time_week = SwapUint16(msg.data, a + 4);
+    pos_time.time_week_ms = be32toh_ptr(msg.data + a);
+    pos_time.time_week = be32toh_ptr(msg.data + a + 4);
     pos_time.num_sats = msg.data[a + 6];
     pos_time.pos_flags1 = msg.data[a + 7];
     pos_time.pos_flags2 = msg.data[a + 8];
@@ -206,9 +156,9 @@ void AP_GSOF::parse_pos_time(uint32_t a)
 void AP_GSOF::parse_pos(uint32_t a)
 {
     // This packet is not documented in Trimble's receiver help as of May 18, 2023
-    position.latitude_rad = SwapDouble(msg.data, a);
-    position.longitude_rad = SwapDouble(msg.data, a + 8);
-    position.altitude = SwapDouble(msg.data, a + 16);
+    position.latitude_rad = be64todouble_ptr(msg.data, a);
+    position.longitude_rad = be64todouble_ptr(msg.data, a + 8);
+    position.altitude = be64todouble_ptr(msg.data, a + 16);
 }
 
 void AP_GSOF::parse_vel(uint32_t a)
@@ -218,13 +168,13 @@ void AP_GSOF::parse_vel(uint32_t a)
 
     constexpr uint8_t BIT_VELOCITY_VALID = 0;
     if (BIT_IS_SET(vel.velocity_flags, BIT_VELOCITY_VALID)) {
-        vel.horizontal_velocity = SwapFloat(msg.data, a + 1);
-        vel.vertical_velocity = SwapFloat(msg.data, a + 9);
+        vel.horizontal_velocity = be32tofloat_ptr(msg.data, a + 1);
+        vel.vertical_velocity = be32tofloat_ptr(msg.data, a + 9);
     }
 
     constexpr uint8_t BIT_HEADING_VALID = 2;
     if (BIT_IS_SET(vel.velocity_flags, BIT_HEADING_VALID)) {
-        vel.heading = SwapFloat(msg.data, a + 5);
+        vel.heading = be32tofloat_ptr(msg.data, a + 5);
     }
 }
 
@@ -232,16 +182,16 @@ void AP_GSOF::parse_dop(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_PDOP.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____12
     // Skip pdop.
-    dop.hdop = SwapFloat(msg.data, a + 4);
+    dop.hdop = be32tofloat_ptr(msg.data, a + 4);
 }
 
 void AP_GSOF::parse_pos_sigma(uint32_t a)
 {
     // https://receiverhelp.trimble.com/oem-gnss/index.html#GSOFmessages_SIGMA.html?TocPath=Output%2520Messages%257CGSOF%2520Messages%257C_____24
     // Skip pos_rms
-    pos_sigma.sigma_east = SwapFloat(msg.data, a + 4);
-    pos_sigma.sigma_north = SwapFloat(msg.data, a + 8);
-    pos_sigma.sigma_up = SwapFloat(msg.data, a + 16);
+    pos_sigma.sigma_east = be32tofloat_ptr(msg.data, a + 4);
+    pos_sigma.sigma_north = be32tofloat_ptr(msg.data, a + 8);
+    pos_sigma.sigma_up = be32tofloat_ptr(msg.data, a + 16);
 }
 #endif // AP_GSOF_ENABLED
 
diff --git a/libraries/AP_GSOF/AP_GSOF.h b/libraries/AP_GSOF/AP_GSOF.h
index 3573d270effeb5..5fb78fd4d4234f 100644
--- a/libraries/AP_GSOF/AP_GSOF.h
+++ b/libraries/AP_GSOF/AP_GSOF.h
@@ -94,10 +94,6 @@ class AP_GSOF
     // Parses current data and returns the number of parsed GSOF messages.
     int process_message() WARN_IF_UNUSED;
 
-    double SwapDouble(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    float SwapFloat(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    uint32_t SwapUint32(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
-    uint16_t SwapUint16(const uint8_t* src, const uint32_t pos) const WARN_IF_UNUSED;
     void parse_pos_time(uint32_t a);
     void parse_pos(uint32_t a);
     void parse_vel(uint32_t a);
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat
index 83e9d53d2fcf35..183c98b57b6a7b 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/CubePilot-PPPGW/hwdef-bl.dat
@@ -15,6 +15,3 @@ define BOARD_PHY_ID  MII_LAN8720_ID
 define BOARD_PHY_RMII
 
 include ../include/network_bootloader.inc
-
-# allow USB bootloader too
-SERIAL_ORDER OTG1
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat b/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat
index 121418d3624d18..bf0ef811ae8c42 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat
+++ b/libraries/AP_HAL_ChibiOS/hwdef/RadiolinkPIX6/hwdef.dat
@@ -171,9 +171,9 @@ SPIDEV osd            SPI2 DEVID2  AT7456_CS    MODE0  10*MHZ 10*MHZ
 SPIDEV ramtron        SPI2 DEVID1  FRAM_CS      MODE3  8*MHZ  8*MHZ 
 
 # up to 2 IMUs
-IMU Invensensev3 SPI:imu2 ROTATION_YAW_90
+IMU Invensensev3 SPI:imu2 ROTATION_PITCH_180_YAW_270
 
-IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_NONE
+IMU BMI088 SPI:bmi088_a SPI:bmi088_g ROTATION_PITCH_180
 
 define HAL_DEFAULT_INS_FAST_SAMPLE 1
 
@@ -183,7 +183,7 @@ define HAL_PROBE_EXTERNAL_I2C_COMPASSES
 
 # we need to stop the probe of an IST8310 as an internal compass with PITCH_180
 define HAL_COMPASS_DISABLE_IST8310_INTERNAL_PROBE
-COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_ROLL_180
+COMPASS IST8310 I2C:ALL_INTERNAL:0x0E false ROTATION_YAW_180
 
 # one baro
 BARO SPx06     I2C:0:0x76
diff --git a/libraries/AP_ICEngine/AP_ICEngine.cpp b/libraries/AP_ICEngine/AP_ICEngine.cpp
index 12ec2b02dd1132..62f92102098f61 100644
--- a/libraries/AP_ICEngine/AP_ICEngine.cpp
+++ b/libraries/AP_ICEngine/AP_ICEngine.cpp
@@ -46,7 +46,8 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
     // @Description: This is an RC input channel for requesting engine start. Engine will try to start when channel is at or above 1700. Engine will stop when channel is at or below 1300. Between 1301 and 1699 the engine will not change state unless a MAVLink command or mission item commands a state change, or the vehicle is disarmed. See ICE_STARTCHN_MIN parameter to change engine stop PWM value and/or to enable debouncing of the START_CH to avoid accidental engine kills due to noise on channel.
     // @User: Standard
     // @Values: 0:None,1:Chan1,2:Chan2,3:Chan3,4:Chan4,5:Chan5,6:Chan6,7:Chan7,8:Chan8,9:Chan9,10:Chan10,11:Chan11,12:Chan12,13:Chan13,14:Chan14,15:Chan15,16:Chan16
-    AP_GROUPINFO("START_CHAN", 1, AP_ICEngine, start_chan, 0),
+
+    // 1 was START_CHAN
 
     // @Param: STARTER_TIME
     // @DisplayName: Time to run starter
@@ -78,28 +79,32 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
     // @Description: This is the value sent to the ignition channel when on
     // @User: Standard
     // @Range: 1000 2000
-    AP_GROUPINFO("PWM_IGN_ON", 5, AP_ICEngine, pwm_ignition_on, 2000),
+
+    // 5 was PWM_IGN_ON
 
     // @Param: PWM_IGN_OFF
     // @DisplayName: PWM value for ignition off
     // @Description: This is the value sent to the ignition channel when off
     // @User: Standard
     // @Range: 1000 2000
-    AP_GROUPINFO("PWM_IGN_OFF", 6, AP_ICEngine, pwm_ignition_off, 1000),
+
+    // 6 was PWM_IGN_OFF
 
     // @Param: PWM_STRT_ON
     // @DisplayName: PWM value for starter on
     // @Description: This is the value sent to the starter channel when on
     // @User: Standard
     // @Range: 1000 2000
-    AP_GROUPINFO("PWM_STRT_ON", 7, AP_ICEngine, pwm_starter_on, 2000),
+
+    // 7 was PWM_STRT_ON
 
     // @Param: PWM_STRT_OFF
     // @DisplayName: PWM value for starter off
     // @Description: This is the value sent to the starter channel when off
     // @User: Standard
     // @Range: 1000 2000
-    AP_GROUPINFO("PWM_STRT_OFF", 8, AP_ICEngine, pwm_starter_off, 1000),
+
+    // 8 was PWM_STRT_OFF
 
 #if AP_RPM_ENABLED
     // @Param: RPM_CHAN
@@ -167,6 +172,9 @@ const AP_Param::GroupInfo AP_ICEngine::var_info[] = {
 
     // 18 was IGNITION_RLY
 
+    // Hidden param used as a flag for param conversion
+    // This allows one time conversion while allowing user to flash between versions with and without converted params
+    AP_GROUPINFO_FLAGS("FMT_VER", 19, AP_ICEngine, param_format_version, 0, AP_PARAM_FLAG_HIDDEN),
 
     AP_GROUPEND
 };
@@ -187,6 +195,81 @@ AP_ICEngine::AP_ICEngine()
 #endif
 }
 
+// One time init call
+void AP_ICEngine::init()
+{
+    // Configure starter and ignition outputs as range type
+    SRV_Channels::set_range(SRV_Channel::k_starter, 1);
+    SRV_Channels::set_range(SRV_Channel::k_ignition, 1);
+
+    // Set default PWM endpoints to 1000 to 2000
+    SRV_Channels::set_output_min_max_defaults(SRV_Channel::k_starter, 1000, 2000);
+    SRV_Channels::set_output_min_max_defaults(SRV_Channel::k_ignition, 1000, 2000);
+
+    // Convert params
+    param_conversion();
+}
+
+// PARAMETER_CONVERSION - Added: Aug 2024
+void AP_ICEngine::param_conversion()
+{
+    if (!enable || (param_format_version == 1)) {
+        // not enabled or conversion has already been done
+        return;
+    }
+
+    // Set format version so the conversion is not done again
+    param_format_version.set_and_save(1);
+
+    AP_Param::ConversionInfo info;
+    if (!AP_Param::find_key_by_pointer(this, info.old_key)) {
+        return;
+    }
+
+    // Conversion table giving the old on and off pwm parameter indexes and the function for both starter and ignition
+    const struct convert_table {
+        uint32_t element[2];
+        SRV_Channel::Aux_servo_function_t fuction;
+    } conversion_table[] = {
+        { {450, 514}, SRV_Channel::k_starter },  // PWM_STRT_ON, PWM_STRT_OFF
+        { {322, 386}, SRV_Channel::k_ignition }, // PWM_IGN_ON, PWM_IGN_OFF
+    };
+
+    // All PWM values were int16
+    info.type = AP_PARAM_INT16;
+
+    for (const auto & elem : conversion_table) {
+        // Use the original default values if params are not saved
+        uint16_t pwm_on = 2000;
+        uint16_t pwm_off = 1000;
+
+        // Get param values if configured
+        AP_Int16 param_value;
+        info.old_group_element = elem.element[0];
+        if (AP_Param::find_old_parameter(&info, &param_value)) {
+            pwm_on = param_value;
+        }
+        info.old_group_element = elem.element[1];
+        if (AP_Param::find_old_parameter(&info, &param_value)) {
+            pwm_off = param_value;
+        }
+
+        // Save as servo endpoints, note that save_output_min_max will set reversed if needed
+        SRV_Channels::save_output_min_max(elem.fuction, pwm_off, pwm_on);
+    }
+
+    // Convert to new RC option
+    AP_Int8 start_chan;
+    info.type = AP_PARAM_INT8;
+    info.old_group_element = 66;
+    if (AP_Param::find_old_parameter(&info, &start_chan) && (start_chan > 0)) {
+        RC_Channel *chan = rc().channel(start_chan-1);
+        if (chan != nullptr) {
+            chan->option.set_and_save((int16_t)RC_Channel::AUX_FUNC::ICE_START_STOP);
+        }
+    }
+}
+
 /*
   update engine state
  */
@@ -197,7 +280,7 @@ void AP_ICEngine::update(void)
     }
 
     uint16_t cvalue = 1500;
-    RC_Channel *c = rc().channel(start_chan-1);
+    RC_Channel *c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ICE_START_STOP);
     if (c != nullptr && rc().has_valid_input()) {
         // get starter control channel
         cvalue = c->get_radio_in();
@@ -508,7 +591,7 @@ bool AP_ICEngine::engine_control(float start_control, float cold_start, float he
         GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Engine: already running");
         return false;
     }
-    RC_Channel *c = rc().channel(start_chan-1);
+    RC_Channel *c = rc().find_channel_for_option(RC_Channel::AUX_FUNC::ICE_START_STOP);
     if (c != nullptr && rc().has_valid_input()) {
         // get starter control channel
         uint16_t cvalue = c->get_radio_in();
@@ -608,7 +691,7 @@ void AP_ICEngine::update_idle_governor(int8_t &min_throttle)
  */
 void AP_ICEngine::set_ignition(bool on)
 {
-    SRV_Channels::set_output_pwm(SRV_Channel::k_ignition, on? pwm_ignition_on : pwm_ignition_off);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_ignition, on ? 1.0 : 0.0);
 
 #if AP_RELAY_ENABLED
     AP_Relay *relay = AP::relay();
@@ -624,7 +707,7 @@ void AP_ICEngine::set_ignition(bool on)
  */
 void AP_ICEngine::set_starter(bool on)
 {
-    SRV_Channels::set_output_pwm(SRV_Channel::k_starter, on? pwm_starter_on : pwm_starter_off);
+    SRV_Channels::set_output_scaled(SRV_Channel::k_starter, on ? 1.0 : 0.0);
 
 #if AP_ICENGINE_TCA9554_STARTER_ENABLED
     tca9554_starter.set_starter(on, option_set(Options::CRANK_DIR_REVERSE));
diff --git a/libraries/AP_ICEngine/AP_ICEngine.h b/libraries/AP_ICEngine/AP_ICEngine.h
index 419b6a62bc70fa..159ee99bb9058a 100644
--- a/libraries/AP_ICEngine/AP_ICEngine.h
+++ b/libraries/AP_ICEngine/AP_ICEngine.h
@@ -39,6 +39,9 @@ class AP_ICEngine {
 
     static const struct AP_Param::GroupInfo var_info[];
 
+    // One time init call
+    void init();
+
     // update engine state. Should be called at 10Hz or more
     void update(void);
 
@@ -90,9 +93,6 @@ class AP_ICEngine {
     // enable library
     AP_Int8 enable;
 
-    // channel for pilot to command engine start, 0 for none
-    AP_Int8 start_chan;
-
     // min pwm on start channel for engine stop
     AP_Int16 start_chan_min_pwm;
     
@@ -106,13 +106,7 @@ class AP_ICEngine {
 
     // delay between start attempts (seconds)
     AP_Float starter_delay;
-    
-    // pwm values 
-    AP_Int16 pwm_ignition_on;
-    AP_Int16 pwm_ignition_off;
-    AP_Int16 pwm_starter_on;
-    AP_Int16 pwm_starter_off;
-    
+
 #if AP_RPM_ENABLED
     // RPM above which engine is considered to be running
     AP_Int32 rpm_threshold;
@@ -185,6 +179,10 @@ class AP_ICEngine {
         float throttle_percentage;
     } redline;
 #endif
+
+    // Param conversion function and flag
+    void param_conversion();
+    AP_Int8 param_format_version;
 };
 
 
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
index bef05d69ab01cb..944b20661a1315 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.cpp
@@ -781,6 +781,14 @@ void AP_InertialSensor_Backend::update_gyro(uint8_t instance) /* front end */
         _imu._new_gyro_data[instance] = false;
     }
 
+    update_gyro_filters(instance);
+}
+
+/*
+  propagate filter changes from front end to backend
+ */
+void AP_InertialSensor_Backend::update_gyro_filters(uint8_t instance) /* front end */
+{
     // possibly update filter frequency
     const float gyro_rate = _gyro_raw_sample_rate(instance);
 
@@ -815,7 +823,16 @@ void AP_InertialSensor_Backend::update_accel(uint8_t instance) /* front end */
         _publish_accel(instance, _imu._accel_filtered[instance]);
         _imu._new_accel_data[instance] = false;
     }
-    
+
+    update_accel_filters(instance);
+}
+
+
+/*
+  propagate filter changes from front end to backend
+ */
+void AP_InertialSensor_Backend::update_accel_filters(uint8_t instance) /* front end */
+{
     // possibly update filter frequency
     if (_last_accel_filter_hz != _accel_filter_cutoff()) {
         _imu._accel_filter[instance].set_cutoff_frequency(_accel_raw_sample_rate(instance), _accel_filter_cutoff());
diff --git a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
index d6a3b5bdf07897..00d149e1492237 100644
--- a/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
+++ b/libraries/AP_InertialSensor/AP_InertialSensor_Backend.h
@@ -276,9 +276,11 @@ class AP_InertialSensor_Backend
 
     // common gyro update function for all backends
     void update_gyro(uint8_t instance) __RAMFUNC__; /* front end */
+    void update_gyro_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
     // common accel update function for all backends
     void update_accel(uint8_t instance) __RAMFUNC__; /* front end */
+    void update_accel_filters(uint8_t instance) __RAMFUNC__; /* front end */
 
     // support for updating filter at runtime
     uint16_t _last_accel_filter_hz;
diff --git a/libraries/AP_Logger/AP_Logger.cpp b/libraries/AP_Logger/AP_Logger.cpp
index 89e54962a98d50..2d00cd6a9f414d 100644
--- a/libraries/AP_Logger/AP_Logger.cpp
+++ b/libraries/AP_Logger/AP_Logger.cpp
@@ -489,7 +489,7 @@ bool AP_Logger::validate_structure(const struct LogStructure *logstructure, cons
     if (false && passed) {
         for (uint8_t j=0; j<strlen(logstructure->multipliers); j++) {
             const char fmt = logstructure->format[j];
-            if (fmt != 'f') {
+            if (fmt != 'f' && fmt != 'd' && fmt != 'g') {
                 continue;
             }
             const char logmultiplier = logstructure->multipliers[j];
@@ -1354,6 +1354,7 @@ int16_t AP_Logger::Write_calc_msg_len(const char *fmt) const
         case 'd' : len += sizeof(double); break;
         case 'e' : len += sizeof(int32_t); break;
         case 'f' : len += sizeof(float); break;
+        case 'g' : len += sizeof(float16_s); break;
         case 'h' : len += sizeof(int16_t); break;
         case 'i' : len += sizeof(int32_t); break;
         case 'n' : len += sizeof(char[4]); break;
diff --git a/libraries/AP_Logger/AP_Logger_Backend.cpp b/libraries/AP_Logger/AP_Logger_Backend.cpp
index 7406270beb97de..6d9db1a43b9a77 100644
--- a/libraries/AP_Logger/AP_Logger_Backend.cpp
+++ b/libraries/AP_Logger/AP_Logger_Backend.cpp
@@ -257,6 +257,13 @@ bool AP_Logger_Backend::Write(const uint8_t msg_type, va_list arg_list, bool is_
             offset += sizeof(float);
             break;
         }
+        case 'g': {
+            Float16_t tmp;
+            tmp.set(va_arg(arg_list, double));;
+            memcpy(&buffer[offset], &tmp, sizeof(tmp));
+            offset += sizeof(tmp);
+            break;
+        }
         case 'n':
             charlen = 4;
             break;
diff --git a/libraries/AP_Logger/AP_Logger_File.cpp b/libraries/AP_Logger/AP_Logger_File.cpp
index 3ac9c7d0935735..35eb8c8f532ad5 100644
--- a/libraries/AP_Logger/AP_Logger_File.cpp
+++ b/libraries/AP_Logger/AP_Logger_File.cpp
@@ -428,7 +428,7 @@ bool AP_Logger_File::StartNewLogOK() const
     if (recent_open_error()) {
         return false;
     }
-#if !APM_BUILD_TYPE(APM_BUILD_Replay)
+#if !APM_BUILD_TYPE(APM_BUILD_Replay) && !APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
     if (hal.scheduler->in_main_thread()) {
         return false;
     }
@@ -869,7 +869,7 @@ bool AP_Logger_File::write_lastlog_file(uint16_t log_num)
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL || CONFIG_HAL_BOARD == HAL_BOARD_LINUX
 void AP_Logger_File::flush(void)
-#if APM_BUILD_TYPE(APM_BUILD_Replay)
+#if APM_BUILD_TYPE(APM_BUILD_Replay) || APM_BUILD_TYPE(APM_BUILD_UNKNOWN)
 {
     uint32_t tnow = AP_HAL::millis();
     while (_write_fd != -1 && _initialised && !recent_open_error() && _writebuf.available()) {
diff --git a/libraries/AP_Logger/README.md b/libraries/AP_Logger/README.md
index d049c16f654c9b..52b99ff583ee2b 100644
--- a/libraries/AP_Logger/README.md
+++ b/libraries/AP_Logger/README.md
@@ -23,6 +23,7 @@ and how the content should be interpreted.
 |M   | uint8_t flight mode|
 |q   | int64_t|
 |Q   | uint64_t|
+|g   | float16_t|
 
 Legacy field types - do not use.  These have been replaced by using  the base C type and an appropriate multiplier column entry.
 
diff --git a/libraries/AP_Logger/examples/AP_Logger_AllTypes/AP_Logger_AllTypes.cpp b/libraries/AP_Logger/examples/AP_Logger_AllTypes/AP_Logger_AllTypes.cpp
index 76e8aac2b696c3..740c0ed0d70c47 100644
--- a/libraries/AP_Logger/examples/AP_Logger_AllTypes/AP_Logger_AllTypes.cpp
+++ b/libraries/AP_Logger/examples/AP_Logger_AllTypes/AP_Logger_AllTypes.cpp
@@ -40,6 +40,7 @@ struct PACKED log_TYP2 {
     uint8_t   M;   // flight mode
     int64_t   q;
     uint64_t  Q;
+    Float16_t g;
 };
 
 
@@ -74,10 +75,10 @@ static const struct LogStructure log_structure[] = {
     { LOG_TYP2_MSG,
       sizeof(log_TYP2),
       "TYP2",
-      "QcCeELMqQ",
-      "TimeUS,c,C,e,E,L,M,q,Q",
-      "s--------",
-      "F--------"
+      "QcCeELMqQg",
+      "TimeUS,c,C,e,E,L,M,q,Q,g",
+      "s---------",
+      "F---------"
     },
     { LOG_MESSAGE_MSG,
       sizeof(log_Message),
@@ -93,8 +94,8 @@ static const struct LogStructure log_structure[] = {
 // structure and that in LogStructure.h
 #define TYP1_FMT "QabBhHiIfdnNZ"
 #define TYP1_LBL "TimeUS,b,B,h,H,i,I,f,d,n,N,Z"
-#define TYP2_FMT "QcCeELMqQ"
-#define TYP2_LBL "TimeUS,c,C,e,E,L,M,q,Q"
+#define TYP2_FMT "QcCeELMqQg"
+#define TYP2_LBL "TimeUS,c,C,e,E,L,M,q,Q,g"
 
 static uint16_t log_num;
 
@@ -158,8 +159,12 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
               'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P',
               'A','B','C','D','E','F','G','H','I','J','K','L','M','N','O','P' }
     };
-    logger.WriteBlock(&typ1, sizeof(typ1));
+    if (!logger.WriteBlock_first_succeed(&typ1, sizeof(typ1))) {
+        abort();
+    }
 
+    Float16_t f16;
+    f16.set(13.54f);
     struct log_TYP2 typ2 = {
         LOG_PACKET_HEADER_INIT(LOG_TYP2_MSG),
         time_us : AP_HAL::micros64(),
@@ -170,7 +175,8 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages()
         L : -3576543,   // uint32_t latitude/longitude;
         M : 5,          //   uint8_t;   // flight mode;
         q : -98239832498328,   // int64_t
-        Q : 3432345232233432   // uint64_t
+        Q : 3432345232233432,  // uint64_t
+        g : f16              // float16_t
     };
     logger.WriteBlock(&typ2, sizeof(typ2));
 
@@ -232,7 +238,8 @@ void AP_LoggerTest_AllTypes::Log_Write_TypeMessages_Log_Write()
                  -3576543,   // uint32_t latitude/longitude;
                  5,          //   uint8_t;   // flight mode;
                  -98239832498328,   // int64_t
-                 3432345232233432   // uint64_t
+                 3432345232233432,   // uint64_t
+                 13.54               // float16_t
         );
 
     // emit a message which contains NaNs:
diff --git a/libraries/AP_Logger/examples/AP_Logger_AllTypes/output.BIN b/libraries/AP_Logger/examples/AP_Logger_AllTypes/output.BIN
index 2976c66e9d7456..f4bd3d14466bb4 100644
Binary files a/libraries/AP_Logger/examples/AP_Logger_AllTypes/output.BIN and b/libraries/AP_Logger/examples/AP_Logger_AllTypes/output.BIN differ
diff --git a/libraries/AP_Math/AP_Math.cpp b/libraries/AP_Math/AP_Math.cpp
index 2e4d6ba9b5b404..e553ade4d941ab 100644
--- a/libraries/AP_Math/AP_Math.cpp
+++ b/libraries/AP_Math/AP_Math.cpp
@@ -339,14 +339,13 @@ uint16_t get_random16(void)
 }
 
 
-#if AP_SIM_ENABLED
-// generate a random float between -1 and 1, for use in SITL
+// generate a random float between -1 and 1
 float rand_float(void)
 {
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     return ((((unsigned)random()) % 2000000) - 1.0e6) / 1.0e6;
 #else
-    return get_random16() / 65535.0;
+    return (get_random16() / 65535.0) * 2 - 1;
 #endif
 }
 
@@ -359,7 +358,6 @@ Vector3f rand_vec3f(void)
         rand_float()
     };
 }
-#endif
 
 /*
   return true if two rotations are equivalent
diff --git a/libraries/AP_Math/tests/test_math.cpp b/libraries/AP_Math/tests/test_math.cpp
index afd8a164c829f3..de3f3a53a6171b 100644
--- a/libraries/AP_Math/tests/test_math.cpp
+++ b/libraries/AP_Math/tests/test_math.cpp
@@ -654,6 +654,23 @@ TEST(MathTest, RANDOM16)
     EXPECT_NE(random_value, get_random16());
 }
 
+TEST(MathTest, RAND_FLOAT)
+{
+    // bodgy range checks
+    float lowest_value = 0;
+    float highest_value = 0;
+    for (auto i=0; i<1000; i++) {
+        const auto value = rand_float();
+        lowest_value = MIN(lowest_value, value);
+        highest_value = MAX(highest_value, value);
+    }
+    EXPECT_NEAR(-0.95, lowest_value, 0.05);
+    EXPECT_NEAR(0.95, highest_value, 0.05);
+    EXPECT_GE(lowest_value, -1.0);
+    EXPECT_LE(highest_value, 1.0);
+
+}
+
 TEST(MathTest, VELCORRECTION)
 {
     static constexpr Vector3F pos{1.0f, 1.0f, 0.0f};
diff --git a/libraries/AP_Mount/AP_Mount_Siyi.cpp b/libraries/AP_Mount/AP_Mount_Siyi.cpp
index 2158eb6dad707f..fcaa4581a15c2b 100644
--- a/libraries/AP_Mount/AP_Mount_Siyi.cpp
+++ b/libraries/AP_Mount/AP_Mount_Siyi.cpp
@@ -28,7 +28,7 @@ const AP_Mount_Siyi::HWInfo AP_Mount_Siyi::hardware_lookup_table[] {
         {{'7','3'}, "A8"},
         {{'6','B'}, "ZR10"},
         {{'7','8'}, "ZR30"},
-        {{'8','2'}, "ZT6"},
+        {{'8','3'}, "ZT6"},
         {{'7','A'}, "ZT30"},
 };
 
@@ -910,30 +910,55 @@ SetFocusResult AP_Mount_Siyi::set_focus(FocusType focus_type, float focus_value)
 // set camera lens as a value from 0 to 8
 bool AP_Mount_Siyi::set_lens(uint8_t lens)
 {
-    // only supported on ZT30.  sanity check lens values
-    if ((_hardware_model != HardwareModel::ZT30) || (lens > 8)) {
-        return false;
+    CameraImageType selected_lens;
+
+    switch (_hardware_model) {
+
+    case HardwareModel::ZT30: {
+        // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
+        static const CameraImageType cam_image_type_table[] {
+            CameraImageType::MAIN_ZOOM_SUB_THERMAL,               // 3
+            CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL,          // 5
+            CameraImageType::MAIN_THERMAL_SUB_ZOOM,               // 7
+            CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0
+            CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1
+            CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2
+            CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE,             // 4
+            CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM,             // 6
+            CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE,          // 8
+        };
+
+        // sanity check lens values
+        if (lens >= ARRAY_SIZE(cam_image_type_table)) {
+            return false;
+        }
+        selected_lens = cam_image_type_table[lens];
+        break;
     }
 
-    // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
-    static const CameraImageType cam_image_type_table[] {
-        CameraImageType::MAIN_ZOOM_SUB_THERMAL,               // 3
-        CameraImageType::MAIN_WIDEANGLE_SUB_THERMAL,          // 5
-        CameraImageType::MAIN_THERMAL_SUB_ZOOM,               // 7
-        CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0
-        CameraImageType::MAIN_PIP_WIDEANGLE_THERMAL_SUB_ZOOM, // 1
-        CameraImageType::MAIN_PIP_ZOOM_WIDEANGLE_SUB_THERMAL, // 2
-        CameraImageType::MAIN_ZOOM_SUB_WIDEANGLE,             // 4
-        CameraImageType::MAIN_WIDEANGLE_SUB_ZOOM,             // 6
-        CameraImageType::MAIN_THERMAL_SUB_WIDEANGLE,          // 8
-    };
-
-    if (lens >= ARRAY_SIZE(cam_image_type_table)) {
+    case HardwareModel::ZT6: {
+        // maps lens to siyi camera image type so that lens of 0, 1, 2 are more useful
+        static const CameraImageType cam_image_type_table[] {
+            CameraImageType::MAIN_ZOOM_SUB_THERMAL,               // 3
+            CameraImageType::MAIN_THERMAL_SUB_ZOOM,               // 7
+            CameraImageType::MAIN_PIP_ZOOM_THERMAL_SUB_WIDEANGLE, // 0
+        };
+
+        // sanity check lens values
+        if (lens >= ARRAY_SIZE(cam_image_type_table)) {
+            return false;
+        }
+        selected_lens = cam_image_type_table[lens];
+        break;
+    }
+
+    default:
+        // set lens not supported on this camera
         return false;
     }
 
     // send desired image type to camera
-    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)cam_image_type_table[lens]);
+    return send_1byte_packet(SiyiCommandId::SET_CAMERA_IMAGE_TYPE, (uint8_t)selected_lens);
 }
 
 // set_camera_source is functionally the same as set_lens except primary and secondary lenses are specified by type
diff --git a/libraries/AP_NavEKF/EKF_Buffer.cpp b/libraries/AP_NavEKF/EKF_Buffer.cpp
index b557b4e88dd3cd..b27dd084baca8e 100644
--- a/libraries/AP_NavEKF/EKF_Buffer.cpp
+++ b/libraries/AP_NavEKF/EKF_Buffer.cpp
@@ -53,12 +53,13 @@ uint32_t ekf_ring_buffer::time_ms(uint8_t idx) const
 bool ekf_ring_buffer::recall(void *element, const uint32_t sample_time_ms)
 {
     bool ret = false;
+    uint8_t best_index = 0;  // only valid when ret becomes true
     while (count > 0) {
         const uint32_t toldest = time_ms(oldest);
         const int32_t dt = sample_time_ms - toldest;
         const bool matches = dt >= 0 && dt < 100;
         if (matches) {
-            memcpy(element, get_offset(oldest), elsize);
+            best_index = oldest;
             ret = true;
         }
         if (dt < 0) {
@@ -70,6 +71,11 @@ bool ekf_ring_buffer::recall(void *element, const uint32_t sample_time_ms)
         count--;
         oldest = (oldest+1) % size;
     }
+
+    if (ret) {
+        memcpy(element, get_offset(best_index), elsize);
+    }
+
     return ret;
 }
 
diff --git a/libraries/AP_NavEKF3/AP_NavEKF3.cpp b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
index 2021b69ba2195d..0a6dcb01f1c688 100644
--- a/libraries/AP_NavEKF3/AP_NavEKF3.cpp
+++ b/libraries/AP_NavEKF3/AP_NavEKF3.cpp
@@ -2043,12 +2043,15 @@ void NavEKF3::updateLaneSwitchPosDownResetData(uint8_t new_primary, uint8_t old_
 // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
 void NavEKF3::writeDefaultAirSpeed(float airspeed, float uncertainty)
 {
+    // ignore any data if the EKF is not started
+    if (!core) {
+        return;
+    }
+
     AP::dal().log_writeDefaultAirSpeed3(airspeed, uncertainty);
 
-    if (core) {
-        for (uint8_t i=0; i<num_cores; i++) {
-            core[i].writeDefaultAirSpeed(airspeed, uncertainty);
-        }
+    for (uint8_t i=0; i<num_cores; i++) {
+        core[i].writeDefaultAirSpeed(airspeed, uncertainty);
     }
 }
 
diff --git a/libraries/AP_Notify/GPIO_LED_1.cpp b/libraries/AP_Notify/GPIO_LED_1.cpp
index 6ea0bd16c9aa0d..431256465f4440 100644
--- a/libraries/AP_Notify/GPIO_LED_1.cpp
+++ b/libraries/AP_Notify/GPIO_LED_1.cpp
@@ -47,6 +47,8 @@ void GPIO_LED_1::update(void)
     uint32_t new_pattern;
     if (AP_Notify::flags.initialising) {
         new_pattern = INITIALIZING;
+    } else if (AP_Notify::flags.failsafe_radio || AP_Notify::flags.failsafe_gcs || AP_Notify::flags.failsafe_battery) {
+        new_pattern = FAILSAFE;
     } else if (AP_Notify::flags.armed) {
         new_pattern = ARMED;
     } else if (AP_Notify::flags.pre_arm_check) {
diff --git a/libraries/AP_Notify/GPIO_LED_1.h b/libraries/AP_Notify/GPIO_LED_1.h
index 129e6cac520185..b9f2a0302ee32c 100644
--- a/libraries/AP_Notify/GPIO_LED_1.h
+++ b/libraries/AP_Notify/GPIO_LED_1.h
@@ -36,9 +36,10 @@ class GPIO_LED_1 : public NotifyDevice
 
     // left-to-right, each bit represents 100ms
     static const uint32_t INITIALIZING = 0b10101010101010101010101010101010UL;
-    static const uint32_t NOT_READY_TO_ARM = 0b11111111000000001111111100000000UL;
+    static const uint32_t NOT_READY_TO_ARM = 0b11110000000000001111111110000000UL;
     static const uint32_t READY_TO_ARM = 0b11111111111111100000000000000000UL;
     static const uint32_t ARMED = 0b11111111111111111111111111111111UL;
+    static const uint32_t FAILSAFE = 0b11110000111100001111000011110000UL;
 
     uint32_t current_pattern = INITIALIZING;
     uint32_t last_timestep_ms;
diff --git a/libraries/AP_RangeFinder/AP_RangeFinder.h b/libraries/AP_RangeFinder/AP_RangeFinder.h
index c67ad5cce76a9a..4d66101b8b6ab9 100644
--- a/libraries/AP_RangeFinder/AP_RangeFinder.h
+++ b/libraries/AP_RangeFinder/AP_RangeFinder.h
@@ -194,11 +194,11 @@ class RangeFinder
     };
 
     enum class Status {
-        NotConnected = 0,
-        NoData,
-        OutOfRangeLow,
-        OutOfRangeHigh,
-        Good
+        NotConnected   = 0,
+        NoData         = 1,
+        OutOfRangeLow  = 2,
+        OutOfRangeHigh = 3,
+        Good           = 4,
     };
 
     static constexpr int8_t SIGNAL_QUALITY_MIN = 0;
diff --git a/libraries/AP_Scripting/modules/MAVLink/Readme.md b/libraries/AP_Scripting/modules/MAVLink/Readme.md
index 97924a1c33a075..24db0379b2d9ee 100644
--- a/libraries/AP_Scripting/modules/MAVLink/Readme.md
+++ b/libraries/AP_Scripting/modules/MAVLink/Readme.md
@@ -3,4 +3,6 @@ These .lua definition for messages can be generated using pymavlink with the fol
 ```
 cd ardupilot/modules/mavlink
 python ./pymavlink/tools/mavgen.py --lang Lua ./message_definitions/v1.0/all.xml --out ./modules/MAVLink
-```
\ No newline at end of file
+```
+
+Add `--wire-protocol 2.0` to include extension fields in the generated code
diff --git a/libraries/Filter/LowPassFilter.cpp b/libraries/Filter/LowPassFilter.cpp
index eba8e0d38bf067..d18cdd00b4d0fc 100644
--- a/libraries/Filter/LowPassFilter.cpp
+++ b/libraries/Filter/LowPassFilter.cpp
@@ -117,20 +117,7 @@ float LowPassFilter<T>::get_cutoff_freq() const {
 // add a new raw value to the filter, retrieve the filtered result
 template <class T>
 T LowPassFilter<T>::apply(const T &sample, const float &dt) {
-    if (is_negative(cutoff_freq) || is_negative(dt)) {
-        INTERNAL_ERROR(AP_InternalError::error_t::invalid_arg_or_result);
-        this->reset(sample);
-        return this->get();
-    }
-    if (is_zero(cutoff_freq)) {
-        this->reset(sample);
-        return this->get();
-    }
-    if (is_zero(dt)) {
-        return this->get();
-    }
-    const float rc = 1.0f/(M_2PI*cutoff_freq);
-    const float alpha = constrain_float(dt/(dt+rc), 0.0f, 1.0f);
+    const float alpha = calc_lowpass_alpha_dt(dt, cutoff_freq);
     return this->_apply(sample, alpha);
 }
 
diff --git a/libraries/RC_Channel/RC_Channel.cpp b/libraries/RC_Channel/RC_Channel.cpp
index 04508c3f7b53cf..5c424706cb9dcc 100644
--- a/libraries/RC_Channel/RC_Channel.cpp
+++ b/libraries/RC_Channel/RC_Channel.cpp
@@ -239,6 +239,7 @@ const AP_Param::GroupInfo RC_Channel::var_info[] = {
     // @Values{Plane}: 176:Quadplane Fwd Throttle Override enable
     // @Values{Copter, Rover, Plane, Blimp}: 177:Mount LRF enable
     // @Values{Copter}: 178:FlightMode Pause/Resume
+    // @Values{Plane}: 179:ICEngine start / stop
     // @Values{Copter, Plane}: 180:Test autotuned gains after tune is complete
     // @Values{Rover}: 201:Roll
     // @Values{Rover}: 202:Pitch
diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h
index 496a3187d5433c..34dfb90e92638e 100644
--- a/libraries/RC_Channel/RC_Channel.h
+++ b/libraries/RC_Channel/RC_Channel.h
@@ -252,6 +252,7 @@ class RC_Channel {
         VFWD_THR_OVERRIDE =  176, // force enabled VTOL forward throttle method
         MOUNT_LRF_ENABLE =   177,  // mount LRF enable/disable
         FLIGHTMODE_PAUSE =   178,  // e.g. pause movement towards waypoint
+        ICE_START_STOP =     179, // AP_ICEngine start stop
         AUTOTUNE_TEST_GAINS = 180, // auto tune tuning switch to test or revert gains
 
 
diff --git a/libraries/SITL/SIM_GPS.cpp b/libraries/SITL/SIM_GPS.cpp
index a6e2564cf9e718..35fe90d90a45be 100644
--- a/libraries/SITL/SIM_GPS.cpp
+++ b/libraries/SITL/SIM_GPS.cpp
@@ -356,6 +356,7 @@ void GPS::update()
 
     last_write_update_ms = now_ms;
 
+    d.num_sats = _sitl->gps_numsats[idx];
     d.latitude = latitude;
     d.longitude = longitude;
     d.yaw_deg = _sitl->state.yawDeg;
@@ -370,8 +371,14 @@ void GPS::update()
     d.speedN = speedN + (velErrorNED.x * rand_float());
     d.speedE = speedE + (velErrorNED.y * rand_float());
     d.speedD = speedD + (velErrorNED.z * rand_float());
+
     d.have_lock = have_lock;
 
+    // fill in accuracies
+    d.horizontal_acc = _sitl->gps_accuracy[idx];
+    d.vertical_acc = _sitl->gps_accuracy[idx];
+    d.speed_acc = _sitl->gps_vel_err[instance].get().xy().length();
+
     if (_sitl->gps_drift_alt[idx] > 0) {
         // add slow altitude drift controlled by a slow sine wave
         d.altitude += _sitl->gps_drift_alt[idx]*sinf(now_ms*0.001f*0.02f);
diff --git a/libraries/SITL/SIM_GPS_MSP.cpp b/libraries/SITL/SIM_GPS_MSP.cpp
index 65d8838b7d6d97..66bde9c22ce6ef 100644
--- a/libraries/SITL/SIM_GPS_MSP.cpp
+++ b/libraries/SITL/SIM_GPS_MSP.cpp
@@ -63,9 +63,9 @@ void GPS_MSP::publish(const GPS_Data *d)
     msp_gps.gps_week = t.week;
     msp_gps.ms_tow = t.ms;
     msp_gps.fix_type = d->have_lock?3:0;
-    msp_gps.satellites_in_view = d->have_lock ? _sitl->gps_numsats[instance] : 3;
-    msp_gps.horizontal_pos_accuracy = _sitl->gps_accuracy[instance]*100;
-    msp_gps.vertical_pos_accuracy = _sitl->gps_accuracy[instance]*100;
+    msp_gps.satellites_in_view = d->have_lock ? d->num_sats : 3;
+    msp_gps.horizontal_pos_accuracy = d->horizontal_acc*100;
+    msp_gps.vertical_pos_accuracy = d->vertical_acc*100;
     msp_gps.horizontal_vel_accuracy = 30;
     msp_gps.hdop = 100;
     msp_gps.longitude = d->longitude * 1.0e7;
diff --git a/libraries/SITL/SIM_GPS_NMEA.cpp b/libraries/SITL/SIM_GPS_NMEA.cpp
index 30c705382e1521..23d2a91ad45f98 100644
--- a/libraries/SITL/SIM_GPS_NMEA.cpp
+++ b/libraries/SITL/SIM_GPS_NMEA.cpp
@@ -74,7 +74,7 @@ void GPS_NMEA::publish(const GPS_Data *d)
                      lat_string,
                      lng_string,
                      d->have_lock?1:0,
-                     d->have_lock?_sitl->gps_numsats[instance]:3,
+                     d->have_lock?d->num_sats:3,
                      1.2,
                      d->altitude);
 
@@ -118,8 +118,8 @@ void GPS_NMEA::publish(const GPS_Data *d)
                     d->roll_deg,
                     d->have_lock?1:0, // 2=rtkfloat 3=rtkfixed,
                     3, // fixed rtk yaw solution,
-                    d->have_lock?_sitl->gps_numsats[instance]:3,
-                    d->have_lock?_sitl->gps_numsats[instance]:3,
+                    d->have_lock?d->num_sats:3,
+                    d->have_lock?d->num_sats:3,
                     d->speedE * 3.6,
                     d->speedN * 3.6,
                     -d->speedD * 3.6);
diff --git a/libraries/SITL/SIM_GPS_NOVA.cpp b/libraries/SITL/SIM_GPS_NOVA.cpp
index 08f793f4d27138..808d6b19cc3caa 100644
--- a/libraries/SITL/SIM_GPS_NOVA.cpp
+++ b/libraries/SITL/SIM_GPS_NOVA.cpp
@@ -129,7 +129,7 @@ void GPS_NOVA::publish(const GPS_Data *d)
     bestpos.lat = d->latitude;
     bestpos.lng = d->longitude;
     bestpos.hgt = d->altitude;
-    bestpos.svsused = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    bestpos.svsused = d->have_lock ? d->num_sats : 3;
     bestpos.latsdev=0.2;
     bestpos.lngsdev=0.2;
     bestpos.hgtsdev=0.2;
diff --git a/libraries/SITL/SIM_GPS_SBF.cpp b/libraries/SITL/SIM_GPS_SBF.cpp
index 13ccf1ab6f4b24..5df55017560dbe 100644
--- a/libraries/SITL/SIM_GPS_SBF.cpp
+++ b/libraries/SITL/SIM_GPS_SBF.cpp
@@ -134,7 +134,7 @@ void GPS_SBF::publish_PVTGeodetic(const GPS_Data *d)
     pvtGeod_buf.rxclkdrift = DNU_FLOAT;
     pvtGeod_buf.timesystem = DNU_UINT8; 
     pvtGeod_buf.datum = DNU_UINT8; 
-    pvtGeod_buf.nrsv = _sitl->gps_numsats[instance];
+    pvtGeod_buf.nrsv = d->num_sats;
     pvtGeod_buf.wacorrinfo = 0; //default value
     pvtGeod_buf.referenceid = DNU_UINT16; 
     pvtGeod_buf.meancorrage = DNU_UINT16; 
diff --git a/libraries/SITL/SIM_GPS_SBP.cpp b/libraries/SITL/SIM_GPS_SBP.cpp
index 9f4c8078e8c935..a3b11f2e873f95 100644
--- a/libraries/SITL/SIM_GPS_SBP.cpp
+++ b/libraries/SITL/SIM_GPS_SBP.cpp
@@ -77,9 +77,9 @@ void GPS_SBP::publish(const GPS_Data *d)
     pos.lon = d->longitude;
     pos.lat= d->latitude;
     pos.height = d->altitude;
-    pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pos.h_accuracy = d->horizontal_acc*1000;
+    pos.v_accuracy = d->vertical_acc*1000;
+    pos.n_sats = d->have_lock ? d->num_sats : 3;
 
     // Send single point position solution
     pos.flags = 0;
@@ -94,7 +94,7 @@ void GPS_SBP::publish(const GPS_Data *d)
     velned.d = 1e3 * d->speedD;
     velned.h_accuracy = 5e3;
     velned.v_accuracy = 5e3;
-    velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    velned.n_sats = d->have_lock ? d->num_sats : 3;
     velned.flags = 0;
     sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
 
diff --git a/libraries/SITL/SIM_GPS_SBP2.cpp b/libraries/SITL/SIM_GPS_SBP2.cpp
index 3acd391e06c9a6..f7b33361f36598 100644
--- a/libraries/SITL/SIM_GPS_SBP2.cpp
+++ b/libraries/SITL/SIM_GPS_SBP2.cpp
@@ -77,9 +77,9 @@ void GPS_SBP2::publish(const GPS_Data *d)
     pos.lon = d->longitude;
     pos.lat= d->latitude;
     pos.height = d->altitude;
-    pos.h_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.v_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pos.h_accuracy = d->horizontal_acc*1000;
+    pos.v_accuracy = d->vertical_acc*1000;
+    pos.n_sats = d->have_lock ? d->num_sats : 3;
 
     // Send single point position solution
     pos.flags = 1;
@@ -94,7 +94,7 @@ void GPS_SBP2::publish(const GPS_Data *d)
     velned.d = 1e3 * d->speedD;
     velned.h_accuracy = 1e3 * 0.5;
     velned.v_accuracy = 1e3 * 0.5;
-    velned.n_sats = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    velned.n_sats = d->have_lock ? d->num_sats : 3;
     velned.flags = 1;
     sbp_send_message(SBP_VEL_NED_MSGTYPE, 0x2222, sizeof(velned), (uint8_t*)&velned);
 
diff --git a/libraries/SITL/SIM_GPS_Trimble.cpp b/libraries/SITL/SIM_GPS_Trimble.cpp
index 9bb3edd541eac0..4e5883ef45d8e7 100644
--- a/libraries/SITL/SIM_GPS_Trimble.cpp
+++ b/libraries/SITL/SIM_GPS_Trimble.cpp
@@ -93,7 +93,7 @@ void GPS_Trimble::publish(const GPS_Data *d)
                 GSOF_POS_TIME_LEN,
                 htobe32(gps_tow.ms),
                 htobe16(gps_tow.week),
-                d->have_lock ? _sitl->gps_numsats[instance] : uint8_t(3),
+                d->have_lock ? d->num_sats : uint8_t(3),
                 pos_flags_1,
                 pos_flags_2,
                 bootcount
diff --git a/libraries/SITL/SIM_GPS_UBLOX.cpp b/libraries/SITL/SIM_GPS_UBLOX.cpp
index 507d767df9f0c6..ce77a44d284a1a 100644
--- a/libraries/SITL/SIM_GPS_UBLOX.cpp
+++ b/libraries/SITL/SIM_GPS_UBLOX.cpp
@@ -195,8 +195,8 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pos.latitude  = d->latitude * 1.0e7;
     pos.altitude_ellipsoid = d->altitude * 1000.0f;
     pos.altitude_msl = d->altitude * 1000.0f;
-    pos.horizontal_accuracy = _sitl->gps_accuracy[instance]*1000;
-    pos.vertical_accuracy = _sitl->gps_accuracy[instance]*1000;
+    pos.horizontal_accuracy = d->horizontal_acc*1000;
+    pos.vertical_accuracy = d->vertical_acc*1000;
 
     status.time = gps_tow.ms;
     status.fix_type = d->have_lock?3:0;
@@ -216,13 +216,13 @@ void GPS_UBlox::publish(const GPS_Data *d)
     if (velned.heading_2d < 0.0f) {
         velned.heading_2d += 360.0f * 100000.0f;
     }
-    velned.speed_accuracy = _sitl->gps_vel_err[instance].get().xy().length() * 100;  // m/s -> cm/s
+    velned.speed_accuracy = d->speed_acc * 100;  // m/s -> cm/s
     velned.heading_accuracy = 4;
 
     memset(&sol, 0, sizeof(sol));
     sol.fix_type = d->have_lock?3:0;
     sol.fix_status = 221;
-    sol.satellites = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    sol.satellites = d->have_lock ? d->num_sats : 3;
     sol.time = gps_tow.ms;
     sol.week = gps_tow.week;
 
@@ -248,13 +248,13 @@ void GPS_UBlox::publish(const GPS_Data *d)
     pvt.fix_type = d->have_lock? 0x3 : 0;
     pvt.flags = 0b10000011; // carrsoln=fixed, psm = na, diffsoln and fixok
     pvt.flags2 =0;
-    pvt.num_sv = d->have_lock ? _sitl->gps_numsats[instance] : 3;
+    pvt.num_sv = d->have_lock ? d->num_sats : 3;
     pvt.lon = d->longitude * 1.0e7;
     pvt.lat  = d->latitude * 1.0e7;
     pvt.height = d->altitude * 1000.0f;
     pvt.h_msl = d->altitude * 1000.0f;
-    pvt.h_acc = _sitl->gps_accuracy[instance] * 1000;
-    pvt.v_acc = _sitl->gps_accuracy[instance] * 1000;
+    pvt.h_acc = d->horizontal_acc * 1000;
+    pvt.v_acc = d->vertical_acc * 1000;
     pvt.velN = 1000.0f * d->speedN;
     pvt.velE = 1000.0f * d->speedE;
     pvt.velD = 1000.0f * d->speedD;
@@ -309,7 +309,7 @@ void GPS_UBlox::publish(const GPS_Data *d)
         for (uint8_t i = 0; i < SV_COUNT; i++) {
             svinfo.sv[i].chn = i;
             svinfo.sv[i].svid = i;
-            svinfo.sv[i].flags = (i < _sitl->gps_numsats[instance]) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
+            svinfo.sv[i].flags = (i < d->num_sats) ? 0x7 : 0x6; // sv used, diff correction data, orbit information
             svinfo.sv[i].quality = 7; // code and carrier lock and time synchronized
             svinfo.sv[i].cno = MAX(20, 30 - i);
             svinfo.sv[i].elev = MAX(30, 90 - i);
diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h
index 22bcc5f7791d4e..9e6d69b2b71a12 100644
--- a/libraries/SRV_Channel/SRV_Channel.h
+++ b/libraries/SRV_Channel/SRV_Channel.h
@@ -415,7 +415,13 @@ class SRV_Channels {
 
     // set MIN/MAX parameters for a function
     static void set_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm);
-    
+
+    // set MIN/MAX parameter defaults for a function
+    static void set_output_min_max_defaults(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm);
+
+    // Save MIN/MAX/REVERSED parameters for a function
+    static void save_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm);
+
     // save trims
     void save_trim(void);
 
diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp
index 4c36ae332ccba3..1412957202c262 100644
--- a/libraries/SRV_Channel/SRV_Channel_aux.cpp
+++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp
@@ -844,6 +844,31 @@ void SRV_Channels::set_output_min_max(SRV_Channel::Aux_servo_function_t function
     }
 }
 
+// set MIN/MAX parameter defaults for a function
+void SRV_Channels::set_output_min_max_defaults(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm)
+{
+    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
+        if (channels[i].function == function) {
+            channels[i].servo_min.set_default(min_pwm);
+            channels[i].servo_max.set_default(max_pwm);
+        }
+    }
+}
+
+// Save MIN/MAX/REVERSED parameters for a function
+void SRV_Channels::save_output_min_max(SRV_Channel::Aux_servo_function_t function, uint16_t min_pwm, uint16_t max_pwm)
+{
+    for (uint8_t i=0; i<NUM_SERVO_CHANNELS; i++) {
+        if (channels[i].function == function) {
+            // If min is larger than max swap and set reversed
+            const bool reversed = min_pwm > max_pwm;
+            channels[i].servo_min.set_and_save(reversed ? max_pwm : min_pwm);
+            channels[i].servo_max.set_and_save(reversed ? min_pwm : max_pwm);
+            channels[i].reversed.set_and_save(reversed ? 1 : 0);
+        }
+    }
+}
+
 // constrain to output min/max for function
 void SRV_Channels::constrain_pwm(SRV_Channel::Aux_servo_function_t function)
 {
diff --git a/modules/mavlink b/modules/mavlink
index 603e3c8e0c6031..60aa4ff8472cf9 160000
--- a/modules/mavlink
+++ b/modules/mavlink
@@ -1 +1 @@
-Subproject commit 603e3c8e0c6031992da4206239681d13df5915be
+Subproject commit 60aa4ff8472cf91506256082bea8a1c3bbf9068b