diff --git a/hal/src/main/native/systemcore/IMU.cpp b/hal/src/main/native/systemcore/IMU.cpp index 91e3c426a6..05b7772d76 100644 --- a/hal/src/main/native/systemcore/IMU.cpp +++ b/hal/src/main/native/systemcore/IMU.cpp @@ -4,160 +4,166 @@ #include "wpi/hal/IMU.h" +#include #include #include "HALInitializer.hpp" -#include "SystemServerInternal.hpp" +#include "mrclib/IMU.h" #include "wpi/hal/Errors.h" -#include "wpi/nt/DoubleArrayTopic.hpp" -#include "wpi/nt/DoubleTopic.hpp" -#include "wpi/nt/NetworkTableInstance.hpp" - -#define IMU_PREFIX "/imu/" namespace { -constexpr const char* kRawAccelKey = IMU_PREFIX "rawaccel"; -constexpr const char* kRawGyroKey = IMU_PREFIX "rawgyro"; -constexpr const char* kQuaternionKey = IMU_PREFIX "quat"; -constexpr const char* kEulerAnglesFlatKey = IMU_PREFIX "euler_flat"; -constexpr const char* kEulerAnglesLandscapeKey = IMU_PREFIX "euler_landscape"; -constexpr const char* kEulerAnglesPortraitKey = IMU_PREFIX "euler_portrait"; -constexpr const char* kYawFlatKey = IMU_PREFIX "yaw_flat"; -constexpr const char* kYawLandscapeKey = IMU_PREFIX "yaw_landscape"; -constexpr const char* kYawPortraitKey = IMU_PREFIX "yaw_portrait"; - -struct IMU { - explicit IMU(wpi::nt::NetworkTableInstance inst) - : rawAccelSub{inst.GetDoubleArrayTopic(kRawAccelKey).Subscribe({})}, - rawGyroSub{inst.GetDoubleArrayTopic(kRawGyroKey).Subscribe({})}, - quatSub{inst.GetDoubleArrayTopic(kQuaternionKey).Subscribe({})}, - eulerFlatSub{ - inst.GetDoubleArrayTopic(kEulerAnglesFlatKey).Subscribe({})}, - eulerLandscapeSub{ - inst.GetDoubleArrayTopic(kEulerAnglesLandscapeKey).Subscribe({})}, - eulerPortraitSub{ - inst.GetDoubleArrayTopic(kEulerAnglesPortraitKey).Subscribe({})}, - yawFlatSub{inst.GetDoubleTopic(kYawFlatKey).Subscribe(0)}, - yawLandscapeSub{inst.GetDoubleTopic(kYawLandscapeKey).Subscribe(0)}, - yawPortraitSub{inst.GetDoubleTopic(kYawPortraitKey).Subscribe(0)} {} - - wpi::nt::DoubleArraySubscriber rawAccelSub; - wpi::nt::DoubleArraySubscriber rawGyroSub; - wpi::nt::DoubleArraySubscriber quatSub; - wpi::nt::DoubleArraySubscriber eulerFlatSub; - wpi::nt::DoubleArraySubscriber eulerLandscapeSub; - wpi::nt::DoubleArraySubscriber eulerPortraitSub; - wpi::nt::DoubleSubscriber yawFlatSub; - wpi::nt::DoubleSubscriber yawLandscapeSub; - wpi::nt::DoubleSubscriber yawPortraitSub; +struct IMUInitializer { + MRC_Status status = MRC_IMU_Initialize(); }; -static IMU* imu; +static MRC_Status InitMrcImu() { + static IMUInitializer init; + return init.status; +} constexpr double kDegreesToRadians = std::numbers::pi / 180.0; -constexpr double kGToMetersPerSecondSquared = 9.80665; } // namespace namespace wpi::hal::init { -void InitializeIMU() { - static IMU imu_static{wpi::hal::GetSystemServer()}; - imu = &imu_static; -} +void InitializeIMU() {} } // namespace wpi::hal::init extern "C" { void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { - auto update = imu->rawAccelSub.GetAtomic(); - if (update.value.size() != 3) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *accel = - HAL_Acceleration3d{.timestamp = update.time, - .x = update.value[0] * kGToMetersPerSecondSquared, - .y = update.value[1] * kGToMetersPerSecondSquared, - .z = update.value[2] * kGToMetersPerSecondSquared}; + + MRC_Acceleration3d accelData; + std::memset(&accelData, 0, sizeof(accelData)); + *status = MRC_IMU_GetAcceleration(&accelData); + *accel = HAL_Acceleration3d{.timestamp = accelData.timestamp, + .x = accelData.x, + .y = accelData.y, + .z = accelData.z}; } void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { - auto update = imu->rawGyroSub.GetAtomic(); - if (update.value.size() != 3) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *rate = HAL_GyroRate3d{.timestamp = update.time, - .x = update.value[0] * kDegreesToRadians, - .y = update.value[1] * kDegreesToRadians, - .z = update.value[2] * kDegreesToRadians}; + MRC_GyroRate3d rateData; + std::memset(&rateData, 0, sizeof(rateData)); + *status = MRC_IMU_GetGyroRate(&rateData); + *rate = HAL_GyroRate3d{.timestamp = rateData.timestamp, + .x = rateData.x, + .y = rateData.y, + .z = rateData.z}; } void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) { - auto update = imu->eulerFlatSub.GetAtomic(); - if (update.value.size() != 3) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *angles = HAL_EulerAngles3d{.timestamp = update.time, - .x = update.value[0] * kDegreesToRadians, - .y = update.value[1] * kDegreesToRadians, - .z = update.value[2] * kDegreesToRadians}; + + MRC_EulerAngles3d angleData; + std::memset(&angleData, 0, sizeof(angleData)); + *status = MRC_IMU_GetEulerAnglesFlat(&angleData); + *angles = HAL_EulerAngles3d{.timestamp = angleData.timestamp, + .x = angleData.x, + .y = angleData.y, + .z = angleData.z}; } void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles, int32_t* status) { - auto update = imu->eulerLandscapeSub.GetAtomic(); - if (update.value.size() != 3) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *angles = HAL_EulerAngles3d{.timestamp = update.time, - .x = update.value[0] * kDegreesToRadians, - .y = update.value[1] * kDegreesToRadians, - .z = update.value[2] * kDegreesToRadians}; + + MRC_EulerAngles3d angleData; + std::memset(&angleData, 0, sizeof(angleData)); + *status = MRC_IMU_GetEulerAnglesLandscape(&angleData); + *angles = HAL_EulerAngles3d{.timestamp = angleData.timestamp, + .x = angleData.x, + .y = angleData.y, + .z = angleData.z}; } void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) { - auto update = imu->eulerPortraitSub.GetAtomic(); - if (update.value.size() != 3) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *angles = HAL_EulerAngles3d{.timestamp = update.time, - .x = update.value[0] * kDegreesToRadians, - .y = update.value[1] * kDegreesToRadians, - .z = update.value[2] * kDegreesToRadians}; + + MRC_EulerAngles3d angleData; + std::memset(&angleData, 0, sizeof(angleData)); + *status = MRC_IMU_GetEulerAnglesPortrait(&angleData); + *angles = HAL_EulerAngles3d{.timestamp = angleData.timestamp, + .x = angleData.x, + .y = angleData.y, + .z = angleData.z}; } void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) { - auto update = imu->quatSub.GetAtomic(); - if (update.value.size() != 4) { - *status = HAL_INCOMPATIBLE_STATE; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *status = initStatus; return; } - *quat = HAL_Quaternion{.timestamp = update.time, - .w = update.value[0], - .x = update.value[1], - .y = update.value[2], - .z = update.value[3]}; + + MRC_Quaternion quatData; + std::memset(&quatData, 0, sizeof(quatData)); + *status = MRC_IMU_GetQuaternion(&quatData); + *quat = HAL_Quaternion{.timestamp = quatData.timestamp, + .w = quatData.w, + .x = quatData.x, + .y = quatData.y, + .z = quatData.z}; } double HAL_GetIMUYawFlat(int64_t* timestamp) { - auto update = imu->yawFlatSub.GetAtomic(); - *timestamp = update.time; - return update.value * kDegreesToRadians; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *timestamp = 0; + return 0; + } + + double ret = 0; + *timestamp = 0; + MRC_IMU_GetYawFlat(&ret, timestamp); + return ret; } double HAL_GetIMUYawLandscape(int64_t* timestamp) { - auto update = imu->yawLandscapeSub.GetAtomic(); - *timestamp = update.time; - return update.value * kDegreesToRadians; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *timestamp = 0; + return 0; + } + + double ret = 0; + *timestamp = 0; + MRC_IMU_GetYawLandscape(&ret, timestamp); + return ret; } double HAL_GetIMUYawPortrait(int64_t* timestamp) { - auto update = imu->yawPortraitSub.GetAtomic(); - *timestamp = update.time; - return update.value * kDegreesToRadians; + MRC_Status initStatus = InitMrcImu(); + if (initStatus != MRC_STATUS_SUCCESS) { + *timestamp = 0; + return 0; + } + + double ret = 0; + *timestamp = 0; + MRC_IMU_GetYawPortrait(&ret, timestamp); + return ret; } } // extern "C"