diff --git a/Tools/AP_Periph/can.cpp b/Tools/AP_Periph/can.cpp
index deadcd03f36f71..01adff69a5f60c 100644
--- a/Tools/AP_Periph/can.cpp
+++ b/Tools/AP_Periph/can.cpp
@@ -1136,8 +1136,9 @@ void AP_Periph_FW::processTx(void)
                   active if it has had a successful transmit in the
                   last 2 seconds
                  */
-                const auto *stats = _ins.iface->get_statistics();
-                if (stats == nullptr || now_us - stats->last_transmit_us < 2000000UL) {
+                volatile const auto *stats = _ins.iface->get_statistics();
+                uint64_t last_transmit_us = stats->last_transmit_us;
+                if (stats == nullptr || AP_HAL::micros64() - last_transmit_us < 2000000UL) {
                     sent = false;
                 }
             } else {
diff --git a/Tools/autotest/blimp.py b/Tools/autotest/blimp.py
index 05a8d5fd980567..661c4a516286cc 100644
--- a/Tools/autotest/blimp.py
+++ b/Tools/autotest/blimp.py
@@ -237,6 +237,15 @@ def FlyLoiter(self):
 
         self.disarm_vehicle()
 
+    def PREFLIGHT_Pressure(self):
+        '''test triggering pressure calibration with mavlink command'''
+        # as airspeed is not instantiated on Blimp we expect to
+        # instantly get back an accepted.
+        self.run_cmd(
+            mavutil.mavlink.MAV_CMD_PREFLIGHT_CALIBRATION,
+            p3=1, # p3, baro
+        )
+
     def tests(self):
         '''return list of all tests'''
         # ret = super(AutoTestBlimp, self).tests()
@@ -244,6 +253,7 @@ def tests(self):
         ret.extend([
             self.FlyManual,
             self.FlyLoiter,
+            self.PREFLIGHT_Pressure,
         ])
         return ret
 
diff --git a/Tools/autotest/rover.py b/Tools/autotest/rover.py
index 2da688ab124107..b61950f1248fb2 100644
--- a/Tools/autotest/rover.py
+++ b/Tools/autotest/rover.py
@@ -5620,9 +5620,9 @@ def SendToComponents(self):
             0,
             0)
 
-        self.context_collect('COMMAND_LONG')
-
         self.progress("Sending control message")
+        self.context_push()
+        self.context_collect('COMMAND_LONG')
         self.mav.mav.digicam_control_send(
             1, # target_system
             1, # target_component
@@ -5641,6 +5641,52 @@ def SendToComponents(self):
             'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL,
             'param6': 17,
         }, timeout=2, check_context=True)
+        self.context_pop()
+
+        # test sending via commands:
+        for run_cmd in self.run_cmd, self.run_cmd_int:
+            self.progress("Sending control command")
+            self.context_push()
+            self.context_collect('COMMAND_LONG')
+            run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL,
+                    p1=1, # start or keep it up
+                    p2=1, # zoom_pos
+                    p3=0, # zoom_step
+                    p4=0, # focus_lock
+                    p5=0, # 1 shot or start filming
+                    p6=37, # command id (de-dupe field)
+                    )
+
+            self.assert_received_message_field_values('COMMAND_LONG', {
+                'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONTROL,
+                'param6': 37,
+            }, timeout=2, check_context=True)
+
+            self.context_pop()
+
+        # test sending via commands:
+        for run_cmd in self.run_cmd, self.run_cmd_int:
+            self.progress("Sending configure command")
+            self.context_push()
+            self.context_collect('COMMAND_LONG')
+            run_cmd(mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE,
+                    p1=1,
+                    p2=1,
+                    p3=0,
+                    p4=0,
+                    p5=12,
+                    p6=37
+                    )
+
+            self.assert_received_message_field_values('COMMAND_LONG', {
+                'command': mavutil.mavlink.MAV_CMD_DO_DIGICAM_CONFIGURE,
+                'param5': 12,
+                'param6': 37,
+            }, timeout=2, check_context=True)
+
+            self.context_pop()
+
+        self.mav.mav.srcSystem = old_srcSystem
 
     def SkidSteer(self):
         '''Check skid-steering'''
