ArduPilot · peterbarker · Nov 27, 2023 · Nov 24, 2023 · Nov 24, 2023 · Nov 24, 2023
diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp
@@ -119,6 +119,9 @@ void AP_CANManager::init()
 {
     WITH_SEMAPHORE(_sem);
 
+    // we need to mutate the HAL to install new CAN interfaces
+    AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();
+
 #if CONFIG_HAL_BOARD == HAL_BOARD_SITL
     if (AP::sitl() == nullptr) {
         AP_HAL::panic("CANManager: SITL not initialised!");
@@ -146,17 +149,17 @@ void AP_CANManager::init()
         }
         drv_num--;
 
-        if (hal.can[i] == nullptr) {
+        if (hal_mutable.can[i] == nullptr) {
             // So if this interface is not allocated allocate it here,
             // also pass the index of the CANBus
-            const_cast <AP_HAL::HAL&> (hal).can[i] = new HAL_CANIface(i);
+            hal_mutable.can[i] = new HAL_CANIface(i);
         }
 
         // Initialise the interface we just allocated
-        if (hal.can[i] == nullptr) {
+        if (hal_mutable.can[i] == nullptr) {
             continue;
         }
-        AP_HAL::CANIface* iface = hal.can[i];
+        AP_HAL::CANIface* iface = hal_mutable.can[i];
 
         // Find the driver type that we need to allocate and register this interface with
         drv_type[drv_num] = (AP_CAN::Protocol) _drv_param[drv_num]._driver_type.get();
@@ -166,13 +169,13 @@ void AP_CANManager::init()
 #if AP_CAN_SLCAN_ENABLED
         if (_slcan_interface.init_passthrough(i)) {
             // we have slcan bridge setup pass that on as can iface
-            can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode);
+            can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode);
             iface = &_slcan_interface;
         } else {
 #else
         if (true) {
 #endif
-            can_initialised = hal.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode);
+            can_initialised = hal_mutable.can[i]->init(_interfaces[i]._bitrate, _interfaces[i]._fdbitrate*1000000, AP_HAL::CANIface::NormalMode);
         }
 
         if (!can_initialised) {
@@ -245,8 +248,8 @@ void AP_CANManager::init()
         bool enable_filter = false;
         for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
             if (_interfaces[i]._driver_number == (drv_num+1) &&
-                hal.can[i] != nullptr &&
-                hal.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) {
+                hal_mutable.can[i] != nullptr &&
+                hal_mutable.can[i]->get_operating_mode() == AP_HAL::CANIface::FilteredMode) {
                 // Don't worry we don't enable Filters for Normal Ifaces under the driver
                 // this is just to ensure we enable them for the ones we already decided on
                 enable_filter = true;
@@ -284,6 +287,9 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver
 {
     WITH_SEMAPHORE(_sem);
 
+    // we need to mutate the HAL to install new CAN interfaces
+    AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();
+
     for (uint8_t i = 0; i < HAL_NUM_CAN_IFACES; i++) {
         uint8_t drv_num = _interfaces[i]._driver_number;
         if (drv_num == 0 || drv_num > HAL_MAX_CAN_PROTOCOL_DRIVERS) {
@@ -302,17 +308,17 @@ bool AP_CANManager::register_driver(AP_CAN::Protocol dtype, AP_CANDriver *driver
             continue;
         }
 
-        if (hal.can[i] == nullptr) {
+        if (hal_mutable.can[i] == nullptr) {
             // if this interface is not allocated allocate it here,
             // also pass the index of the CANBus
-            const_cast <AP_HAL::HAL&> (hal).can[i] = new HAL_CANIface(i);
+            hal_mutable.can[i] = new HAL_CANIface(i);
         }
 
         // Initialise the interface we just allocated
-        if (hal.can[i] == nullptr) {
+        if (hal_mutable.can[i] == nullptr) {
             continue;
         }
-        AP_HAL::CANIface* iface = hal.can[i];
+        AP_HAL::CANIface* iface = hal_mutable.can[i];
 
         _drivers[drv_num] = driver;
         _drivers[drv_num]->add_interface(iface);

diff --git a/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp b/libraries/AP_DroneCAN/examples/DroneCAN_sniffer/DroneCAN_sniffer.cpp
@@ -117,15 +117,18 @@ static void cb_GetNodeInfoRequest(const CanardRxTransfer &transfer, const uavcan
 
 void DroneCAN_sniffer::init(void)
 {
-    const_cast <AP_HAL::HAL&> (hal).can[driver_index] = new HAL_CANIface(driver_index);
+    // we need to mutate the HAL to install new CAN interfaces
+    AP_HAL::HAL& hal_mutable = AP_HAL::get_HAL_mutable();
+
+    hal_mutable.can[driver_index] = new HAL_CANIface(driver_index);
 
-    if (hal.can[driver_index] == nullptr) {
+    if (hal_mutable.can[driver_index] == nullptr) {
         AP_HAL::panic("Couldn't allocate CANManager, something is very wrong");
     }
 
-    hal.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode);
+    hal_mutable.can[driver_index]->init(1000000, AP_HAL::CANIface::NormalMode);
 
-    if (!hal.can[driver_index]->is_initialized()) {
+    if (!hal_mutable.can[driver_index]->is_initialized()) {
         debug_dronecan("Can not initialised\n");
         return;
     }
@@ -135,7 +138,7 @@ void DroneCAN_sniffer::init(void)
         return;
     }
 
-    if (!_uavcan_iface_mgr->add_interface(hal.can[driver_index])) {
+    if (!_uavcan_iface_mgr->add_interface(hal_mutable.can[driver_index])) {
         debug_dronecan("Failed to add iface");
         return;
     }

diff --git a/libraries/AP_HAL/AP_HAL_Namespace.h b/libraries/AP_HAL/AP_HAL_Namespace.h
@@ -67,6 +67,7 @@ namespace AP_HAL {
 
     class SIMState;
 
-    // Must be implemented by the concrete HALs.
+    // Must be implemented by the concrete HALs and return the same reference.
     const HAL& get_HAL();
+    HAL& get_HAL_mutable();
 }
diff --git a/libraries/AP_HAL_ChibiOS/CANFDIface.cpp b/libraries/AP_HAL_ChibiOS/CANFDIface.cpp
@@ -85,7 +85,7 @@
 #error "Unsupported MCU for FDCAN"
 #endif
 
-extern AP_HAL::HAL& hal;
+extern const AP_HAL::HAL& hal;
 
 #define STR(x) #x
 #define XSTR(x) STR(x)
@@ -568,7 +568,7 @@ bool CANIface::init(const uint32_t bitrate, const uint32_t fdbitrate, const Oper
     if (can_ifaces[self_index_] == nullptr) {
         can_ifaces[self_index_] = this;
 #if !defined(HAL_BOOTLOADER_BUILD)
-        hal.can[self_index_] = this;
+        AP_HAL::get_HAL_mutable().can[self_index_] = this;
 #endif
     }
 

diff --git a/libraries/AP_HAL_ChibiOS/CanIface.cpp b/libraries/AP_HAL_ChibiOS/CanIface.cpp
@@ -84,7 +84,7 @@
 #endif
 
 
-extern AP_HAL::HAL& hal;
+extern const AP_HAL::HAL& hal;
 
 using namespace ChibiOS;
 
@@ -846,7 +846,7 @@ bool CANIface::init(const uint32_t bitrate, const CANIface::OperatingMode mode)
     if (can_ifaces[self_index_] == nullptr) {
         can_ifaces[self_index_] = this;
 #if !defined(HAL_BOOTLOADER_BUILD)
-        hal.can[self_index_] = this;
+        AP_HAL::get_HAL_mutable().can[self_index_] = this;
 #endif
     }
 

diff --git a/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp b/libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp
@@ -344,8 +344,13 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
     main_loop();
 }
 
+static HAL_ChibiOS hal_chibios;
+
 const AP_HAL::HAL& AP_HAL::get_HAL() {
-    static const HAL_ChibiOS hal_chibios;
+    return hal_chibios;
+}
+
+AP_HAL::HAL& AP_HAL::get_HAL_mutable() {
     return hal_chibios;
 }
 

diff --git a/libraries/AP_HAL_ESP32/system.cpp b/libraries/AP_HAL_ESP32/system.cpp
@@ -56,8 +56,14 @@ uint64_t millis64()
 
 } // namespace AP_HAL
 
+static HAL_ESP32 hal_esp32;
+
 const AP_HAL::HAL& AP_HAL::get_HAL()
 {
-    static const HAL_ESP32 hal;
-    return hal;
+    return hal_esp32;
+}
+
+AP_HAL::HAL& AP_HAL::get_HAL_mutable()
+{
+    return hal_esp32;
 }
diff --git a/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp b/libraries/AP_HAL_Empty/HAL_Empty_Class.cpp
@@ -66,9 +66,14 @@ void HAL_Empty::run(int argc, char* const argv[], Callbacks* callbacks) const
     }
 }
 
+static HAL_Empty hal_empty;
+
 const AP_HAL::HAL& AP_HAL::get_HAL() {
-    static const HAL_Empty hal;
-    return hal;
+    return hal_empty;
+}
+
+AP_HAL::HAL& AP_HAL::get_HAL_mutable() {
+    return hal_empty;
 }
 
 #endif
diff --git a/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp b/libraries/AP_HAL_Linux/HAL_Linux_Class.cpp
@@ -491,3 +491,8 @@ const AP_HAL::HAL &AP_HAL::get_HAL()
 {
     return hal_linux;
 }
+
+AP_HAL::HAL &AP_HAL::get_HAL_mutable()
+{
+    return hal_linux;
+}
diff --git a/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp b/libraries/AP_HAL_SITL/HAL_SITL_Class.cpp
@@ -33,7 +33,7 @@
 
 using namespace HALSITL;
 
-HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL();
+HAL_SITL& hal_sitl = (HAL_SITL&)AP_HAL::get_HAL_mutable();
 
 static Storage sitlStorage;
 static SITL_State sitlState;
@@ -299,9 +299,14 @@ void HAL_SITL::actually_reboot()
     AP_HAL::panic("PANIC: REBOOT FAILED: %s", strerror(errno));
 }
 
+static HAL_SITL hal_sitl_inst;
+
 const AP_HAL::HAL& AP_HAL::get_HAL() {
-    static const HAL_SITL hal;
-    return hal;
+    return hal_sitl_inst;
+}
+
+AP_HAL::HAL& AP_HAL::get_HAL_mutable() {
+    return hal_sitl_inst;
 }
 
 #endif  // CONFIG_HAL_BOARD == HAL_BOARD_SITL