Merge branch 'KRSHKF7Mini-hwdef' of https://github.com/Rahul-Karshak/ardupilot into KRSHKF7Mini-hwdef

Tools/AP_Bootloader/board_types.txt

@@ -336,6 +336,8 @@ AP_HW_HGLRCF405V4                    1524
 
 AP_HW_MFT-SEMA100                    2000
 
+AP_HW_AET-H743-Basic                 2024
+
 AP_HW_SakuraRC-H743                  2714
 
 # IDs 4000-4009 reserved for Karshak Drones

Tools/ardupilotwaf/boards.py

@@ -177,7 +177,10 @@ class Board:
 
         if cfg.options.enable_networking_tests:
             env.CXXFLAGS += ['-DAP_NETWORKING_TESTS_ENABLED=1']
-            
+
+        if cfg.options.enable_iomcu_profiled_support:
+            env.CXXFLAGS += ['-DAP_IOMCU_PROFILED_SUPPORT_ENABLED=1']
+
         d = env.get_merged_dict()
         # Always prepend so that arguments passed in the command line get
         # the priority.

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/plane_waypoint_follower.py

@@ -33,6 +33,7 @@ from geopy import distance
 from geopy import point
 from ardupilot_msgs.srv import ArmMotors
 from ardupilot_msgs.srv import ModeSwitch
+from geographic_msgs.msg import GeoPointStamped
 
 
 PLANE_MODE_TAKEOFF = 13
@@ -78,6 +79,15 @@ class PlaneWaypointFollower(Node):
 
         self._subscription_geopose = self.create_subscription(GeoPoseStamped, self._geopose_topic, self.geopose_cb, qos)
         self._cur_geopose = GeoPoseStamped()
+        
+        self.declare_parameter("goal_topic", "/ap/goal_lla")
+        self._goal_topic = self.get_parameter("goal_topic").get_parameter_value().string_value
+        qos = rclpy.qos.QoSProfile(
+            reliability=rclpy.qos.ReliabilityPolicy.RELIABLE, durability=rclpy.qos.DurabilityPolicy.TRANSIENT_LOCAL, depth=1
+        )
+
+        self._subscription_goal = self.create_subscription(GeoPointStamped, self._goal_topic, self.goal_cb, qos)
+        self._cur_goal = GeoPointStamped()        
 
     def geopose_cb(self, msg: GeoPoseStamped):
         """Process a GeoPose message."""
@@ -87,6 +97,15 @@ class PlaneWaypointFollower(Node):
 
             # Store current state
             self._cur_geopose = msg
+            
+    def goal_cb(self, msg: GeoPointStamped):
+        """Process a Goal message."""
+        stamp = msg.header.stamp
+        self.get_logger().info("From AP : Goal [sec:{}, nsec: {}, lat:{} lon:{}]"
+                               .format(stamp.sec, stamp.nanosec,msg.position.latitude, msg.position.longitude))
+
+        # Store current state
+        self._cur_goal = msg
 
     def arm(self):
         req = ArmMotors.Request()
@@ -127,6 +146,10 @@ class PlaneWaypointFollower(Node):
     def get_cur_geopose(self):
         """Return latest geopose."""
         return self._cur_geopose
+    
+    def get_cur_goal(self):
+        """Return latest goal."""
+        return self._cur_goal 
 
     def send_goal_position(self, goal_global_pos):
         """Send goal position. Must be in guided for this to work."""
@@ -148,6 +171,15 @@ def achieved_goal(goal_global_pos, cur_geopose):
     print(f"Goal is {euclidian_distance} meters away")
     return euclidian_distance < 150
 
+def going_to_goal(goal_global_pos, cur_goal):
+    p1 = (goal_global_pos.latitude, goal_global_pos.longitude, goal_global_pos.altitude)
+    cur_pos_lla = cur_goal.position
+    p2 = (cur_pos_lla.latitude, cur_pos_lla.longitude, cur_pos_lla.altitude)
+
+    flat_distance = distance.distance(p1[:2], p2[:2]).m
+    euclidian_distance = math.sqrt(flat_distance**2 + (p2[2] - p1[2]) ** 2)
+    print(f"Commanded and received goal are {euclidian_distance} meters away")
+    return euclidian_distance < 1    
 
 def main(args=None):
     """Node entry point."""
@@ -191,11 +223,15 @@ def main(args=None):
 
         start = node.get_clock().now()
         has_achieved_goal = False
+        is_going_to_goal = False
         while not has_achieved_goal and node.get_clock().now() - start < rclpy.duration.Duration(seconds=120):
             rclpy.spin_once(node)
+            is_going_to_goal = going_to_goal(goal_pos, node.get_cur_goal())
             has_achieved_goal = achieved_goal(goal_pos, node.get_cur_geopose())
             time.sleep(1.0)
 
+        if not is_going_to_goal:
+            raise RuntimeError("Unable to go to goal location")
         if not has_achieved_goal:
             raise RuntimeError("Unable to achieve goal location")
 

Tools/ros2/ardupilot_dds_tests/ardupilot_dds_tests/pre_arm_check_service.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+# Copyright 2023 ArduPilot.org.
+#
+# 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 <https://www.gnu.org/licenses/>.
+
+"""
+Run pre_arm check test on Copter.
+
+Warning - This is NOT production code; it's a simple demo of capability.
+"""
+
+import math
+import rclpy
+import time
+import errno
+
+from rclpy.node import Node
+from builtin_interfaces.msg import Time
+from std_srvs.srv import Trigger
+
+
+class CopterPreArm(Node):
+    """Check Prearm Service."""
+
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("copter_prearm")
+
+        self.declare_parameter("pre_arm_service", "/ap/prearm_check")
+        self._prearm_service = self.get_parameter("pre_arm_service").get_parameter_value().string_value
+        self._client_prearm = self.create_client(Trigger, self._prearm_service)
+        while not self._client_prearm.wait_for_service(timeout_sec=1.0):
+            self.get_logger().info('prearm service not available, waiting again...')
+
+    def prearm(self):
+        req = Trigger.Request()
+        future = self._client_prearm.call_async(req)
+        rclpy.spin_until_future_complete(self, future)
+        return future.result()
+
+    def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+        """Check if armable. Returns true on success, or false if arming will fail or times out."""
+        armable = False
+        start = self.get_clock().now()
+        while not armable and self.get_clock().now() - start < timeout:
+            armable = self.prearm().success
+            time.sleep(1)
+        return armable
+
+def main(args=None):
+    """Node entry point."""
+    rclpy.init(args=args)
+    node = CopterPreArm()
+    try:
+        # Block till armed, which will wait for EKF3 to initialize
+        if not node.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            raise RuntimeError("Vehicle not armable")
+    except KeyboardInterrupt:
+        pass
+
+    # Destroy the node explicitly.
+    node.destroy_node()
+    rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

Tools/ros2/ardupilot_dds_tests/setup.py

@@ -26,6 +26,7 @@ setup(
         'console_scripts': [
             "time_listener = ardupilot_dds_tests.time_listener:main",
             "plane_waypoint_follower = ardupilot_dds_tests.plane_waypoint_follower:main",
+            "pre_arm_check = ardupilot_dds_tests.pre_arm_check_service:main",
         ],
     },
 )

Tools/ros2/ardupilot_dds_tests/test/ardupilot_dds_tests/test_prearm_service.py