diff --git a/Tools/scripts/build_options.py b/Tools/scripts/build_options.py
index 1e92c7baa8a185..7ec8811fef8951 100644
--- a/Tools/scripts/build_options.py
+++ b/Tools/scripts/build_options.py
@@ -123,6 +123,7 @@ def __init__(self,
     Feature('Camera', 'Camera_Relay', 'AP_CAMERA_RELAY_ENABLED', 'Enable Camera Trigger via Relay support', 0, 'Camera,RELAY'),
     Feature('Camera', 'Camera_Servo', 'AP_CAMERA_SERVO_ENABLED', 'Enable Camera Trigger via Servo support', 0, 'Camera'),
     Feature('Camera', 'Camera_Solo', 'AP_CAMERA_SOLOGIMBAL_ENABLED', 'Enable Solo Gimbal support', 0, 'Camera'),
+    Feature('Camera', 'Camera_FOV_Status', 'AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'Enable Camera FOV Status send to GCS', 0, 'Camera,MOUNT'),  # noqa: E501
 
     Feature('Camera', 'RUNCAM', 'HAL_RUNCAM_ENABLED', 'Enable RunCam Control', 0, None),
 
diff --git a/Tools/scripts/extract_features.py b/Tools/scripts/extract_features.py
index 5e0285fa9bdd8d..e847065e833bad 100755
--- a/Tools/scripts/extract_features.py
+++ b/Tools/scripts/extract_features.py
@@ -122,6 +122,7 @@ def __init__(self, filename, nm="arm-none-eabi-nm", strings="strings"):
 
             ('AP_CAMERA_ENABLED', 'AP_Camera::var_info',),
             ('AP_CAMERA_{type}_ENABLED', 'AP_Camera_(?P<type>.*)::trigger_pic',),
+            ('AP_CAMERA_SEND_FOV_STATUS_ENABLED', 'AP_Camera::send_camera_fov_status'),
             ('HAL_RUNCAM_ENABLED', 'AP_RunCam::AP_RunCam',),
 
             ('HAL_PROXIMITY_ENABLED', 'AP_Proximity::AP_Proximity',),
diff --git a/libraries/AP_Camera/AP_Camera.cpp b/libraries/AP_Camera/AP_Camera.cpp
index 7be0cb8e83d58a..b3583a8468df60 100644
--- a/libraries/AP_Camera/AP_Camera.cpp
+++ b/libraries/AP_Camera/AP_Camera.cpp
@@ -279,14 +279,14 @@ void AP_Camera::handle_message(mavlink_channel_t chan, const mavlink_message_t &
 }
 
 // handle command_long mavlink messages
-MAV_RESULT AP_Camera::handle_command_long(const mavlink_command_long_t &packet)
+MAV_RESULT AP_Camera::handle_command(const mavlink_command_int_t &packet)
 {
     switch (packet.command) {
     case MAV_CMD_DO_DIGICAM_CONFIGURE:
-        configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6, packet.param7);
+        configure(packet.param1, packet.param2, packet.param3, packet.param4, packet.x, packet.y, packet.z);
         return MAV_RESULT_ACCEPTED;
     case MAV_CMD_DO_DIGICAM_CONTROL:
-        control(packet.param1, packet.param2, packet.param3, packet.param4, packet.param5, packet.param6);
+        control(packet.param1, packet.param2, packet.param3, packet.param4, packet.x, packet.y);
         return MAV_RESULT_ACCEPTED;
     case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
         set_trigger_distance(packet.param1);
@@ -438,7 +438,7 @@ void AP_Camera::cam_mode_toggle(uint8_t instance)
 }
 
 // configure camera
-void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
+void AP_Camera::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
 {
     WITH_SEMAPHORE(_rsem);
 
@@ -448,7 +448,7 @@ void AP_Camera::configure(float shooting_mode, float shutter_speed, float apertu
     primary->configure(shooting_mode, shutter_speed, aperture, ISO, exposure_type, cmd_id, engine_cutoff_time);
 }
 
-void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
+void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
 {
     WITH_SEMAPHORE(_rsem);
 
@@ -462,7 +462,7 @@ void AP_Camera::configure(uint8_t instance, float shooting_mode, float shutter_s
 }
 
 // handle camera control
-void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
+void AP_Camera::control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
 {
     WITH_SEMAPHORE(_rsem);
 
@@ -472,7 +472,7 @@ void AP_Camera::control(float session, float zoom_pos, float zoom_step, float fo
     primary->control(session, zoom_pos, zoom_step, focus_lock, shooting_cmd, cmd_id);
 }
 
-void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
+void AP_Camera::control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
 {
     WITH_SEMAPHORE(_rsem);
 
@@ -526,6 +526,21 @@ void AP_Camera::send_camera_settings(mavlink_channel_t chan)
     }
 }
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+// send camera field of view status
+void AP_Camera::send_camera_fov_status(mavlink_channel_t chan)
+{
+    WITH_SEMAPHORE(_rsem);
+
+    // call each instance
+    for (uint8_t instance = 0; instance < AP_CAMERA_MAX_INSTANCES; instance++) {
+        if (_backends[instance] != nullptr) {
+            _backends[instance]->send_camera_fov_status(chan);
+        }
+    }
+}
+#endif
+
 /*
   update; triggers by distance moved and camera trigger
 */
diff --git a/libraries/AP_Camera/AP_Camera.h b/libraries/AP_Camera/AP_Camera.h
index 80221ac6434ba7..f0f384a1991857 100644
--- a/libraries/AP_Camera/AP_Camera.h
+++ b/libraries/AP_Camera/AP_Camera.h
@@ -84,7 +84,7 @@ class AP_Camera {
     void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg);
 
     // handle MAVLink command from GCS to control the camera
-    MAV_RESULT handle_command_long(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command(const mavlink_command_int_t &packet);
 
     // send camera feedback message to GCS
     void send_feedback(mavlink_channel_t chan);
@@ -95,13 +95,18 @@ class AP_Camera {
     // send camera settings message to GCS
     void send_camera_settings(mavlink_channel_t chan);
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+    // send camera field of view status
+    void send_camera_fov_status(mavlink_channel_t chan);
+#endif
+
     // configure camera
-    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
-    void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time);
+    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time);
+    void configure(uint8_t instance, float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time);
 
     // handle camera control
-    void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
-    void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
+    void control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id);
+    void control(uint8_t instance, float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id);
 
     // set camera trigger distance in a mission
     void set_trigger_distance(float distance_m);
diff --git a/libraries/AP_Camera/AP_Camera_Backend.cpp b/libraries/AP_Camera/AP_Camera_Backend.cpp
index 668053768169a9..abda67057dfae8 100644
--- a/libraries/AP_Camera/AP_Camera_Backend.cpp
+++ b/libraries/AP_Camera/AP_Camera_Backend.cpp
@@ -161,10 +161,10 @@ void AP_Camera_Backend::stop_capture()
 }
 
 // handle camera control
-void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
+void AP_Camera_Backend::control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
 {
     // take picture
-    if (is_equal(shooting_cmd, 1.0f)) {
+    if (shooting_cmd == 1) {
         take_picture();
     }
 }
@@ -247,6 +247,45 @@ void AP_Camera_Backend::send_camera_settings(mavlink_channel_t chan) const
         NaN);               // focusLevel float, percentage from 0 to 100, NaN if unknown
 }
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+// send camera field of view status
+void AP_Camera_Backend::send_camera_fov_status(mavlink_channel_t chan) const
+{
+    // getting corresponding mount instance for camera
+    const AP_Mount* mount = AP::mount();
+    if (mount == nullptr) {
+        return;
+    }
+    Quaternion quat;
+    Location loc;
+    Location poi_loc;
+    if (!mount->get_poi(get_mount_instance(), quat, loc, poi_loc)) {
+        return;
+    }
+    // send camera fov status message only if the last calculated values aren't stale
+    const float NaN = nanf("0x4152");
+    const float quat_array[4] = {
+        quat.q1,
+        quat.q2,
+        quat.q3,
+        quat.q4
+    };
+    mavlink_msg_camera_fov_status_send(
+        chan,
+        AP_HAL::millis(),
+        loc.lat,
+        loc.lng,
+        loc.alt,
+        poi_loc.lat,
+        poi_loc.lng,
+        poi_loc.alt,
+        quat_array,
+        horizontal_fov() > 0 ? horizontal_fov() : NaN,
+        vertical_fov() > 0 ? vertical_fov() : NaN
+    );
+}
+#endif
+
 // setup a callback for a feedback pin. When on PX4 with the right FMU
 // mode we can use the microsecond timer.
 void AP_Camera_Backend::setup_feedback_callback()
diff --git a/libraries/AP_Camera/AP_Camera_Backend.h b/libraries/AP_Camera/AP_Camera_Backend.h
index 5f8f8b1d78fb9c..bae4fb8ea291eb 100644
--- a/libraries/AP_Camera/AP_Camera_Backend.h
+++ b/libraries/AP_Camera/AP_Camera_Backend.h
@@ -85,14 +85,18 @@ class AP_Camera_Backend
     // set camera lens as a value from 0 to 5
     virtual bool set_lens(uint8_t lens) { return false; }
 
+    // get camera image horizontal or vertical field of view in degrees.  returns 0 if unknown
+    float horizontal_fov() const { return MAX(0, _params.hfov); }
+    float vertical_fov() const { return MAX(0, _params.vfov); }
+
     // handle MAVLink messages from the camera
     virtual void handle_message(mavlink_channel_t chan, const mavlink_message_t &msg) {}
 
     // configure camera
-    virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) {}
+    virtual void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) {}
 
     // handle camera control
-    virtual void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id);
+    virtual void control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id);
 
     // set camera trigger distance in meters
     void set_trigger_distance(float distance_m) { _params.trigg_dist.set(distance_m); }
@@ -106,6 +110,11 @@ class AP_Camera_Backend
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const;
 
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+    // send camera field of view status
+    void send_camera_fov_status(mavlink_channel_t chan) const;
+#endif
+
 #if AP_CAMERA_SCRIPTING_ENABLED
     // accessor to allow scripting backend to retrieve state
     // returns true on success and cam_state is filled in
diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.cpp b/libraries/AP_Camera/AP_Camera_MAVLink.cpp
index 85b24b2d88314e..b5bebb91a0944c 100644
--- a/libraries/AP_Camera/AP_Camera_MAVLink.cpp
+++ b/libraries/AP_Camera/AP_Camera_MAVLink.cpp
@@ -19,7 +19,7 @@ bool AP_Camera_MAVLink::trigger_pic()
 }
 
 // configure camera
-void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
+void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
 {
     // convert to mavlink message and send to all components
     mavlink_command_long_t mav_cmd_long = {};
@@ -30,8 +30,8 @@ void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, floa
     mav_cmd_long.param2 = shutter_speed;
     mav_cmd_long.param3 = aperture;
     mav_cmd_long.param4 = ISO;
-    mav_cmd_long.param5 = exposure_type;
-    mav_cmd_long.param6 = cmd_id;
+    mav_cmd_long.param5 = float(exposure_type);
+    mav_cmd_long.param6 = float(cmd_id);
     mav_cmd_long.param7 = engine_cutoff_time;
 
     // send to all components
@@ -39,10 +39,10 @@ void AP_Camera_MAVLink::configure(float shooting_mode, float shutter_speed, floa
 }
 
 // handle camera control message
-void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id)
+void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id)
 {
     // take picture and ignore other arguments
-    if (is_equal(shooting_cmd, 1.0f)) {
+    if (shooting_cmd == 1) {
         take_picture();
         return;
     }
@@ -54,8 +54,8 @@ void AP_Camera_MAVLink::control(float session, float zoom_pos, float zoom_step,
     mav_cmd_long.param2 = zoom_pos;
     mav_cmd_long.param3 = zoom_step;
     mav_cmd_long.param4 = focus_lock;
-    mav_cmd_long.param5 = shooting_cmd;
-    mav_cmd_long.param6 = cmd_id;
+    mav_cmd_long.param5 = float(shooting_cmd);
+    mav_cmd_long.param6 = float(cmd_id);
 
     // send to all components
     GCS_MAVLINK::send_to_components(MAVLINK_MSG_ID_COMMAND_LONG, (char*)&mav_cmd_long, sizeof(mav_cmd_long));
diff --git a/libraries/AP_Camera/AP_Camera_MAVLink.h b/libraries/AP_Camera/AP_Camera_MAVLink.h
index 0be92f09c0729c..396b31f7ac1f37 100644
--- a/libraries/AP_Camera/AP_Camera_MAVLink.h
+++ b/libraries/AP_Camera/AP_Camera_MAVLink.h
@@ -36,10 +36,10 @@ class AP_Camera_MAVLink : public AP_Camera_Backend
     bool trigger_pic() override;
 
     // configure camera
-    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override;
+    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) override;
 
     // handle camera control message
-    void control(float session, float zoom_pos, float zoom_step, float focus_lock, float shooting_cmd, float cmd_id) override;
+    void control(float session, float zoom_pos, float zoom_step, float focus_lock, int32_t shooting_cmd, int32_t cmd_id) override;
 };
 
 #endif // AP_CAMERA_MAVLINK_ENABLED
diff --git a/libraries/AP_Camera/AP_Camera_Params.cpp b/libraries/AP_Camera/AP_Camera_Params.cpp
index ee3f7796590ab7..7b32ca60cf1bb8 100644
--- a/libraries/AP_Camera/AP_Camera_Params.cpp
+++ b/libraries/AP_Camera/AP_Camera_Params.cpp
@@ -87,6 +87,22 @@ const AP_Param::GroupInfo AP_Camera_Params::var_info[] = {
     // @User: Standard
     AP_GROUPINFO("_MNT_INST", 11, AP_Camera_Params, mount_instance, 0),
 
+    // @Param: _HFOV
+    // @DisplayName: Camera horizontal field of view
+    // @Description: Camera horizontal field of view. 0 if unknown
+    // @Units: deg
+    // @Range: 0 360
+    // @User: Standard
+    AP_GROUPINFO("_HFOV", 12, AP_Camera_Params, hfov, 0),
+
+    // @Param: _VFOV
+    // @DisplayName: Camera vertical field of view
+    // @Description: Camera vertical field of view. 0 if unknown
+    // @Units: deg
+    // @Range: 0 180
+    // @User: Standard
+    AP_GROUPINFO("_VFOV", 13, AP_Camera_Params, vfov, 0),
+
     AP_GROUPEND
 
 };
diff --git a/libraries/AP_Camera/AP_Camera_Params.h b/libraries/AP_Camera/AP_Camera_Params.h
index ca54550fe1ddbe..cba713a16319ea 100644
--- a/libraries/AP_Camera/AP_Camera_Params.h
+++ b/libraries/AP_Camera/AP_Camera_Params.h
@@ -23,6 +23,8 @@ class AP_Camera_Params {
     AP_Float interval_min;      // minimum time (in seconds) between shots required by camera
     AP_Int8 options;            // whether to start recording when armed and stop when disarmed
     AP_Int8 mount_instance;     // mount instance to which camera is associated with
+    AP_Float hfov;              // horizontal field of view in degrees
+    AP_Float vfov;              // vertical field of view in degrees
 
     // pin number for accurate camera feedback messages
     AP_Int8 feedback_pin;
diff --git a/libraries/AP_Camera/AP_Camera_Servo.cpp b/libraries/AP_Camera/AP_Camera_Servo.cpp
index b297576d0347f2..80c3d7d7ba41f9 100644
--- a/libraries/AP_Camera/AP_Camera_Servo.cpp
+++ b/libraries/AP_Camera/AP_Camera_Servo.cpp
@@ -44,7 +44,7 @@ bool AP_Camera_Servo::trigger_pic()
 }
 
 // configure camera
-void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time)
+void AP_Camera_Servo::configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time)
 {
     // designed to control Blackmagic Micro Cinema Camera (BMMCC) cameras
     // if the message contains non zero values then use them for the below functions
diff --git a/libraries/AP_Camera/AP_Camera_Servo.h b/libraries/AP_Camera/AP_Camera_Servo.h
index bd2a585b8e0d29..7a9afa25d3cb37 100644
--- a/libraries/AP_Camera/AP_Camera_Servo.h
+++ b/libraries/AP_Camera/AP_Camera_Servo.h
@@ -39,7 +39,7 @@ class AP_Camera_Servo : public AP_Camera_Backend
     bool trigger_pic() override;
 
     // configure camera
-    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, float exposure_type, float cmd_id, float engine_cutoff_time) override;
+    void configure(float shooting_mode, float shutter_speed, float aperture, float ISO, int32_t exposure_type, int32_t cmd_id, float engine_cutoff_time) override;
 
 private:
 
diff --git a/libraries/AP_Camera/AP_Camera_config.h b/libraries/AP_Camera/AP_Camera_config.h
index 565c344f31e808..b153c479639486 100644
--- a/libraries/AP_Camera/AP_Camera_config.h
+++ b/libraries/AP_Camera/AP_Camera_config.h
@@ -9,6 +9,10 @@
 #define AP_CAMERA_ENABLED 1
 #endif
 
+#ifndef AP_CAMERA_SEND_FOV_STATUS_ENABLED
+#define AP_CAMERA_SEND_FOV_STATUS_ENABLED AP_MOUNT_POI_TO_LATLONALT_ENABLED
+#endif
+
 #ifndef AP_CAMERA_BACKEND_DEFAULT_ENABLED
 #define AP_CAMERA_BACKEND_DEFAULT_ENABLED AP_CAMERA_ENABLED
 #endif
