diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index 27a0f4a729..b3fd01c6c4 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -293,6 +293,9 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { return true; } + // Initialize system server first, other things might use it + hal::InitializeSystemServer(); + hal::init::InitializeHAL(); hal::init::HAL_IsInitialized.store(true); @@ -307,8 +310,6 @@ HAL_Bool HAL_Initialize(int32_t timeout, int32_t mode) { return false; } - hal::InitializeSystemServer(); - // // Return false if program failed to kill an existing program // if (!killExistingProgram(timeout, mode)) { // return false; diff --git a/hal/src/main/native/systemcore/Power.cpp b/hal/src/main/native/systemcore/Power.cpp index 4c8c2df51f..08ecbd2d94 100644 --- a/hal/src/main/native/systemcore/Power.cpp +++ b/hal/src/main/native/systemcore/Power.cpp @@ -6,8 +6,12 @@ #include +#include + #include "HALInitializer.h" +#include "SystemServerInternal.h" #include "hal/Errors.h" +#include "mrc/NtNetComm.h" using namespace hal; @@ -19,16 +23,34 @@ static void initializePower(int32_t* status) { } // namespace hal +namespace { +struct SystemServerPower { + nt::NetworkTableInstance ntInst; + + nt::DoubleSubscriber batterySubscriber; + + explicit SystemServerPower(nt::NetworkTableInstance inst) { + ntInst = inst; + + batterySubscriber = + ntInst.GetDoubleTopic(ROBOT_BATTERY_VOLTAGE_PATH).Subscribe(0.0); + } +}; +} // namespace + +static ::SystemServerPower* systemServerPower; + namespace hal::init { -void InitializePower() {} +void InitializePower() { + systemServerPower = new ::SystemServerPower{hal::GetSystemServer()}; +} } // namespace hal::init extern "C" { double HAL_GetVinVoltage(int32_t* status) { initializePower(status); - *status = HAL_HANDLE_ERROR; - return 0; + return systemServerPower->batterySubscriber.Get(); } double HAL_GetUserVoltage3V3(int32_t* status) {