@@ -0,0 +1,166 @@
+# Copyright 2023 ArduPilot.org.
+#
+# 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 <https://www.gnu.org/licenses/>.
+
+"""
+Bring up ArduPilot SITL and check whether the vehicle can be armed.
+
+colcon test --executor sequential --parallel-workers 0 --base-paths src/ardupilot \
+    --event-handlers=console_cohesion+ --packages-select ardupilot_dds_tests \
+    --pytest-args -k test_prearm_service
+
+"""
+
+import launch_pytest
+import math
+import time
+import pytest
+import rclpy
+import rclpy.node
+import threading
+
+from launch import LaunchDescription
+
+from launch_pytest.tools import process as process_tools
+from std_srvs.srv import Trigger
+
+
+SERVICE = "/ap/prearm_check"
+
+class PreamService(rclpy.node.Node):
+    def __init__(self):
+        """Initialise the node."""
+        super().__init__("prearm_client")
+        self.service_available_object = threading.Event()
+        self.is_armable_object = threading.Event()
+        self._client_prearm = self.create_client(Trigger, "/ap/prearm_check")
+
+    def start_node(self):
+        # Add a spin thread.
+        self.ros_spin_thread = threading.Thread(target=lambda node: rclpy.spin(node), args=(self,))
+        self.ros_spin_thread.start()
+
+    def prearm_check(self):
+        req = Trigger.Request()
+        future = self._client_prearm.call_async(req)
+        time.sleep(0.2)
+        try:
+            return future.result().success
+        except Exception  as e:
+            print(e)
+            return False
+
+    def prearm_with_timeout(self, timeout: rclpy.duration.Duration):
+        result = False
+        start = self.get_clock().now()
+        while not result and self.get_clock().now() - start < timeout:
+            result = self.prearm_check()
+            time.sleep(1)
+        return result
+
+    def process_prearm(self):
+        if self.prearm_with_timeout(rclpy.duration.Duration(seconds=30)):
+            self.is_armable_object.set()
+
+    def start_prearm(self):
+        try:
+            self.prearm_thread.stop()
+        except:
+            print("start_prearm not started yet")
+        self.prearm_thread = threading.Thread(target=self.process_prearm)
+        self.prearm_thread.start()
+        
+        
+
+
+@launch_pytest.fixture
+def launch_sitl_copter_dds_serial(sitl_copter_dds_serial):
+    """Fixture to create the launch description."""
+    sitl_ld, sitl_actions = sitl_copter_dds_serial
+
+    ld = LaunchDescription(
+        [
+            sitl_ld,
+            launch_pytest.actions.ReadyToTest(),
+        ]
+    )
+    actions = sitl_actions
+    yield ld, actions
+
+
+@launch_pytest.fixture
+def launch_sitl_copter_dds_udp(sitl_copter_dds_udp):
+    """Fixture to create the launch description."""
+    sitl_ld, sitl_actions = sitl_copter_dds_udp
+
+    ld = LaunchDescription(
+        [
+            sitl_ld,
+            launch_pytest.actions.ReadyToTest(),
+        ]
+    )
+    actions = sitl_actions
+    yield ld, actions
+
+
+@pytest.mark.launch(fixture=launch_sitl_copter_dds_serial)
+def test_dds_serial_prearm_service_call(launch_context, launch_sitl_copter_dds_serial):
+    """Test prearm service AP_DDS."""
+    _, actions = launch_sitl_copter_dds_serial
+    virtual_ports = actions["virtual_ports"].action
+    micro_ros_agent = actions["micro_ros_agent"].action
+    mavproxy = actions["mavproxy"].action
+    sitl = actions["sitl"].action
+
+    # Wait for process to start.
+    process_tools.wait_for_start_sync(launch_context, virtual_ports, timeout=2)
+    process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
+    process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
+    process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)
+
+    rclpy.init()
+    try:
+        node = PreamService()
+        node.start_node()
+        node.start_prearm()
+        is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+        assert is_armable_flag, f"Vehicle not armable."
+    finally:
+        rclpy.shutdown()
+    yield
+
+
+@pytest.mark.launch(fixture=launch_sitl_copter_dds_udp)
+def test_dds_udp_prearm_service_call(launch_context, launch_sitl_copter_dds_udp):
+    """Test prearm service AP_DDS."""
+    _, actions = launch_sitl_copter_dds_udp
+    micro_ros_agent = actions["micro_ros_agent"].action
+    mavproxy = actions["mavproxy"].action
+    sitl = actions["sitl"].action
+
+    # Wait for process to start.
+    process_tools.wait_for_start_sync(launch_context, micro_ros_agent, timeout=2)
+    process_tools.wait_for_start_sync(launch_context, mavproxy, timeout=2)
+    process_tools.wait_for_start_sync(launch_context, sitl, timeout=2)
+
+    rclpy.init()
+    try:
+        node = PreamService()
+        node.start_node()
+        node.start_prearm()
+        is_armable_flag = node.is_armable_object.wait(timeout=25.0)
+        assert is_armable_flag, f"Vehicle not armable."
+    finally:
+        rclpy.shutdown()
+    yield

Tools/scripts/build_iofirmware.py

@@ -29,12 +29,24 @@ run_program(["./waf", "iofirmware"])
 shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_lowpolh.bin')
 shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_lowpolh.bin')
+shutil.copy('build/iomcu/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-dshot'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])
 shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_lowpolh.bin')
 shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_dshot_highpolh.bin')
 
+run_program(["./waf", "configure", "--board", 'iomcu-dshot', '--enable-iomcu-profiled-support'])
+run_program(["./waf", "clean"])
+run_program(["./waf", "iofirmware"])
+shutil.copy('build/iomcu-dshot/bin/iofirmware_lowpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_lowpolh.bin')
+shutil.copy('build/iomcu-dshot/bin/iofirmware_highpolh.bin', 'Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin')
+
 run_program(["./waf", "configure", "--board", 'iomcu-f103'])
 run_program(["./waf", "clean"])
 run_program(["./waf", "iofirmware"])

libraries/AP_Airspeed/AP_Airspeed.cpp

@@ -157,7 +157,7 @@ const AP_Param::GroupInfo AP_Airspeed::var_info[] = {
     
     // @Param: _OFF_PCNT
     // @DisplayName: Maximum offset cal speed error 
-    // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibrations before a warning is issued. This potential speed error is in percent of ASPD_FBW_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
+    // @Description: The maximum percentage speed change in airspeed reports that is allowed due to offset changes between calibrations before a warning is issued. This potential speed error is in percent of AIRSPEED_MIN. 0 disables. Helps warn of calibrations without pitot being covered.
     // @Range: 0.0 10.0
     // @Units: %
     // @User: Advanced

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -281,7 +281,7 @@ const AP_Param::GroupInfo AP_BoardConfig::var_info[] = {
     // @Param: OPTIONS
     // @DisplayName: Board options
     // @Description: Board specific option flags
-    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm
+    // @Bitmask: 0:Enable hardware watchdog, 1:Disable MAVftp, 2:Enable set of internal parameters, 3:Enable Debug Pins, 4:Unlock flash on reboot, 5:Write protect firmware flash on reboot, 6:Write protect bootloader flash on reboot, 7:Skip board validation, 8:Disable board arming gpio output change on arm/disarm, 9:Use safety pins as profiled
     // @User: Advanced
     AP_GROUPINFO("OPTIONS", 19, AP_BoardConfig, _options, HAL_BRD_OPTIONS_DEFAULT),
 

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -158,7 +158,8 @@ public:
         WRITE_PROTECT_FLASH = (1<<5),
         WRITE_PROTECT_BOOTLOADER = (1<<6),
         SKIP_BOARD_VALIDATION = (1<<7),
-        DISABLE_ARMING_GPIO = (1<<8)
+        DISABLE_ARMING_GPIO = (1<<8),
+        IO_SAFETY_PINS_AS_PROFILED = (1<<9),
     };
 
     //return true if arming gpio output is disabled
@@ -200,6 +201,12 @@ public:
         return _singleton?(_singleton->_options & ALLOW_SET_INTERNAL_PARM)!=0:false;
     }
     
+#if HAL_WITH_IO_MCU
+    static bool use_safety_as_led(void) {
+        return _singleton?(_singleton->_options & IO_SAFETY_PINS_AS_PROFILED)!=0:false;
+    }
+#endif
+
     // handle press of safety button. Return true if safety state
     // should be toggled
     bool safety_button_handle_pressed(uint8_t press_count);

libraries/AP_DDS/AP_DDS_Client.cpp

@@ -26,6 +26,9 @@
 #if AP_DDS_MODE_SWITCH_SERVER_ENABLED
 #include "ardupilot_msgs/srv/ModeSwitch.h"
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+#include "std_srvs/srv/Trigger.h"
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
 
 #if AP_EXTERNAL_CONTROL_ENABLED
 #include "AP_DDS_ExternalControl.h"
@@ -37,6 +40,8 @@
 #include "AP_DDS_Service_Table.h"
 #include "AP_DDS_External_Odom.h"
 
+#define STRCPY(D,S) strncpy(D, S, ARRAY_SIZE(D))
+
 // Enable DDS at runtime by default
 static constexpr uint8_t ENABLED_BY_DEFAULT = 1;
 #if AP_DDS_TIME_PUB_ENABLED
@@ -60,6 +65,9 @@ static constexpr uint16_t DELAY_AIRSPEED_TOPIC_MS = AP_DDS_DELAY_AIRSPEED_TOPIC_
 #if AP_DDS_GEOPOSE_PUB_ENABLED
 static constexpr uint16_t DELAY_GEO_POSE_TOPIC_MS = AP_DDS_DELAY_GEO_POSE_TOPIC_MS;
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+static constexpr uint16_t DELAY_GOAL_TOPIC_MS = AP_DDS_DELAY_GOAL_TOPIC_MS ;
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
 static constexpr uint16_t DELAY_CLOCK_TOPIC_MS =AP_DDS_DELAY_CLOCK_TOPIC_MS;
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -289,8 +297,8 @@ void AP_DDS_Client::populate_static_transforms(tf2_msgs_msg_TFMessage& msg)
         char gps_frame_id[16];
         //! @todo should GPS frame ID's be 0 or 1 indexed in ROS?
         hal.util->snprintf(gps_frame_id, sizeof(gps_frame_id), "GPS_%u", i);