diff --git a/libraries/AP_DroneCAN/AP_Canard_iface.cpp b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
index 540120564d8075..5fa96606f5be43 100644
--- a/libraries/AP_DroneCAN/AP_Canard_iface.cpp
+++ b/libraries/AP_DroneCAN/AP_Canard_iface.cpp
@@ -214,6 +214,20 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
         if (txq == nullptr) {
             return;
         }
+        // volatile as the value can change at any time during can interrupt
+        // we need to ensure that this is not optimized
+        volatile const auto *stats = ifaces[iface]->get_statistics();
+        uint64_t last_transmit_us = stats->last_transmit_us;
+        bool iface_down = true;
+        if (stats == nullptr || (AP_HAL::micros64() - last_transmit_us) < 200000UL) {
+            /*
+            We were not able to queue the frame for
+            sending. Only mark the send as failing if the
+            interface is active. We consider an interface as
+            active if it has had successful transmits for some time.
+            */
+            iface_down = false;
+        } 
         // scan through list of pending transfers
         while (true) {
             auto txf = &txq->frame;
@@ -240,15 +254,22 @@ void CanardInterface::processTx(bool raw_commands_only = false) {
             if (!write) {
                 // if there is no space then we need to start from the
                 // top of the queue, so wait for the next loop
-                break;
-            }
-            if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
+                if (!iface_down) {
+                    break;
+                } else {
+                    txf->iface_mask &= ~(1U<<iface);
+                }
+            } else if ((txf->iface_mask & (1U<<iface)) && (AP_HAL::micros64() < txf->deadline_usec)) {
                 // try sending to interfaces, clearing the mask if we succeed
                 if (ifaces[iface]->send(txmsg, txf->deadline_usec, 0) > 0) {
                     txf->iface_mask &= ~(1U<<iface);
                 } else {
                     // if we fail to send then we try sending on next interface
-                    break;
+                    if (!iface_down) {
+                        break;
+                    } else {
+                        txf->iface_mask &= ~(1U<<iface);
+                    }
                 }
             }
             // look at next transfer
diff --git a/libraries/AP_GPS/AP_GPS.cpp b/libraries/AP_GPS/AP_GPS.cpp
index a085c021f283cc..0a5f3a7b6e1f7c 100644
--- a/libraries/AP_GPS/AP_GPS.cpp
+++ b/libraries/AP_GPS/AP_GPS.cpp
@@ -343,7 +343,7 @@ const AP_Param::GroupInfo AP_GPS::var_info[] = {
     // @Units: s
     // @Range: 5.0 30.0
     // @User: Advanced
-    AP_GROUPINFO("_BLEND_TC", 21, AP_GPS, _blend_tc, 10.0f),
+    // Had key 21, no longer used
 #endif
 
     // @Param: _DRV_OPTIONS
@@ -485,11 +485,6 @@ void AP_GPS::init(const AP_SerialManager& serial_manager)
     }
     _last_instance_swap_ms = 0;
 
-#if defined(GPS_BLENDED_INSTANCE)
-    // Initialise class variables used to do GPS blending
-    _omega_lpf = 1.0f / constrain_float(_blend_tc, 5.0f, 30.0f);
-#endif
-
     // prep the state instance fields
     for (uint8_t i = 0; i < GPS_MAX_INSTANCES; i++) {
         state[i].instance = i;
diff --git a/libraries/AP_GPS/AP_GPS.h b/libraries/AP_GPS/AP_GPS.h
index a097e25138ee29..4b184dcde0a827 100644
--- a/libraries/AP_GPS/AP_GPS.h
+++ b/libraries/AP_GPS/AP_GPS.h
@@ -605,7 +605,6 @@ class AP_GPS
     AP_Int16 _delay_ms[GPS_MAX_RECEIVERS];
     AP_Int8  _com_port[GPS_MAX_RECEIVERS];
     AP_Int8 _blend_mask;
-    AP_Float _blend_tc;
     AP_Int16 _driver_options;
     AP_Int8 _primary;
 #if HAL_ENABLE_DRONECAN_DRIVERS
@@ -739,7 +738,6 @@ class AP_GPS
     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.
-    float _omega_lpf; // cutoff frequency in rad/sec of LPF applied to position offsets
     bool _output_is_blended; // true when a blended GPS solution being output
     uint8_t _blend_health_counter;  // 0 = perfectly health, 100 = very unhealthy
 
diff --git a/libraries/AP_HAL/SIMState.cpp b/libraries/AP_HAL/SIMState.cpp
index 776e56ef7402d8..9a270445abf71c 100644
--- a/libraries/AP_HAL/SIMState.cpp
+++ b/libraries/AP_HAL/SIMState.cpp
@@ -99,6 +99,7 @@ void SIMState::fdm_input_local(void)
     // ride_along.receive(input);
 
     // update the model
+    sitl_model->update_home();
     sitl_model->update_model(input);
 
     // get FDM output from the model
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/README.md b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/README.md
index 87f58272ae8af7..a5f52fdd550d87 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/JFB110/README.md
+++ b/libraries/AP_HAL_ChibiOS/hwdef/JFB110/README.md
@@ -113,8 +113,8 @@ The JFB-110 has 9 analog inputs
  - ADC Pin5  -> ADC 5V Sense
  - ADC Pin11 -> ADC 3.3V Sense
  - ADC Pin10 -> RSSI voltage monitoring
- - ADC Pin12 -> ADC SPARE 1
- - ADC Pin13 -> ADC SPARE 2
+ - ADC Pin13 -> ADC SPARE 1
+ - ADC Pin12 -> ADC SPARE 2
  
 ## I2C Buses
 
diff --git a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
index 4d2cb796a01162..f72214aa46d697 100644
--- a/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_Periph_State.cpp
@@ -294,6 +294,7 @@ SimMCast::SimMCast(const char *frame_str) :
 void SimMCast::update(const struct sitl_input &input)
 {
     multicast_read();
+    update_home();
     update_external_payload(input);
 
     auto *_sitl = AP::sitl();
diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp
index 8c3af4c856bd64..361b837cf1dbef 100644
--- a/libraries/AP_HAL_SITL/SITL_State.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State.cpp
@@ -346,6 +346,7 @@ void SITL_State::_fdm_input_local(void)
     multicast_servo_update(input);
 
     // update the model
+    sitl_model->update_home();
     sitl_model->update_model(input);
 
     // get FDM output from the model
diff --git a/libraries/AP_HAL_SITL/SITL_State_common.cpp b/libraries/AP_HAL_SITL/SITL_State_common.cpp
index 395c66b1be0791..320fcf9615f78c 100644
--- a/libraries/AP_HAL_SITL/SITL_State_common.cpp
+++ b/libraries/AP_HAL_SITL/SITL_State_common.cpp
@@ -216,6 +216,10 @@ SITL::SerialDevice *SITL_State_Common::create_serial_sim(const char *name, const
             AP_HAL::panic("Only one sagetech_mxs at a time");
         }
         sagetech_mxs = new SITL::ADSB_Sagetech_MXS();
+        if (adsb == nullptr) {
+            adsb = new SITL::ADSB();
+        }
+        sitl_model->set_adsb(adsb);
         return sagetech_mxs;
 #endif
 #if !defined(HAL_BUILD_AP_PERIPH)
diff --git a/libraries/AP_Mount/AP_Mount.cpp b/libraries/AP_Mount/AP_Mount.cpp
index c7f1701fbfe9e2..ebcf2a0f8cbddd 100644
--- a/libraries/AP_Mount/AP_Mount.cpp
+++ b/libraries/AP_Mount/AP_Mount.cpp
@@ -609,6 +609,18 @@ void AP_Mount::send_gimbal_manager_status(mavlink_channel_t chan)
 }
 #endif  // HAL_GCS_ENABLED
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+// get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+bool AP_Mount::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const
+{
+    auto *backend = get_instance(instance);
+    if (backend == nullptr) {
+        return false;
+    }
+    return backend->get_poi(instance, quat, loc, poi_loc);
+}
+#endif
+
 // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
 // returns true on success
 bool AP_Mount::get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg)
diff --git a/libraries/AP_Mount/AP_Mount.h b/libraries/AP_Mount/AP_Mount.h
index 7e8dc661d60289..1dec3ab9d50f9d 100644
--- a/libraries/AP_Mount/AP_Mount.h
+++ b/libraries/AP_Mount/AP_Mount.h
@@ -195,6 +195,11 @@ class AP_Mount
     // send a GIMBAL_MANAGER_STATUS message to GCS
     void send_gimbal_manager_status(mavlink_channel_t chan);
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc) const;
+#endif
+
     // get mount's current attitude in euler angles in degrees.  yaw angle is in body-frame
     // returns true on success
     bool get_attitude_euler(uint8_t instance, float& roll_deg, float& pitch_deg, float& yaw_bf_deg);
