diff --git a/libraries/AP_CANManager/AP_CANManager.cpp b/libraries/AP_CANManager/AP_CANManager.cpp index 78be64a97c66c..9275ea2ba05ed 100644 --- 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 (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 (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 index 97a7850d41efd..5745e94b761d0 100644 --- 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 (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 index b3e84314ed9ef..aca8e02293763 100644 --- 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 index 2d261f7242084..eaefd34324c69 100644 --- 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 index 677f484804943..97cd8acf2ad1b 100644 --- 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 index 6454f108e85ee..46b352e5649f5 100644 --- 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 index 20f07a014758b..4ee607c5617b7 100644 --- 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 index b2271a7df8eb8..2045e9c2893ee 100644 --- 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 index ba68930e89a78..0fc691598b092 100644 --- 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 index 8a7537ab9730f..bf8bc07351097 100644 --- 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