-        strcpy(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
-        strcpy(msg.transforms[i].child_frame_id, gps_frame_id);
+        STRCPY(msg.transforms[i].header.frame_id, BASE_LINK_FRAME_ID);
+        STRCPY(msg.transforms[i].child_frame_id, gps_frame_id);
         // The body-frame offsets
         // X - Forward
         // Y - Right
@@ -392,7 +400,7 @@ void AP_DDS_Client::update_topic(sensor_msgs_msg_BatteryState& msg, const uint8_
 void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -443,7 +451,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_PoseStamped& msg)
 void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -486,7 +494,7 @@ void AP_DDS_Client::update_topic(geometry_msgs_msg_TwistStamped& msg)
 bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
     // In ROS REP 103, axis orientation uses the following convention:
@@ -515,7 +523,7 @@ bool AP_DDS_Client::update_topic(geometry_msgs_msg_Vector3Stamped& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -552,11 +560,44 @@ void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPoseStamped& msg)
 }
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+bool AP_DDS_Client::update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg)
+{
+    const auto &vehicle = AP::vehicle();
+    update_topic(msg.header.stamp);
+    Location target_loc;
+    // Exit if no target is available.
+    if (!vehicle->get_target_location(target_loc)) {
+        return false;
+    }
+    target_loc.change_alt_frame(Location::AltFrame::ABSOLUTE);
+    msg.position.latitude = target_loc.lat * 1e-7;
+    msg.position.longitude = target_loc.lng * 1e-7;
+    msg.position.altitude = target_loc.alt * 1e-2;
+
+    // Check whether the goal has changed or if the topic has never been published.
+    const double tolerance_lat_lon = 1e-8; // One order of magnitude smaller than the target's resolution.
+    const double distance_alt = 1e-3;
+    if (abs(msg.position.latitude - prev_goal_msg.position.latitude) >  tolerance_lat_lon ||
+        abs(msg.position.longitude - prev_goal_msg.position.longitude) >  tolerance_lat_lon ||
+        abs(msg.position.altitude - prev_goal_msg.position.altitude) > distance_alt ||
+        prev_goal_msg.header.stamp.sec == 0 ) {
+        update_topic(prev_goal_msg.header.stamp);
+        prev_goal_msg.position.latitude = msg.position.latitude;
+        prev_goal_msg.position.longitude = msg.position.longitude;
+        prev_goal_msg.position.altitude = msg.position.altitude;
+        return true;
+    } else {
+        return false;
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_IMU_PUB_ENABLED
 void AP_DDS_Client::update_topic(sensor_msgs_msg_Imu& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_NED_FRAME_ID);
 
     auto &imu = AP::ins();
     auto &ahrs = AP::ahrs();
@@ -603,7 +644,7 @@ void AP_DDS_Client::update_topic(rosgraph_msgs_msg_Clock& msg)
 void AP_DDS_Client::update_topic(geographic_msgs_msg_GeoPointStamped& msg)
 {
     update_topic(msg.header.stamp);
-    strcpy(msg.header.frame_id, BASE_LINK_FRAME_ID);
+    STRCPY(msg.header.frame_id, BASE_LINK_FRAME_ID);
 
     auto &ahrs = AP::ahrs();
     WITH_SEMAPHORE(ahrs.get_semaphore());
@@ -874,6 +915,35 @@ void AP_DDS_Client::on_request(uxrSession* uxr_session, uxrObjectId object_id, u
         break;
     }
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+    case services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id: {
+        std_srvs_srv_Trigger_Request prearm_check_request;
+        std_srvs_srv_Trigger_Response prearm_check_response;
+        const bool deserialize_success = std_srvs_srv_Trigger_Request_deserialize_topic(ub, &prearm_check_request);
+        if (deserialize_success == false) {
+            break;
+        }
+        prearm_check_response.success = AP::arming().pre_arm_checks(false);
+        STRCPY(prearm_check_response.message, prearm_check_response.success ? "Vehicle is Armable" : "Vehicle is Not Armable");
+
+        const uxrObjectId replier_id = {
+            .id = services[to_underlying(ServiceIndex::PREARM_CHECK)].rep_id,
+            .type = UXR_REPLIER_ID
+        };
+
+        uint8_t reply_buffer[sizeof(prearm_check_response.message) + 1] {};
+        ucdrBuffer reply_ub;
+
+        ucdr_init_buffer(&reply_ub, reply_buffer, sizeof(reply_buffer));
+        const bool serialize_success = std_srvs_srv_Trigger_Response_serialize_topic(&reply_ub, &prearm_check_response);
+        if (serialize_success == false) {
+            break;
+        }
+
+        uxr_buffer_reply(uxr_session, reliable_out, replier_id, sample_id, reply_buffer, ucdr_buffer_length(&reply_ub));
+        break;
+    }
+#endif //AP_DDS_ARM_CHECK_SERVER_ENABLED    
 #if AP_DDS_PARAMETER_SERVER_ENABLED
     case services[to_underlying(ServiceIndex::SET_PARAMETERS)].rep_id: {
         const bool deserialize_success = rcl_interfaces_srv_SetParameters_Request_deserialize_topic(ub, &set_parameter_request);
@@ -1522,6 +1592,22 @@ void AP_DDS_Client::write_gps_global_origin_topic()
 }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+void AP_DDS_Client::write_goal_topic()
+{
+    WITH_SEMAPHORE(csem);
+    if (connected) {
+        ucdrBuffer ub {};
+        const uint32_t topic_size = geographic_msgs_msg_GeoPointStamped_size_of_topic(&goal_topic, 0);
+        uxr_prepare_output_stream(&session, reliable_out, topics[to_underlying(TopicIndex::GOAL_PUB)].dw_id, &ub, topic_size);
+        const bool success = geographic_msgs_msg_GeoPointStamped_serialize_topic(&ub, &goal_topic);
+        if (!success) {
+            // AP_HAL::panic("FATAL: DDS_Client failed to serialize\n");
+        }
+    }
+}
+#endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_STATUS_PUB_ENABLED
 void AP_DDS_Client::write_status_topic()
 {
@@ -1618,6 +1704,14 @@ void AP_DDS_Client::update()
         write_gps_global_origin_topic();
     }
 #endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    if (cur_time_ms - last_goal_time_ms > DELAY_GOAL_TOPIC_MS) {
+        if (update_topic_goal(goal_topic)) {
+            write_goal_topic();
+        }
+        last_goal_time_ms = cur_time_ms;
+    }
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_STATUS_PUB_ENABLED
     if (cur_time_ms - last_status_check_time_ms > DELAY_STATUS_TOPIC_MS) {
         if (update_topic(status_topic)) {

libraries/AP_DDS/AP_DDS_Client.h

@@ -114,6 +114,16 @@ private:
     static void update_topic(geographic_msgs_msg_GeoPointStamped& msg);
 # endif // AP_DDS_GPS_GLOBAL_ORIGIN_PUB_ENABLED
 
+#if AP_DDS_GOAL_PUB_ENABLED
+    geographic_msgs_msg_GeoPointStamped goal_topic;
+    // The last ms timestamp AP_DDS wrote a goal message
+    uint64_t last_goal_time_ms;
+    //! @brief Serialize the current goal and publish to the IO stream(s)
+    void write_goal_topic();
+    bool update_topic_goal(geographic_msgs_msg_GeoPointStamped& msg);
+    geographic_msgs_msg_GeoPointStamped prev_goal_msg;
+# endif // AP_DDS_GOAL_PUB_ENABLED
+
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     geographic_msgs_msg_GeoPoseStamped geo_pose_topic;
     // The last ms timestamp AP_DDS wrote a GeoPose message

libraries/AP_DDS/AP_DDS_Service_Table.h

@@ -8,6 +8,9 @@ enum class ServiceIndex: uint8_t {
 #if AP_DDS_MODE_SWITCH_SERVER_ENABLED
     MODE_SWITCH,
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+    PREARM_CHECK,
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED
 #if AP_DDS_PARAMETER_SERVER_ENABLED
     SET_PARAMETERS,
     GET_PARAMETERS
@@ -51,6 +54,18 @@ constexpr struct AP_DDS_Client::Service_table AP_DDS_Client::services[] = {
         .reply_topic_name = "rr/ap/mode_switchReply",
     },
 #endif // AP_DDS_MODE_SWITCH_SERVER_ENABLED
+#if AP_DDS_ARM_CHECK_SERVER_ENABLED
+    {
+        .req_id = to_underlying(ServiceIndex::PREARM_CHECK),
+        .rep_id = to_underlying(ServiceIndex::PREARM_CHECK),
+        .service_rr = Service_rr::Replier,
+        .service_name = "rs/ap/prearm_checkService",
+        .request_type = "std_srvs::srv::dds_::Trigger_Request_",
+        .reply_type = "std_srvs::srv::dds_::Trigger_Response_",
+        .request_topic_name = "rq/ap/prearm_checkRequest",
+        .reply_topic_name = "rr/ap/prearm_checkReply",
+    },
+#endif // AP_DDS_ARM_CHECK_SERVER_ENABLED    
 #if AP_DDS_PARAMETER_SERVER_ENABLED
     {
         .req_id = to_underlying(ServiceIndex::SET_PARAMETERS),

libraries/AP_DDS/AP_DDS_Topic_Table.h

@@ -42,6 +42,9 @@ enum class TopicIndex: uint8_t {
 #if AP_DDS_GEOPOSE_PUB_ENABLED
     GEOPOSE_PUB,
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#ifdef AP_DDS_GOAL_PUB_ENABLED
+    GOAL_PUB,
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     CLOCK_PUB,
 #endif // AP_DDS_CLOCK_PUB_ENABLED
@@ -235,6 +238,24 @@ constexpr struct AP_DDS_Client::Topic_table AP_DDS_Client::topics[] = {
         },
     },
 #endif // AP_DDS_GEOPOSE_PUB_ENABLED
+#if AP_DDS_GOAL_PUB_ENABLED
+    {
+        .topic_id = to_underlying(TopicIndex::GOAL_PUB),
+        .pub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .sub_id = to_underlying(TopicIndex::GOAL_PUB),
+        .dw_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAWRITER_ID},
+        .dr_id = uxrObjectId{.id=to_underlying(TopicIndex::GOAL_PUB), .type=UXR_DATAREADER_ID},
+        .topic_rw = Topic_rw::DataWriter,
+        .topic_name = "rt/ap/goal_lla",
+        .type_name = "geographic_msgs::msg::dds_::GeoPointStamped_",
+        .qos = {
+            .durability = UXR_DURABILITY_TRANSIENT_LOCAL,
+            .reliability = UXR_RELIABILITY_RELIABLE,
+            .history = UXR_HISTORY_KEEP_LAST,
+            .depth = 1,
+        },
+    },
+#endif // AP_DDS_GOAL_PUB_ENABLED
 #if AP_DDS_CLOCK_PUB_ENABLED
     {
         .topic_id = to_underlying(TopicIndex::CLOCK_PUB),

libraries/AP_DDS/AP_DDS_config.h

@@ -106,6 +106,13 @@
 #define AP_DDS_DELAY_CLOCK_TOPIC_MS 10
 #endif
 
+#ifndef AP_DDS_GOAL_PUB_ENABLED
+#define AP_DDS_GOAL_PUB_ENABLED 1
+#endif
+
+#ifndef AP_DDS_DELAY_GOAL_TOPIC_MS
+#define AP_DDS_DELAY_GOAL_TOPIC_MS  200
+#endif
 #ifndef AP_DDS_STATUS_PUB_ENABLED
 #define AP_DDS_STATUS_PUB_ENABLED 1
 #endif
@@ -138,6 +145,10 @@
 #define AP_DDS_PARAMETER_SERVER_ENABLED 1
 #endif
 
+#ifndef AP_DDS_ARM_CHECK_SERVER_ENABLED
+#define AP_DDS_ARM_CHECK_SERVER_ENABLED 1
+#endif
+
 // Whether to include Twist support
 #define AP_DDS_NEEDS_TWIST AP_DDS_VEL_CTRL_ENABLED || AP_DDS_LOCAL_VEL_PUB_ENABLED
 

libraries/AP_DDS/Idl/std_srvs/srv/Trigger.idl

@@ -0,0 +1,21 @@
+// generated from rosidl_adapter/resource/srv.idl.em
+// with input from std_srvs/srv/Trigger.srv
+// generated code does not contain a copyright notice
+
+
+module std_srvs {
+  module srv {
+    struct Trigger_Request {
+      uint8 structure_needs_at_least_one_member;
+    };
+    struct Trigger_Response {
+      @verbatim (language="comment", text=
+        "indicate successful run of triggered service")
+      boolean success;
+
+      @verbatim (language="comment", text=
+        "informational, e.g. for error messages")
+      string message;
+    };
+  };
+};

libraries/AP_DDS/README.md

@@ -209,6 +209,7 @@ nanosec: 729410000
 $ ros2 service list
 /ap/arm_motors
 /ap/mode_switch
+/ap/prearm_check
 ---
 ```
 
@@ -234,6 +235,7 @@ List the available services:
 $ ros2 service list -t
 /ap/arm_motors [ardupilot_msgs/srv/ArmMotors]
 /ap/mode_switch [ardupilot_msgs/srv/ModeSwitch]
+/ap/prearm_check [std_srvs/srv/Trigger]
 ```
 
 Call the arm motors service:
@@ -256,6 +258,20 @@ response:
 ardupilot_msgs.srv.ModeSwitch_Response(status=True, curr_mode=4)
 ```
 
+Call the prearm check service:
+
+```bash
+$ ros2 service call /ap/prearm_check std_srvs/srv/Trigger
+requester: making request: std_srvs.srv.Trigger_Request()
+
+response:
+std_srvs.srv.Trigger_Response(success=False, message='Vehicle is Not Armable')
+
+or
+
+std_srvs.srv.Trigger_Response(success=True, message='Vehicle is Armable')
+```
+
 ## Commanding using ROS 2 Topics
 
 The following topic can be used to control the vehicle.

libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/README.md

@@ -0,0 +1,142 @@
+# AET-H743-Basic Flight Controller
+
+The AET-H743-Basic is a flight controller designed and produced by AeroEggTech
+
+## Features
+
+ - STM32H743 microcontroller
+ - Dual ICM42688P IMUs
+ - 13 PWM / Dshot outputs
+ - 8 UARTs, one with CTS/RTS flow control pins
+ - 1 CAN
+ - Dedicated USB board
+ - DPS310 or SPL06 barometer
+ - 5V/6V/7V 10A Servo rail BEC
+ - 9V 2A BEC for VTX, GPIO controlled
+ - 5V 4A BEC
+ - MicroSD Card Slot
+ - 2-way camera input
+ - AT7456E OSD
+ - 2 I2Cs
+
+## Physical
+
+![AET-H743-Basic overview](AET-H743-Basic_overview.jpg)
+
+![AET-H743-Basic core board](AET-H743-Basic_core_board.png)
+
+![AET-H743-Basic power board](AET-H743-Basic_power_board.png)
+
+
+## Mechanical
+
+ - Dimensions: 36 x 47 x 17 mm
+ - Weight: 45g
+
+
+## Power supply
+
+The AET-H743-Basic supports 2-6s Li battery input. It has 3 ways of BEC, which result in 6 ways of power supplys. Please see the table below.
+
+| Power symbol | Power source | Max power (current) |
+|--------------|--------------|---------------------|
+| 5V | from 5V BEC | 20W (4A) |
+| 9V | from 9V BEC | 18W (2A) |
+| 9Vsw | from 9V BEC, controlled by MCU with an NPN MOS | 10W (1A) |
+| 4V5 | from USB or 5V BEC, with a diode | 5W (1A) |
+| VX | from Servo rail VX BEC, default 5V, can be changed to 6V or 7V | 50W (10A) |
+| BAT | directly from battery | (5A) |
+
+
+## Loading Firmware
+
+Initial firmware load can be done with DFU by plugging in USB with the bootloader button pressed. Then you should load the "with_bl.hex" firmware, using your favorite DFU loading tool, such as Mission Planner.
+
+Once the initial firmware is loaded you can update the firmware using any ArduPilot ground station software. Updates should be done with the "\*.apj" firmware files.
+
+## UART Mapping
+
+All UARTs are DMA capable.
+
+ - SERIAL0 -> USB
+ - SERIAL1 -> UART1 (MAVLink2)
+ - SERIAL2 -> UART2 (GPS)
+ - SERIAL3 -> UART3 (MAVLink2)
+ - SERIAL4 -> UART4 (GPS2, RX4 is also available as ESC telem if protocol is changed for this UART)
+ - SERIAL5 -> USB (SLCAN)
+ - SERIAL6 -> UART6 (RCIN)
+ - SERIAL7 -> UART7 (MAVLink2, Integrated Bluetooth module)
+ - SERIAL8 -> UART8 (User)
+
+
+## RC Input
+
+The default RC input is configured on the UART6 and supports all RC protocols except PPM. The SBUS pin is inverted and connected to RX6. RC can be attached to any UART port as long as the serial port protocol is set to `SERIALn_PROTOCOL=23` and SERIAL6_Protocol is changed to something other than '23'.
+
+
+## OSD Support
+
+The AET-H743-Basic supports onboard analog SD OSD using a AT7456 chip. The analog VTX should connect to the VTX pin.
+
+
+## PWM Output
+
+The AET-H743-Basic supports up to 13 PWM outputs.
+
+All the channels support DShot.
+
+Outputs are grouped and every output within a group must use the same output protocol:
+
+1, 2 are Group 1;
+
+3, 4, 5, 6 are Group 2;
+
+7, 8, 9, 10 are Group 3;
+
+11, 12 are Group 4;
+
+13(LED) is Group 5;
+
+Output 13 can be used as LED neopixel output;
+
+## Battery Monitoring
+
+The board has two internal voltage sensors and one integrated current sensor, and a second external current sensor input.
+
+The voltage sensors can handle up to 6S LiPo batteries.
+
+The first voltage/current sensor is enabled by default and the pin inputs for the second, unenabled sensor are also set by default:
+* BATT_MONITOR 4
+* BATT_VOLT_PIN 10
+* BATT_CURR_PIN 11
+* BATT_VOLT_MULT 11
+* BATT_CURR_SCALE 40
+* BATT2_VOLT_PIN 18
+* BATT2_CURR_PIN 7
+* BATT2_VOLT_MULT 11
+
+## Compass
+
+The AET-H743-Basic has no built-in compass, so if needed, you should use an external compass.
+
+## Analog cameras
+
+The AET-H743-Basic supports up to 2 cameras, connected to pin CAM1 and CAM2. You can select the video signal to VTX from camera by an RC channel. Set the parameters below:
+
+- RELAY2_FUNCTION = 1
+- RELAY_PIN2 = 82
+- RC8_OPTION = 34
+
+## 9V switch
+
+The 9Vsw power supply can be controlled by an RC channel. Set the parameters below:
+
+- RELAY1_FUNCTION = 1
+- RELAY_PIN = 81
+- RC7_OPTION = 28
+
+## Bluetooth
+
+The AET-H743-Basic support both legacy bluetooth SPP and BLE serial. The bluetooth uses UART7 as serial port. Search for `AET-H743-SPP` or `AET-H743-BLE` to connect.
+
+Note: you should connect a battery to the board so the bluetooth module can work.

libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/defaults.parm

@@ -0,0 +1,3 @@
+# setup for LEDs on chan13
+SERVO13_FUNCTION 120
+NTF_LED_TYPES 257

libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef-bl.dat

@@ -0,0 +1,49 @@
+# hw definition file for processing by chibios_pins.py
+# for AET H743-Basic bootloader
+
+# MCU class and specific type
+MCU STM32H7xx STM32H743xx
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_AET-H743-Basic
+
+# crystal frequency, setup to use external oscillator
+OSCILLATOR_HZ 8000000
+
+FLASH_SIZE_KB 2048
+
+# bootloader starts at zero offset
+FLASH_RESERVE_START_KB 0
+
+# the location where the bootloader will put the firmware
+# the H743 has 128k sectors
+FLASH_BOOTLOADER_LOAD_KB 128
+
+
+# order of UARTs (and USB). Allow bootloading on USB and telem1
+SERIAL_ORDER OTG1 UART7
+
+# UART7 (telem1)
+PE7 UART7_RX UART7 NODMA
+PE8 UART7_TX UART7 NODMA
+
+# PA10 IO-debug-console
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+PA13 JTMS-SWDIO SWD
+PA14 JTCK-SWCLK SWD
+
+# make sure Vsw is on during bootloader
+PD10  PINIO1 OUTPUT LOW
+
+PE3 LED_BOOTLOADER OUTPUT LOW
+define HAL_LED_ON 0
+
+# Add CS pins to ensure they are high in bootloader
+PC15 IMU1_CS CS
+PB12 MAX7456_CS CS
+PE11 IMU2_CS CS
+PD4 EXT_CS1 CS
+PE2 EXT_CS2 CS
+PC13 IMU3_CS CS

libraries/AP_HAL_ChibiOS/hwdef/AET-H743-Basic/hwdef.dat

@@ -0,0 +1,205 @@
+# hw definition file for processing by chibios_pins.py
+# for AET H743-Basic
+
+# MCU class and specific type
+MCU STM32H7xx STM32H743xx
+
+# board ID for firmware load
+APJ_BOARD_ID AP_HW_AET-H743-Basic
+
+# crystal frequency, setup to use external oscillator
+OSCILLATOR_HZ 8000000
+
+FLASH_SIZE_KB 2048
+env OPTIMIZE -Os
+
+USB_STRING_MANUFACTURER "AET"
+
+# bootloader takes first sector
+FLASH_RESERVE_START_KB 128
+
+# ChibiOS system timer
+STM32_ST_USE_TIMER 12
+define CH_CFG_ST_RESOLUTION 16
+
+# USB
+PA11 OTG_FS_DM OTG1
+PA12 OTG_FS_DP OTG1
+
+# SPI1 for IMU1 (ICM-42688)
+PA5 SPI1_SCK SPI1
+PA6 SPI1_MISO SPI1
+PD7 SPI1_MOSI SPI1
+PC15 IMU1_CS CS
+
+# SPI2 for MAX7456 OSD
+PB12 MAX7456_CS CS
+PB13 SPI2_SCK SPI2
+PB14 SPI2_MISO SPI2
+PB15 SPI2_MOSI SPI2
+
+# SPI4 for IMU3 (ICM-42688)
+PC13 IMU3_CS CS
+PE12 SPI4_SCK SPI4
+PE13 SPI4_MISO SPI4
+PE14 SPI4_MOSI SPI4
+
+# two I2C bus
+I2C_ORDER I2C2 I2C1
+
+# I2C1
+PB6 I2C1_SCL I2C1
+PB7 I2C1_SDA I2C1
+
+# I2C2
+PB10 I2C2_SCL I2C2
+PB11 I2C2_SDA I2C2
+
+# ADC
+PC0 BATT_VOLTAGE_SENS ADC1 SCALE(1)
+PC1 BATT_CURRENT_SENS ADC1 SCALE(1)
+PA4 BATT2_VOLTAGE_SENS ADC1 SCALE(1)
+PA7 BATT2_CURRENT_SENS ADC1 SCALE(1)
+
+define HAL_BATT_MONITOR_DEFAULT 4
+define HAL_BATT_VOLT_PIN 10
+define HAL_BATT_CURR_PIN 11
+define HAL_BATT2_VOLT_PIN 18
+define HAL_BATT2_CURR_PIN 7
+define HAL_BATT_VOLT_SCALE 11.0
+define HAL_BATT_CURR_SCALE 40.0
+define HAL_BATT2_VOLT_SCALE 11.0
+
+PC4 PRESSURE_SENS ADC1 SCALE(2)
+define HAL_DEFAULT_AIRSPEED_PIN 4
+
+PC5 RSSI_ADC ADC1
+define BOARD_RSSI_ANA_PIN 8
+
+# LED
+# green LED1 marked as B/E
+# blue LED0 marked as ACT
+PE3 LED0 OUTPUT LOW GPIO(90) # blue
+PE4 LED1 OUTPUT LOW GPIO(91) # green
+define HAL_GPIO_A_LED_PIN 91
+define HAL_GPIO_B_LED_PIN 90
+
+define HAL_GPIO_LED_ON 1
+define AP_NOTIFY_GPIO_LED_2_ENABLED 1
+
+# order of UARTs (and USB)
+SERIAL_ORDER OTG1 USART1 USART2 USART3 UART4 OTG2 USART6 UART7 UART8
+
+# USART1
+PA10 USART1_RX USART1
+PA9  USART1_TX USART1
+
+# USART2
+PD5 USART2_TX USART2
+PD6 USART2_RX USART2
+
+# USART3
+PD9 USART3_RX USART3
+PD8 USART3_TX USART3
+
+# UART4
+PB9 UART4_TX UART4
+PB8 UART4_RX UART4
+
+# USART6
+PC6 USART6_TX USART6
+PC7 USART6_RX USART6
+
+# UART7
+PE7 UART7_RX UART7
+PE8 UART7_TX UART7
+PE10 UART7_CTS UART7
+PE9  UART7_RTS UART7
+
+# UART8
+PE0 UART8_RX UART8
+PE1 UART8_TX UART8
+
+# Serial functions
+define DEFAULT_SERIAL2_PROTOCOL SerialProtocol_GPS
+define DEFAULT_SERIAL3_BAUD 115
+define DEFAULT_SERIAL3_PROTOCOL SerialProtocol_MAVLink2
+define DEFAULT_SERIAL4_PROTOCOL SerialProtocol_GPS
+define DEFAULT_SERIAL6_PROTOCOL SerialProtocol_RCIN
+define DEFAULT_SERIAL7_BAUD 115
+define DEFAULT_SERIAL7_PROTOCOL SerialProtocol_MAVLink2
+
+# CAN bus
+PD0 CAN1_RX CAN1
+PD1 CAN1_TX CAN1
+PD3 GPIO_CAN1_SILENT OUTPUT PUSHPULL SPEED_LOW LOW GPIO(70)
+
+# Motors
+PB0  TIM8_CH2N TIM8  PWM(1)  GPIO(50)
+PB1  TIM8_CH3N TIM8  PWM(2)  GPIO(51)
+PA0  TIM5_CH1  TIM5  PWM(3)  GPIO(52)
+PA1  TIM5_CH2  TIM5  PWM(4)  GPIO(53)
+PA2  TIM5_CH3  TIM5  PWM(5)  GPIO(54)
+PA3  TIM5_CH4  TIM5  PWM(6)  GPIO(55)
+PD12 TIM4_CH1 TIM4 PWM(7) GPIO(56)
+PD13 TIM4_CH2 TIM4 PWM(8) GPIO(57)
+PD14 TIM4_CH3 TIM4 PWM(9) GPIO(58)
+PD15 TIM4_CH4 TIM4 PWM(10) GPIO(59)
+PE5  TIM15_CH1 TIM15 PWM(11) GPIO(60)
+PE6  TIM15_CH2 TIM15 PWM(12) GPIO(61)
+PA8 TIM1_CH1 TIM1 PWM(13) GPIO(62) # for WS2812 LED
+
+# Beeper
+PA15 TIM2_CH1 TIM2 GPIO(32) ALARM
+
+# microSD support
+PC8 SDMMC1_D0 SDMMC1
+PC9 SDMMC1_D1 SDMMC1
+PC10 SDMMC1_D2 SDMMC1
+PC11 SDMMC1_D3 SDMMC1
+PC12 SDMMC1_CK SDMMC1
+PD2 SDMMC1_CMD SDMMC1
+
+# GPIOs
+PD10  PINIO1 OUTPUT GPIO(81) LOW
+PD11  PINIO2 OUTPUT GPIO(82) LOW
+
+DMA_PRIORITY S*
+
+define HAL_STORAGE_SIZE 32768
+
+# use last 2 pages for flash storage
+# H743 has 16 pages of 128k each
+STORAGE_FLASH_PAGE 14
+
+# spi devices
+SPIDEV icm42688_0  SPI1 DEVID1 IMU1_CS      MODE3  2*MHZ 16*MHZ   # Clock is 100Mhz so highest clock <= 24Mhz is 100Mhz/8
+SPIDEV icm42688_1  SPI4 DEVID1 IMU3_CS      MODE3  2*MHZ 16*MHZ
+SPIDEV osd         SPI2 DEVID4 MAX7456_CS   MODE0 10*MHZ 10*MHZ
+
+
+DMA_NOSHARE SPI1* SPI4*
+
+# no built-in compass, but probe the i2c bus for all possible
+# external compass types
+define ALLOW_ARM_NO_COMPASS
+define HAL_PROBE_EXTERNAL_I2C_COMPASSES
+define HAL_I2C_INTERNAL_MASK 0
+define HAL_COMPASS_AUTO_ROT_DEFAULT 2
+
+# two IMUs
+# ICM42688P, ICM42688P
+IMU Invensensev3 SPI:icm42688_0 ROTATION_YAW_180
+IMU Invensensev3 SPI:icm42688_1 ROTATION_YAW_270
+define HAL_DEFAULT_INS_FAST_SAMPLE 1
+
+# BAROs
+BARO SPL06  I2C:0:0x76
+BARO DPS310 I2C:0:0x76
+
+define HAL_OS_FATFS_IO 1
+
+# setup for OSD
+define OSD_ENABLED 1
+define HAL_OSD_TYPE_DEFAULT 1
+ROMFS_WILDCARD libraries/AP_OSD/fonts/font*.bin

libraries/AP_HAL_ChibiOS/hwdef/CubeBlack+/hwdef.dat

@@ -19,4 +19,6 @@ IMU Invensensev2 SPI:icm20948_ext ROTATION_PITCH_180
 IMU Invensense SPI:mpu9250 ROTATION_YAW_270
 
 undef ROMFS
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
+
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1

libraries/AP_HAL_ChibiOS/hwdef/CubeBlack/hwdef.dat

@@ -102,8 +102,9 @@ define HAL_IMU_TEMP_MARGIN_LOW_DEFAULT 5
 
 # default the 2nd interface to SLCAN
 define HAL_OTG2_PROTOCOL SerialProtocol_SLCAN
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
 # enable support for dshot on iomcu
 define HAL_WITH_IO_MCU_DSHOT 1

libraries/AP_HAL_ChibiOS/hwdef/CubeOrange/hwdef.inc

@@ -296,12 +296,14 @@ define HAL_GPIO_PWM_VOLT_3v3 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root.
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1
+
 DMA_NOSHARE SPI1* SPI4* USART6*
 
 # for users who have replaced their CubeSolo with a CubeOrange:

libraries/AP_HAL_ChibiOS/hwdef/CubeYellow/hwdef.dat

@@ -445,9 +445,10 @@ STORAGE_FLASH_PAGE 1
 # we can automatically update the IOMCU firmware on boot. The format
 # is "ROMFS ROMFS-filename source-filename". Paths are relative to the
 # ardupilot root
-ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_highpolh.bin
+ROMFS io_firmware.bin Tools/IO_Firmware/iofirmware_cube_highpolh.bin
 
 # enable support for dshot on iomcu
-ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_dshot_highpolh.bin
+ROMFS io_firmware_dshot.bin Tools/IO_Firmware/iofirmware_cube_dshot_highpolh.bin
 define HAL_WITH_IO_MCU_DSHOT 1
 
+define AP_IOMCU_PROFILED_SUPPORT_ENABLED 1

libraries/AP_IOMCU/AP_IOMCU.cpp

@@ -43,6 +43,7 @@ enum ioevents {
     IOEVENT_SET_DSHOT_PERIOD,
     IOEVENT_SET_CHANNEL_MASK,
     IOEVENT_DSHOT,
+    IOEVENT_PROFILED,
 };
 
 // max number of consecutve protocol failures we accept before raising
@@ -89,7 +90,9 @@ void AP_IOMCU::init(void)
         crc_is_ok = true;
     }
 #endif
-
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    use_safety_as_led = boardconfig->use_safety_as_led();
+#endif
     if (!hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_IOMCU::thread_main, void), "IOMCU",
                                       1024, AP_HAL::Scheduler::PRIORITY_BOOST, 1)) {
         AP_HAL::panic("Unable to allocate IOMCU thread");
@@ -300,6 +303,16 @@ void AP_IOMCU::thread_main(void)
         }
         mask &= ~EVENT_MASK(IOEVENT_DSHOT);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+        if (mask & EVENT_MASK(IOEVENT_PROFILED)) {
+            if (!write_registers(PAGE_PROFILED, 0, sizeof(profiled)/sizeof(uint16_t), (const uint16_t*)&profiled)) {
+                event_failed(mask);
+                continue;
+            }
+        }
+        mask &= ~EVENT_MASK(IOEVENT_PROFILED);
+#endif
+
         // check for regular timed events
         uint32_t now = AP_HAL::millis();
         if (now - last_rc_read_ms > 20) {
@@ -1439,6 +1452,23 @@ void AP_IOMCU::toggle_GPIO(uint8_t pin)
     trigger_event(IOEVENT_GPIO);
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// set profiled R G B values
+void AP_IOMCU::set_profiled(uint8_t r, uint8_t g, uint8_t b)
+{
+    if (!use_safety_as_led) {
+        return;
+    }
+    if (r == profiled.red && g == profiled.green && b == profiled.blue) {
+        return;
+    }
+    profiled.magic = PROFILED_ENABLE_MAGIC;
+    profiled.red = r;
+    profiled.green = g;
+    profiled.blue = b;
+    trigger_event(IOEVENT_PROFILED);
+}
+#endif
 
 namespace AP {
     AP_IOMCU *iomcu(void) {

libraries/AP_IOMCU/AP_IOMCU.h

@@ -176,6 +176,11 @@ public:
     // toggle a output pin
     void toggle_GPIO(uint8_t pin);
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // set profiled values
+    void set_profiled(uint8_t r, uint8_t g, uint8_t b);
+#endif
+
     // channel group masks
     const uint8_t ch_masks[3] = { 0x03,0x0C,0xF0 };
 
@@ -297,6 +302,11 @@ private:
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control
+    struct page_profiled profiled {0, 255, 255, 255}; // by default, white
+#endif
+
     // IMU heater duty cycle
     uint8_t heater_duty_cycle;
 
@@ -310,6 +320,9 @@ private:
     bool detected_io_reset;
     bool initialised;
     bool is_chibios_backend;
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    bool use_safety_as_led;
+#endif
 
     uint32_t protocol_fail_count;
     uint32_t protocol_count;

libraries/AP_IOMCU/iofirmware/iofirmware.cpp

@@ -489,6 +489,10 @@ void AP_IOMCU_FW::update()
         last_status_ms = now;
         page_status_update();
     }
+
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    profiled_update();
+#endif
 #ifdef HAL_WITH_BIDIR_DSHOT
     // EDT updates are semt at ~1Hz per ESC, but we want to make sure
     // that we don't delay updates unduly so sample at 5Hz
@@ -1029,6 +1033,20 @@ bool AP_IOMCU_FW::handle_code_write()
         break;
     }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    case PAGE_PROFILED:
+        if (rx_io_packet.count != 2 || (rx_io_packet.regs[0] & 0xFF) != PROFILED_ENABLE_MAGIC) {
+            return false;
+        }
+        profiled_brg[0] = rx_io_packet.regs[0] >> 8;
+        profiled_brg[1] = rx_io_packet.regs[1] & 0xFF;
+        profiled_brg[2] = rx_io_packet.regs[1] >> 8;
+        // push new led data
+        profiled_num_led_pushed = 0;
+        profiled_control_enabled = true;
+        break;
+#endif
+
     default:
         break;
     }
@@ -1063,6 +1081,48 @@ void AP_IOMCU_FW::calculate_fw_crc(void)
     reg_setup.crc[1] = sum >> 16;
 }
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+// bitbang profiled bitstream, 8-10 chunks at a time
+// Max time taken per call is ~7us
+void AP_IOMCU_FW::profiled_update(void)
+{
+    if (profiled_num_led_pushed > PROFILED_LED_LEN) {
+        profiled_byte_index = 0;
+        profiled_leading_zeros = PROFILED_LEADING_ZEROS;
+        return;
+    }
+
+    // push 10 zero leading bits at a time
+    if (profiled_leading_zeros != 0) {
+        for (uint8_t i = 0; i < 10; i++) {
+            palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+            profiled_leading_zeros--;
+        }
+        return;
+    }
+
+    if ((profiled_byte_index == 0) ||
+        (profiled_byte_index == PROFILED_OUTPUT_BYTE_LEN)) {
+        // start bit
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palSetLine(HAL_GPIO_PIN_SAFETY_LED);
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        profiled_byte_index = 0;
+        profiled_num_led_pushed++;
+    }
+
+    uint8_t byte_val = profiled_brg[profiled_byte_index];
+    for (uint8_t i = 0; i < 8; i++) {
+        palClearLine(HAL_GPIO_PIN_SAFETY_INPUT);
+        palWriteLine(HAL_GPIO_PIN_SAFETY_LED, byte_val & 1);
+        byte_val >>= 1;
+        palSetLine(HAL_GPIO_PIN_SAFETY_INPUT);
+    }
+
+    profiled_byte_index++;
+}
+#endif
 
 /*
   update safety state
@@ -1076,6 +1136,15 @@ void AP_IOMCU_FW::safety_update(void)
     }
     safety_update_ms = now;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    if (profiled_control_enabled) {
+        // set line mode to output for safety input pin
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_INPUT, PAL_MODE_OUTPUT_PUSHPULL);
+        palSetLineMode(HAL_GPIO_PIN_SAFETY_LED, PAL_MODE_OUTPUT_PUSHPULL);
+        return;
+    }
+#endif
+
     bool safety_pressed = palReadLine(HAL_GPIO_PIN_SAFETY_INPUT);
     if (safety_pressed) {
         if (reg_status.flag_safety_off && (reg_setup.arming & P_SETUP_ARMING_SAFETY_DISABLE_ON)) {

libraries/AP_IOMCU/iofirmware/iofirmware.h

@@ -17,6 +17,10 @@
 #define PWM_IGNORE_THIS_CHANNEL UINT16_MAX
 #define SERVO_COUNT 8
 
+#define PROFILED_LED_LEN         2
+#define PROFILED_OUTPUT_BYTE_LEN 3
+#define PROFILED_LEADING_ZEROS   50
+
 class AP_IOMCU_FW {
 public:
     void process_io_packet();
@@ -36,6 +40,9 @@ public:
     bool handle_code_write();
     bool handle_code_read();
     void schedule_reboot(uint32_t time_ms);
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    void profiled_update();
+#endif
     void safety_update();
     void rcout_config_update();
     void rcin_serial_init();
@@ -81,6 +88,14 @@ public:
 
     uint16_t last_channel_mask;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    uint8_t profiled_byte_index;
+    uint8_t profiled_leading_zeros;
+    uint8_t profiled_num_led_pushed;
+    uint8_t profiled_brg[3];
+    bool profiled_control_enabled;
+#endif
+
     // CONFIG values
     struct page_config config;
 
@@ -117,6 +132,11 @@ public:
     // output mode values
     struct page_mode_out mode_out;
 
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    // profiled control values
+    struct page_profiled profiled;
+#endif
+
     uint16_t last_output_mode_mask;
     uint16_t last_output_bdmask;
     uint16_t last_output_esc_type;

libraries/AP_IOMCU/iofirmware/ioprotocol.h

@@ -7,6 +7,10 @@
   common protocol definitions between AP_IOMCU and iofirmware
  */
 
+#ifndef AP_IOMCU_PROFILED_SUPPORT_ENABLED
+#define AP_IOMCU_PROFILED_SUPPORT_ENABLED 0
+#endif
+
 // 22 is enough for the rc_input page in one transfer
 #define PKT_MAX_REGS 22
 // The number of channels that can be propagated - due to SBUS_OUT is higher than the physical channels
@@ -68,6 +72,9 @@ enum iopage {
     PAGE_RAW_DSHOT_TELEM_5_8 = 205,
     PAGE_RAW_DSHOT_TELEM_9_12 = 206,
     PAGE_RAW_DSHOT_TELEM_13_16 = 207,
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+    PAGE_PROFILED = 208,
+#endif
 };
 
 // setup page registers
@@ -114,6 +121,8 @@ enum iopage {
 #define PAGE_REG_SETUP_FORCE_SAFETY_ON  14
 #define FORCE_SAFETY_MAGIC 22027
 
+#define PROFILED_ENABLE_MAGIC 123
+
 struct page_config {
     uint16_t protocol_version;
     uint16_t protocol_version2;
@@ -230,3 +239,12 @@ struct page_dshot_telem {
     uint8_t   edt2_stress[4];
 #endif
 };
+
+#if AP_IOMCU_PROFILED_SUPPORT_ENABLED
+struct __attribute__((packed, aligned(2))) page_profiled {
+    uint8_t magic;
+    uint8_t blue;
+    uint8_t red;
+    uint8_t green;
+};
+#endif

libraries/AP_Logger/LogStructure.h

@@ -556,6 +556,7 @@ struct PACKED log_Performance {
     uint32_t i2c_count;
     uint32_t i2c_isr_count;
     uint32_t extra_loop_us;
+    uint64_t rtc;
 };
 
 struct PACKED log_SRTL {
@@ -895,14 +896,15 @@ struct PACKED log_VER {
 // @Field: MaxT: Maximum loop time
 // @Field: Mem: Free memory available
 // @Field: Load: System processor load
-// @Field: IntE: Internal error mask; which internal errors have been detected
+// @Field: InE: Internal error mask; which internal errors have been detected
 // @FieldBitmaskEnum: IntE: AP_InternalError::error_t
 // @Field: ErrL: Internal error line number; last line number on which a internal error was detected
-// @Field: ErrC: Internal error count; how many internal errors have been detected
+// @Field: ErC: Internal error count; how many internal errors have been detected
 // @Field: SPIC: Number of SPI transactions processed
 // @Field: I2CC: Number of i2c transactions processed
 // @Field: I2CI: Number of i2c interrupts serviced
 // @Field: Ex: number of microseconds being added to each loop to address scheduler overruns
+// @Field: R: RTC time, time since Unix epoch
 
 // @LoggerMessage: POWR
 // @Description: System power information
@@ -1211,7 +1213,7 @@ LOG_STRUCTURE_FROM_MOUNT \
     LOG_STRUCTURE_FROM_BEACON                                       \
     LOG_STRUCTURE_FROM_PROXIMITY                                    \
     { LOG_PERFORMANCE_MSG, sizeof(log_Performance),                     \
-      "PM",  "QHHHIIHHIIIIII", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,IntE,ErrC,SPIC,I2CC,I2CI,Ex", "sz---b%------s", "F----0A------F" }, \
+      "PM",  "QHHHIIHHIIIIIIQ", "TimeUS,LR,NLon,NL,MaxT,Mem,Load,ErrL,InE,ErC,SPIC,I2CC,I2CI,Ex,R", "sz---b%------ss", "F----0A------FF" }, \
     { LOG_SRTL_MSG, sizeof(log_SRTL), \
       "SRTL", "QBHHBfff", "TimeUS,Active,NumPts,MaxPts,Action,N,E,D", "s----mmm", "F----000" }, \
 LOG_STRUCTURE_FROM_AVOIDANCE \

libraries/AP_Networking/AP_Networking_tests.cpp

@@ -41,7 +41,7 @@ void AP_Networking::start_tests(void)
     if (param.tests & TEST_CONNECTOR_LOOPBACK) {
         hal.scheduler->thread_create(FUNCTOR_BIND_MEMBER(&AP_Networking::test_connector_loopback, void),
                                      "connector_loopback",
-                                     8192, AP_HAL::Scheduler::PRIORITY_UART, -1);
+                                     8192, AP_HAL::Scheduler::PRIORITY_IO, -1);
     }
 }
 

libraries/AP_Notify/AP_Notify.cpp

@@ -42,6 +42,7 @@
 #include "ProfiLED.h"
 #include "ScriptingLED.h"
 #include "DShotLED.h"
+#include "ProfiLED_IOMCU.h"
 
 extern const AP_HAL::HAL& hal;
 
@@ -115,7 +116,11 @@ AP_Notify *AP_Notify::_singleton;
 #endif  // defined (DEFAULT_NTF_LED_TYPES)
 
 #ifndef DEFAULT_NTF_LED_TYPES
-    #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | I2C_LEDS)
+    #if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+      #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | Notify_LED_ProfiLED_IOMCU | I2C_LEDS)
+    #else
+        #define DEFAULT_NTF_LED_TYPES (Notify_LED_Board | I2C_LEDS)
+    #endif
 #endif // DEFAULT_NTF_LED_TYPES
 
 #ifndef BUZZER_ENABLE_DEFAULT
@@ -203,7 +208,7 @@ const AP_Param::GroupInfo AP_Notify::var_info[] = {
     // @Param: LED_TYPES
     // @DisplayName: LED Driver Types
     // @Description: Controls what types of LEDs will be enabled
-    // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI, 13:LP5562 External, 14: LP5562 Internal, 15:IS31FL3195 External, 16: IS31FL3195 Internal, 17: DiscreteRGB, 18: NeoPixelRGB
+    // @Bitmask: 0:Built-in LED, 1:Internal ToshibaLED, 2:External ToshibaLED, 3:External PCA9685, 4:Oreo LED, 5:DroneCAN, 6:NCP5623 External, 7:NCP5623 Internal, 8:NeoPixel, 9:ProfiLED, 10:Scripting, 11:DShot, 12:ProfiLED_SPI, 13:LP5562 External, 14: LP5562 Internal, 15:IS31FL3195 External, 16: IS31FL3195 Internal, 17: DiscreteRGB, 18: NeoPixelRGB, 19:ProfiLED_IOMCU
     // @User: Advanced
     AP_GROUPINFO("LED_TYPES", 6, AP_Notify, _led_type, DEFAULT_NTF_LED_TYPES),
 
@@ -368,6 +373,11 @@ void AP_Notify::add_backends(void)
                 ADD_BACKEND(NEW_NOTHROW DShotLED());
                 break;
 #endif
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+            case Notify_LED_ProfiLED_IOMCU:
+                ADD_BACKEND(NEW_NOTHROW ProfiLED_IOMCU());
+                break;
+#endif
 #if AP_NOTIFY_LP5562_ENABLED
             case Notify_LED_LP5562_I2C_External:
                 FOREACH_I2C_EXTERNAL(b) {

libraries/AP_Notify/AP_Notify.h

@@ -17,6 +17,7 @@
 #include <AP_Common/AP_Common.h>
 #include <AP_Param/AP_Param.h>
 #include "AP_Notify_config.h"
+#include <AP_IOMCU/AP_IOMCU.h>
 
 #include "NotifyDevice.h"
 
@@ -97,6 +98,9 @@ public:
 #endif
 #if AP_NOTIFY_NEOPIXEL_ENABLED
         Notify_LED_NeoPixelRGB              = (1 << 18), // NeoPixel AdaFruit 4544 Worldsemi WS2811
+#endif
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+        Notify_LED_ProfiLED_IOMCU           = (1 << 19), // ProfiLED IOMCU
 #endif
         Notify_LED_MAX
     };

libraries/AP_Notify/ProfiLED_IOMCU.h

@@ -0,0 +1,40 @@
+/*
+   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 "RGBLed.h"
+#include <AP_Common/AP_Common.h>
+#include <AP_IOMCU/AP_IOMCU.h>
+
+#if HAL_WITH_IO_MCU && AP_IOMCU_PROFILED_SUPPORT_ENABLED
+
+class ProfiLED_IOMCU : public RGBLed {
+public:
+    ProfiLED_IOMCU() : RGBLed(0, 0xFF, 0x7F, 0x33) {}
+
+    bool init(void) override { return true; }
+
+protected:
+    bool hw_set_rgb(uint8_t r, uint8_t g, uint8_t b) override {
+        const auto iomcu = AP::iomcu();
+        if (iomcu == nullptr) {
+            return false;
+        }
+        iomcu->set_profiled(r, g, b);
+        return true;
+    }
+};
+
+#endif

libraries/AP_RTC/AP_RTC.cpp

@@ -9,6 +9,12 @@
 #include <GCS_MAVLink/GCS.h>
 #include <AP_Common/time.h>
 
+#define DEBUG_RTC_SHIFT 0
+
+#if DEBUG_RTC_SHIFT
+#include <AP_Logger/AP_Logger.h>
+#endif
+
 extern const AP_HAL::HAL& hal;
 
 AP_RTC::AP_RTC()
@@ -47,9 +53,19 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
 {
     const uint64_t oldest_acceptable_date_us = 1640995200ULL*1000*1000; // 2022-01-01 0:00
 
-    if (type >= rtc_source_type) {
-        // e.g. system-time message when we've been set by the GPS
-        return;
+    // only allow time to be moved forward from the same sourcetype
+    // while the vehicle is disarmed:
+    if (hal.util->get_soft_armed()) {
+        if (type >= rtc_source_type) {
+            // e.g. system-time message when we've been set by the GPS
+            return;
+        }
+    } else {
+        // vehicle is disarmed; accept (e.g.) GPS time source updates
+        if (type > rtc_source_type) {
+            // e.g. system-time message when we've been set by the GPS
+            return;
+        }
     }
 
     // check it's from an allowed sources:
@@ -70,6 +86,11 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
     }
     WITH_SEMAPHORE(rsem);
 
+#if DEBUG_RTC_SHIFT
+    uint64_t old_utc = 0;
+    UNUSED_RESULT(get_utc_usec(old_utc));
+#endif
+
     rtc_shift = tmp;
 
     // update hardware clock:
@@ -83,6 +104,32 @@ void AP_RTC::set_utc_usec(uint64_t time_utc_usec, source_type type)
     // update signing timestamp
     GCS_MAVLINK::update_signing_timestamp(time_utc_usec);
 #endif
+
+#if DEBUG_RTC_SHIFT
+    uint64_t new_utc = 0;
+    UNUSED_RESULT(get_utc_usec(new_utc));
+    if (old_utc != new_utc) {
+        if (AP::logger().should_log(0xFFFF)){
+            // log to AP_Logger
+            // @LoggerMessage: RTC
+            // @Description: Information about RTC clock resets
+            // @Field: TimeUS: Time since system startup
+            // @Field: old: old time
+            // @Field: new: new time
+            // @Field: in: new argument time
+            AP::logger().WriteStreaming(
+                "RTC",
+                "TimeUS,old_utc,new_utc",
+                "sss",
+                "FFF",
+                "QQQ",
+                AP_HAL::micros64(),
+                old_utc,
+                new_utc
+                );
+        }
+    }
+#endif
 }
 
 bool AP_RTC::get_utc_usec(uint64_t &usec) const

libraries/AP_Scheduler/AP_Scheduler.cpp

@@ -447,6 +447,11 @@ void AP_Scheduler::update_logging()
 // Write a performance monitoring packet
 void AP_Scheduler::Log_Write_Performance()
 {
+    uint64_t rtc = 0;
+#if AP_RTC_ENABLED
+    UNUSED_RESULT(AP::rtc().get_utc_usec(rtc));
+#endif
+
     const AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
     struct log_Performance pkt = {
         LOG_PACKET_HEADER_INIT(LOG_PERFORMANCE_MSG),
@@ -464,6 +469,7 @@ void AP_Scheduler::Log_Write_Performance()
         i2c_count        : pd.i2c_count,
         i2c_isr_count    : pd.i2c_isr_count,
         extra_loop_us    : extra_loop_us,
+        rtc              : rtc,
     };
     AP::logger().WriteCriticalBlock(&pkt, sizeof(pkt));
 }

wscript

@@ -429,6 +429,11 @@ configuration in order to save typing.
                  default=0,
                  help='zero time on boot in microseconds')
 
+    g.add_option('--enable-iomcu-profiled-support',
+                    action='store_true',
+                    default=False,
+                    help='enable iomcu profiled support')
+
     g.add_option('--enable-new-checking',
         action='store_true',
         default=False,