diff --git a/libraries/AP_Mount/AP_Mount_Backend.cpp b/libraries/AP_Mount/AP_Mount_Backend.cpp
index c50ab5a5d5e7b5..ee4c76d6255f54 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.cpp
+++ b/libraries/AP_Mount/AP_Mount_Backend.cpp
@@ -3,16 +3,29 @@
 #include <AP_AHRS/AP_AHRS.h>
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Logger/AP_Logger.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 extern const AP_HAL::HAL& hal;
 
 #define AP_MOUNT_UPDATE_DT 0.02     // update rate in seconds.  update() should be called at this rate
+#define AP_MOUNT_POI_REQUEST_TIMEOUT_MS 30000   // POI calculations continue to be updated for this many seconds after last request
+#define AP_MOUNT_POI_RESULT_TIMEOUT_MS  3000    // POI calculations valid for 3 seconds
+#define AP_MOUNT_POI_DIST_M_MAX         10000   // POI calculations limit of 10,000m (10km)
 
 // Default init function for every mount
 void AP_Mount_Backend::init()
 {
     // setting default target sysid from parameters
     _target_sysid = _params.sysid_default.get();
+
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // create a calculation thread for poi.
+    if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Mount_Backend::calculate_poi, void),
+                                      "mount_calc_poi",
+                                      8192, AP_HAL::Scheduler::PRIORITY_IO, -1)) {
+        GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Mount: failed to start POI thread");
+    }
+#endif
 }
 
 // set device id of this instance, for MNTx_DEVID parameter
@@ -414,6 +427,127 @@ void AP_Mount_Backend::write_log(uint64_t timestamp_us)
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+// get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+bool AP_Mount_Backend::get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc)
+{
+    WITH_SEMAPHORE(poi_calculation.sem);
+
+    // record time of request
+    const uint32_t now_ms = AP_HAL::millis();
+    poi_calculation.poi_request_ms = now_ms;
+
+    // check if poi calculated recently
+    if (now_ms - poi_calculation.poi_update_ms > AP_MOUNT_POI_RESULT_TIMEOUT_MS) {
+        return false;
+    }
+
+    // check attitude is valid
+    if (poi_calculation.att_quat.is_nan()) {
+        return false;
+    }
+
+    quat = poi_calculation.att_quat;
+    loc = poi_calculation.loc;
+    poi_loc = poi_calculation.poi_loc;
+    return true;
+}
+
+// calculate the Location that the gimbal is pointing at
+void AP_Mount_Backend::calculate_poi()
+{
+    while (true) {
+        // run this loop at 10hz
+        hal.scheduler->delay(100);
+
+        // calculate poi if requested within last 30 seconds
+        {
+            WITH_SEMAPHORE(poi_calculation.sem);
+            if ((poi_calculation.poi_request_ms == 0) ||
+                (AP_HAL::millis() - poi_calculation.poi_request_ms > AP_MOUNT_POI_REQUEST_TIMEOUT_MS)) {
+                continue;
+            }
+        }
+
+        // get the current location of vehicle
+        const AP_AHRS &ahrs = AP::ahrs();
+        Location curr_loc;
+        if (!ahrs.get_location(curr_loc)) {
+            continue;
+        }
+
+        // change vehicle alt to AMSL
+        curr_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
+
+        // project forward from vehicle looking for terrain
+        // start testing at vehicle's location
+        Location test_loc = curr_loc;
+        Location prev_test_loc = curr_loc;
+
+        // get terrain altitude (AMSL) at test_loc
+        auto terrain = AP_Terrain::get_singleton();
+        float terrain_amsl_m;
+        if ((terrain == nullptr) || !terrain->height_amsl(test_loc, terrain_amsl_m, true)) {
+            continue;
+        }
+
+        // retrieve gimbal attitude
+        Quaternion quat;
+        if (!get_attitude_quaternion(quat)) {
+            // gimbal attitude unavailable
+            continue;
+        }
+
+        // iteratively move test_loc forward until its alt-above-sea-level is below terrain-alt-above-sea-level
+        const float dist_increment_m = MAX(terrain->get_grid_spacing(), 10);
+        const float mount_pitch_deg = degrees(quat.get_euler_pitch());
+        const float mount_yaw_ef_deg = wrap_180(degrees(quat.get_euler_yaw()) + degrees(ahrs.get_yaw()));
+        float total_dist_m = 0;
+        bool get_terrain_alt_success = true;
+        float prev_terrain_amsl_m = terrain_amsl_m;
+        while (total_dist_m < AP_MOUNT_POI_DIST_M_MAX && (test_loc.alt * 0.01) > terrain_amsl_m) {
+            total_dist_m += dist_increment_m;
+
+            // backup previous test location and terrain amsl
+            prev_test_loc = test_loc;
+            prev_terrain_amsl_m = terrain_amsl_m;
+
+            // move test location forward
+            test_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_increment_m);
+
+            // get terrain's alt-above-sea-level (at test_loc)
+            // fail if terrain alt cannot be retrieved
+            if (!terrain->height_amsl(test_loc, terrain_amsl_m, true) || std::isnan(terrain_amsl_m)) {
+                get_terrain_alt_success = false;
+                continue;
+            }
+        }
+
+        // if a fail occurred above when getting terrain alt then restart calculations from the beginning
+        if (!get_terrain_alt_success) {
+            continue;
+        }
+
+        if (total_dist_m >= AP_MOUNT_POI_DIST_M_MAX) {
+            // unable to find terrain within dist_max
+            continue;
+        }
+
+        // test location has dropped below terrain
+        // interpolate along line between prev_test_loc and test_loc
+        float dist_interp_m = linear_interpolate(0, dist_increment_m, 0, prev_test_loc.alt * 0.01 - prev_terrain_amsl_m, test_loc.alt * 0.01 - terrain_amsl_m);
+        {
+            WITH_SEMAPHORE(poi_calculation.sem);
+            poi_calculation.poi_loc = prev_test_loc;
+            poi_calculation.poi_loc.offset_bearing_and_pitch(mount_yaw_ef_deg, mount_pitch_deg, dist_interp_m);
+            poi_calculation.att_quat = {quat[0], quat[1], quat[2], quat[3]};
+            poi_calculation.loc = curr_loc;
+            poi_calculation.poi_update_ms = AP_HAL::millis();
+        }
+    }
+}
+#endif
+
 // get pilot input (in the range -1 to +1) received through RC
 void AP_Mount_Backend::get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const
 {
diff --git a/libraries/AP_Mount/AP_Mount_Backend.h b/libraries/AP_Mount/AP_Mount_Backend.h
index 7e5563f44e210b..fbd2ca0a801044 100644
--- a/libraries/AP_Mount/AP_Mount_Backend.h
+++ b/libraries/AP_Mount/AP_Mount_Backend.h
@@ -182,6 +182,11 @@ class AP_Mount_Backend
     // send camera settings message to GCS
     virtual void send_camera_settings(mavlink_channel_t chan) const {}
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // get poi information.  Returns true on success and fills in gimbal attitude, location and poi location
+    bool get_poi(uint8_t instance, Quaternion &quat, Location &loc, Location &poi_loc);
+#endif
+
     //
     // rangefinder
     //
@@ -221,6 +226,11 @@ class AP_Mount_Backend
     // returns true if mavlink heartbeat should be suppressed for this gimbal (only used by Solo gimbal)
     virtual bool suppress_heartbeat() const { return false; }
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    // calculate the Location that the gimbal is pointing at
+    void calculate_poi();
+#endif
+
     // get pilot input (in the range -1 to +1) received through RC
     void get_rc_input(float& roll_in, float& pitch_in, float& yaw_in) const;
 
@@ -269,6 +279,17 @@ class AP_Mount_Backend
         MountTarget rate_rads;      // rate target in rad/s
     } mnt_target;
 
+#if AP_MOUNT_POI_TO_LATLONALT_ENABLED
+    struct {
+        HAL_Semaphore sem;        // semaphore protecting this structure
+        uint32_t poi_request_ms;  // system time POI was last requested
+        uint32_t poi_update_ms;   // system time POI was calculated
+        Location loc;             // gimbal location used for poi calculation
+        Location poi_loc;         // location of the POI
+        Quaternion att_quat;      // attitude quaternion of the gimbal
+    } poi_calculation;
+#endif
+
     Location _roi_target;           // roi target location
     bool _roi_target_set;           // true if the roi target has been set
 
diff --git a/libraries/AP_Mount/AP_Mount_config.h b/libraries/AP_Mount/AP_Mount_config.h
index 3195a5ce387d47..9159653dcc79e7 100644
--- a/libraries/AP_Mount/AP_Mount_config.h
+++ b/libraries/AP_Mount/AP_Mount_config.h
@@ -1,6 +1,7 @@
 #pragma once
 
 #include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_Terrain/AP_Terrain.h>
 
 #ifndef HAL_MOUNT_ENABLED
 #define HAL_MOUNT_ENABLED 1
@@ -48,3 +49,7 @@
 #ifndef HAL_MOUNT_VIEWPRO_ENABLED
 #define HAL_MOUNT_VIEWPRO_ENABLED AP_MOUNT_BACKEND_DEFAULT_ENABLED && BOARD_FLASH_SIZE > 1024
 #endif
+
+#ifndef AP_MOUNT_POI_TO_LATLONALT_ENABLED
+#define AP_MOUNT_POI_TO_LATLONALT_ENABLED HAL_MOUNT_ENABLED && AP_TERRAIN_AVAILABLE && BOARD_FLASH_SIZE > 1024
+#endif
diff --git a/libraries/AP_Terrain/AP_Terrain.h b/libraries/AP_Terrain/AP_Terrain.h
index 2321ba1350607c..ff55ce0dbbcb05 100644
--- a/libraries/AP_Terrain/AP_Terrain.h
+++ b/libraries/AP_Terrain/AP_Terrain.h
@@ -182,6 +182,11 @@ class AP_Terrain {
      */
     void get_statistics(uint16_t &pending, uint16_t &loaded) const;
 
+    /*
+      get grid spacing in meters
+     */
+    uint16_t get_grid_spacing() const { return MAX(grid_spacing, 0); };
+
     /*
       returns true if initialisation failed because out-of-memory
      */
diff --git a/libraries/AP_Terrain/TerrainGCS.cpp b/libraries/AP_Terrain/TerrainGCS.cpp
index 8ad7ae352e2ce2..30a2067c29b5db 100644
--- a/libraries/AP_Terrain/TerrainGCS.cpp
+++ b/libraries/AP_Terrain/TerrainGCS.cpp
@@ -196,7 +196,6 @@ void AP_Terrain::get_statistics(uint16_t &pending, uint16_t &loaded) const
     }
 }
 
-
 /* 
    handle terrain messages from GCS
  */
diff --git a/libraries/GCS_MAVLink/GCS.h b/libraries/GCS_MAVLink/GCS.h
index a505f760b9c49e..7ba2c0c2c8b460 100644
--- a/libraries/GCS_MAVLink/GCS.h
+++ b/libraries/GCS_MAVLink/GCS.h
@@ -654,7 +654,7 @@ class GCS_MAVLINK
     virtual bool mav_frame_for_command_long(MAV_FRAME &fame, MAV_CMD packet_command) const;
     MAV_RESULT try_command_long_as_command_int(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
     MAV_RESULT handle_command_long_packet(const mavlink_command_long_t &packet, const mavlink_message_t &msg);
-    MAV_RESULT handle_command_camera(const mavlink_command_long_t &packet);
+    MAV_RESULT handle_command_camera(const mavlink_command_int_t &packet);
     MAV_RESULT handle_command_do_set_roi(const mavlink_command_int_t &packet);
     virtual MAV_RESULT handle_command_do_set_roi(const Location &roi_loc);
     MAV_RESULT handle_command_do_gripper(const mavlink_command_long_t &packet);
diff --git a/libraries/GCS_MAVLink/GCS_Common.cpp b/libraries/GCS_MAVLink/GCS_Common.cpp
index d5c407ed2fa873..0cff164fd2944f 100644
--- a/libraries/GCS_MAVLink/GCS_Common.cpp
+++ b/libraries/GCS_MAVLink/GCS_Common.cpp
@@ -998,6 +998,7 @@ ap_message GCS_MAVLINK::mavlink_id_to_ap_message_id(const uint32_t mavlink_id) c
         { MAVLINK_MSG_ID_CAMERA_FEEDBACK,       MSG_CAMERA_FEEDBACK},
         { MAVLINK_MSG_ID_CAMERA_INFORMATION,    MSG_CAMERA_INFORMATION},
         { MAVLINK_MSG_ID_CAMERA_SETTINGS,       MSG_CAMERA_SETTINGS},
+        { MAVLINK_MSG_ID_CAMERA_FOV_STATUS,     MSG_CAMERA_FOV_STATUS},
 #endif
 #if HAL_MOUNT_ENABLED
         { MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, MSG_GIMBAL_DEVICE_ATTITUDE_STATUS},
@@ -3449,14 +3450,14 @@ void GCS_MAVLINK::handle_system_time_message(const mavlink_message_t &msg)
 #endif
 
 #if AP_CAMERA_ENABLED
-MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_long_t &packet)
+MAV_RESULT GCS_MAVLINK::handle_command_camera(const mavlink_command_int_t &packet)
 {
     AP_Camera *camera = AP::camera();
     if (camera == nullptr) {
         return MAV_RESULT_UNSUPPORTED;
     }
 
-    return camera->handle_command_long(packet);
+    return camera->handle_command(packet);
 }
 #endif
 
@@ -4352,10 +4353,11 @@ MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration_baro(const mavlink
             return MAV_RESULT_TEMPORARILY_REJECTED;
         }
         airspeed->calibrate(false);
+        return MAV_RESULT_IN_PROGRESS;
     }
 #endif
 
-    return MAV_RESULT_IN_PROGRESS;
+    return MAV_RESULT_ACCEPTED;
 }
 
 MAV_RESULT GCS_MAVLINK::_handle_command_preflight_calibration(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
@@ -4724,22 +4726,6 @@ MAV_RESULT GCS_MAVLINK::handle_command_long_packet(const mavlink_command_long_t
 
     switch (packet.command) {
 
-#if AP_CAMERA_ENABLED
-    case MAV_CMD_DO_DIGICAM_CONFIGURE:
-    case MAV_CMD_DO_DIGICAM_CONTROL:
-    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
-    case MAV_CMD_SET_CAMERA_ZOOM:
-    case MAV_CMD_SET_CAMERA_FOCUS:
-    case MAV_CMD_IMAGE_START_CAPTURE:
-    case MAV_CMD_IMAGE_STOP_CAPTURE:
-    case MAV_CMD_CAMERA_TRACK_POINT:
-    case MAV_CMD_CAMERA_TRACK_RECTANGLE:
-    case MAV_CMD_CAMERA_STOP_TRACKING:
-    case MAV_CMD_VIDEO_START_CAPTURE:
-    case MAV_CMD_VIDEO_STOP_CAPTURE:
-        result = handle_command_camera(packet);
-        break;
-#endif
 #if AP_GRIPPER_ENABLED
     case MAV_CMD_DO_GRIPPER:
         result = handle_command_do_gripper(packet);
@@ -5036,6 +5022,7 @@ MAV_RESULT GCS_MAVLINK::handle_command_storage_format(const mavlink_command_int_
 MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &packet, const mavlink_message_t &msg)
 {
     switch (packet.command) {
+
 #if HAL_INS_ACCELCAL_ENABLED
     case MAV_CMD_ACCELCAL_VEHICLE_POS:
         return handle_command_accelcal_vehicle_pos(packet);
@@ -5072,6 +5059,22 @@ MAV_RESULT GCS_MAVLINK::handle_command_int_packet(const mavlink_command_int_t &p
     case MAV_CMD_DO_SET_MODE:
         return handle_command_do_set_mode(packet);
 
+#if AP_CAMERA_ENABLED
+    case MAV_CMD_DO_DIGICAM_CONFIGURE:
+    case MAV_CMD_DO_DIGICAM_CONTROL:
+    case MAV_CMD_DO_SET_CAM_TRIGG_DIST:
+    case MAV_CMD_SET_CAMERA_ZOOM:
+    case MAV_CMD_SET_CAMERA_FOCUS:
+    case MAV_CMD_IMAGE_START_CAPTURE:
+    case MAV_CMD_IMAGE_STOP_CAPTURE:
+    case MAV_CMD_CAMERA_TRACK_POINT:
+    case MAV_CMD_CAMERA_TRACK_RECTANGLE:
+    case MAV_CMD_CAMERA_STOP_TRACKING:
+    case MAV_CMD_VIDEO_START_CAPTURE:
+    case MAV_CMD_VIDEO_STOP_CAPTURE:
+        return handle_command_camera(packet);
+#endif
+
     case MAV_CMD_DO_SET_ROI_NONE: {
         const Location zero_loc = Location();
         return handle_command_do_set_roi(zero_loc);
@@ -5773,6 +5776,18 @@ bool GCS_MAVLINK::try_send_message(const enum ap_message id)
             camera->send_camera_settings(chan);
         }
         break;
+#if AP_CAMERA_SEND_FOV_STATUS_ENABLED
+    case MSG_CAMERA_FOV_STATUS:
+        {
+            AP_Camera *camera = AP::camera();
+            if (camera == nullptr) {
+                break;
+            }
+            CHECK_PAYLOAD_SIZE(CAMERA_FOV_STATUS);
+            camera->send_camera_fov_status(chan);
+        }
+        break;
+#endif
 #endif
 
     case MSG_SYSTEM_TIME:
diff --git a/libraries/GCS_MAVLink/ap_message.h b/libraries/GCS_MAVLink/ap_message.h
index 6b4fbb0e295878..3a707bc4628d82 100644
--- a/libraries/GCS_MAVLink/ap_message.h
+++ b/libraries/GCS_MAVLink/ap_message.h
@@ -51,6 +51,7 @@ enum ap_message : uint8_t {
     MSG_CAMERA_FEEDBACK,
     MSG_CAMERA_INFORMATION,
     MSG_CAMERA_SETTINGS,
+    MSG_CAMERA_FOV_STATUS,
     MSG_GIMBAL_DEVICE_ATTITUDE_STATUS,
     MSG_GIMBAL_MANAGER_INFORMATION,
     MSG_GIMBAL_MANAGER_STATUS,
diff --git a/libraries/SITL/SIM_ADSB.cpp b/libraries/SITL/SIM_ADSB.cpp
index b0f48d88e46824..784d032902642a 100644
--- a/libraries/SITL/SIM_ADSB.cpp
+++ b/libraries/SITL/SIM_ADSB.cpp
@@ -35,19 +35,26 @@ namespace SITL {
 /*
   update a simulated vehicle
  */
-void ADSB_Vehicle::update(float delta_t)
+void ADSB_Vehicle::update(const class Aircraft &aircraft, float delta_t)
 {
-    if (!initialised) {
-        const SIM *_sitl = AP::sitl();
-        if (_sitl == nullptr) {
-            return;
-        }
+    const SIM *_sitl = AP::sitl();
+    if (_sitl == nullptr) {
+        return;
+    }
+
+    const Location &origin { aircraft.get_origin() };
 
+    if (!initialised) {
+        // spawn another aircraft
         initialised = true;
         ICAO_address = (uint32_t)(rand() % 10000);
         snprintf(callsign, sizeof(callsign), "SIM%u", ICAO_address);
-        position.x = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
-        position.y = Aircraft::rand_normal(0, _sitl->adsb_radius_m);
+        Location aircraft_location = aircraft.get_location();
+        const Vector2f aircraft_offset_ne = aircraft_location.get_distance_NE(origin);
+        position.x = aircraft_offset_ne[1];
+        position.y = aircraft_offset_ne[0];
+        position.x += Aircraft::rand_normal(0, _sitl->adsb_radius_m);
+        position.y += Aircraft::rand_normal(0, _sitl->adsb_radius_m);
         position.z = -fabsf(_sitl->adsb_altitude_m);
 
         double vel_min = 5, vel_max = 20;
@@ -81,11 +88,16 @@ void ADSB_Vehicle::update(float delta_t)
         // it has crashed! reset
         initialised = false;
     }
+
+    Location ret = origin;
+    ret.offset(position.x, position.y);
+
+    location = ret;
 }
 
-bool ADSB_Vehicle::location(Location &loc) const
+const Location &ADSB_Vehicle::get_location() const
 {
-    return AP::ahrs().get_location_from_home_offset(loc, position);
+    return location;
 }
 
 /*
@@ -115,18 +127,15 @@ void ADSB::update_simulated_vehicles(const class Aircraft &aircraft)
     float delta_t = (now_us - last_update_us) * 1.0e-6f;
     last_update_us = now_us;
 
-    const Location &home = aircraft.get_home();
+    // prune any aircraft which get too far away from our simulated vehicle:
+    const Location &aircraft_loc = aircraft.get_location();
 
     for (uint8_t i=0; i<num_vehicles; i++) {
-        vehicles[i].update(delta_t);
-
-        Location loc = home;
-
-        ADSB_Vehicle &vehicle = vehicles[i];
-        loc.offset(vehicle.position.x, vehicle.position.y);
+        auto &vehicle = vehicles[i];
+        vehicle.update(aircraft, delta_t);
 
         // re-init when exceeding radius range
-        if (home.get_distance(loc) > _sitl->adsb_radius_m) {
+        if (aircraft_loc.get_distance(vehicle.get_location()) > _sitl->adsb_radius_m) {
             vehicle.initialised = false;
         }
     }
@@ -216,8 +225,6 @@ void ADSB::send_report(const class Aircraft &aircraft)
     /*
       send a ADSB_VEHICLE messages
      */
-    const Location &home = aircraft.get_home();
-
     uint32_t now_us = AP_HAL::micros();
     if (now_us - last_report_us >= reporting_period_ms*1000UL) {
         for (uint8_t i=0; i<num_vehicles; i++) {
@@ -226,8 +233,7 @@ void ADSB::send_report(const class Aircraft &aircraft)
                 continue;
             }
 
-            Location loc = home;
-            loc.offset(vehicle.position.x, vehicle.position.y);
+            const Location &loc { vehicle.get_location() };
 
             mavlink_adsb_vehicle_t adsb_vehicle {};
             last_report_us = now_us;
diff --git a/libraries/SITL/SIM_ADSB.h b/libraries/SITL/SIM_ADSB.h
index 4ac66f287ed6a9..9b9682c15b515a 100644
--- a/libraries/SITL/SIM_ADSB.h
+++ b/libraries/SITL/SIM_ADSB.h
@@ -37,7 +37,7 @@ class ADSB_Vehicle {
 public:
     bool initialised = false;
 
-    bool location(class Location &) const;  // return vehicle absolute location
+    const Location &get_location() const;  // return vehicle absolute location
     // return earth-frame vehicle velocity:
     bool velocity(Vector3F &ret) const { ret = velocity_ef; return true; }
 
@@ -46,11 +46,14 @@ class ADSB_Vehicle {
     char callsign[9];
 
 private:
-    void update(float delta_t);
-    
+    void update(const class Aircraft &aircraft, float delta_t);
+
     Vector3p position; // NED from origin
+    Location location;
+
     ADSB_EMITTER_TYPE type;
     uint64_t stationary_object_created_ms; // allows expiring of slow/stationary objects
+
 };
 
 class ADSB : public SerialDevice {
diff --git a/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp b/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp
index 53c36c04f99d2f..88a76717d21b97 100644
--- a/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp
+++ b/libraries/SITL/SIM_ADSB_Sagetech_MXS.cpp
@@ -429,10 +429,7 @@ void ADSB_Sagetech_MXS::send_vehicle_message_state_vector(const SITL::ADSB_Vehic
     pack_int32_into_uint8_ts(vehicle.ICAO_address, my_report.participant_address);
     my_report.address_qualifier = 0x02;  // aircraft
 
-    Location loc;
-    if (!vehicle.location(loc)) {
-        return;
-    }
+    Location loc = vehicle.get_location();
 
     // pack in latitude/longitude:
     pack_scaled_geocoord(my_report.latitude, loc.lat * 1e-7);
diff --git a/libraries/SITL/SIM_Aircraft.cpp b/libraries/SITL/SIM_Aircraft.cpp
index 1632e5d9e969cd..07d0f6064e66ec 100644
--- a/libraries/SITL/SIM_Aircraft.cpp
+++ b/libraries/SITL/SIM_Aircraft.cpp
@@ -590,7 +590,7 @@ void Aircraft::set_speedup(float speedup)
     setup_frame_time(rate_hz, speedup);
 }
 
-void Aircraft::update_model(const struct sitl_input &input)
+void Aircraft::update_home()
 {
     if (!home_is_set) {
         if (sitl == nullptr) {
@@ -602,6 +602,10 @@ void Aircraft::update_model(const struct sitl_input &input)
         loc.alt = sitl->opos.alt.get() * 1.0e2;
         set_start_location(loc, sitl->opos.hdg.get());
     }
+}
+
+void Aircraft::update_model(const struct sitl_input &input)
+{
     local_ground_level = 0.0f;
     update(input);
 }
diff --git a/libraries/SITL/SIM_Aircraft.h b/libraries/SITL/SIM_Aircraft.h
index dd0836fefcbcb3..1c867d56c11286 100644
--- a/libraries/SITL/SIM_Aircraft.h
+++ b/libraries/SITL/SIM_Aircraft.h
@@ -83,6 +83,8 @@ class Aircraft {
 
     void update_model(const struct sitl_input &input);
 
+    void update_home();
+
     /* fill a sitl_fdm structure from the simulator state */
     void fill_fdm(struct sitl_fdm &fdm);
 
@@ -121,6 +123,8 @@ class Aircraft {
         config_ = config;
     }
 
+    // return simulation origin:
+    const Location &get_origin() const { return origin; }
 
     const Location &get_location() const { return location; }
 
diff --git a/libraries/SITL/SITL.cpp b/libraries/SITL/SITL.cpp
index 7af63f16c465e8..9d70ad013aab3e 100644
--- a/libraries/SITL/SITL.cpp
+++ b/libraries/SITL/SITL.cpp
@@ -452,7 +452,7 @@ const AP_Param::GroupInfo SIM::var_info3[] = {
     // @DisplayName: Simulated ADSB Type mask
     // @Description: specifies which simulated ADSB types are active
     // @User: Advanced
-    // @Bitmask: 0:MAVLink,1:SageTechMXS
+    // @Bitmask: 0:MAVLink,3:SageTechMXS
     AP_GROUPINFO("ADSB_TYPES",    52, SIM,  adsb_types, 1),
 
 #ifdef SFML_JOYSTICK