Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-19 00:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal] Switch IMU to mrclib (#8961)

Browse Source
This commit is contained in:
Thad House
2026-06-06 16:30:08 -07:00
committed by GitHub
parent 71ed28f768
commit 06fb36ba8e
1 changed files with 105 additions and 99 deletions
Download Patch File Download Diff File Expand all files Collapse all files

204
hal/src/main/native/systemcore/IMU.cpp
View File

@@ -4,160 +4,166 @@
#include "wpi/hal/IMU.h" #include "wpi/hal/IMU.h"
#include <cstring>
#include <numbers> #include <numbers>
#include "HALInitializer.hpp" #include "HALInitializer.hpp"
#include "SystemServerInternal.hpp" #include "mrclib/IMU.h"
#include "wpi/hal/Errors.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 { namespace {
constexpr const char* kRawAccelKey = IMU_PREFIX "rawaccel"; struct IMUInitializer {
constexpr const char* kRawGyroKey = IMU_PREFIX "rawgyro"; MRC_Status status = MRC_IMU_Initialize();
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;
}; };
static IMU* imu; static MRC_Status InitMrcImu() {
static IMUInitializer init;
return init.status;
}
constexpr double kDegreesToRadians = std::numbers::pi / 180.0; constexpr double kDegreesToRadians = std::numbers::pi / 180.0;
constexpr double kGToMetersPerSecondSquared = 9.80665;
} // namespace } // namespace
namespace wpi::hal::init { namespace wpi::hal::init {
void InitializeIMU() { void InitializeIMU() {}
static IMU imu_static{wpi::hal::GetSystemServer()};
imu = &imu_static;
}
} // namespace wpi::hal::init } // namespace wpi::hal::init
extern "C" { extern "C" {
void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) { void HAL_GetIMUAcceleration(HAL_Acceleration3d* accel, int32_t* status) {
auto update = imu->rawAccelSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 3) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*accel =
HAL_Acceleration3d{.timestamp = update.time, MRC_Acceleration3d accelData;
.x = update.value[0] * kGToMetersPerSecondSquared, std::memset(&accelData, 0, sizeof(accelData));
.y = update.value[1] * kGToMetersPerSecondSquared, *status = MRC_IMU_GetAcceleration(&accelData);
.z = update.value[2] * kGToMetersPerSecondSquared}; *accel = HAL_Acceleration3d{.timestamp = accelData.timestamp,
.x = accelData.x,
.y = accelData.y,
.z = accelData.z};
} }
void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) { void HAL_GetIMUGyroRates(HAL_GyroRate3d* rate, int32_t* status) {
auto update = imu->rawGyroSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 3) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*rate = HAL_GyroRate3d{.timestamp = update.time, MRC_GyroRate3d rateData;
.x = update.value[0] * kDegreesToRadians, std::memset(&rateData, 0, sizeof(rateData));
.y = update.value[1] * kDegreesToRadians, *status = MRC_IMU_GetGyroRate(&rateData);
.z = update.value[2] * kDegreesToRadians}; *rate = HAL_GyroRate3d{.timestamp = rateData.timestamp,
.x = rateData.x,
.y = rateData.y,
.z = rateData.z};
} }
void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) { void HAL_GetIMUEulerAnglesFlat(HAL_EulerAngles3d* angles, int32_t* status) {
auto update = imu->eulerFlatSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 3) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*angles = HAL_EulerAngles3d{.timestamp = update.time,
.x = update.value[0] * kDegreesToRadians, MRC_EulerAngles3d angleData;
.y = update.value[1] * kDegreesToRadians, std::memset(&angleData, 0, sizeof(angleData));
.z = update.value[2] * kDegreesToRadians}; *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, void HAL_GetIMUEulerAnglesLandscape(HAL_EulerAngles3d* angles,
int32_t* status) { int32_t* status) {
auto update = imu->eulerLandscapeSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 3) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*angles = HAL_EulerAngles3d{.timestamp = update.time,
.x = update.value[0] * kDegreesToRadians, MRC_EulerAngles3d angleData;
.y = update.value[1] * kDegreesToRadians, std::memset(&angleData, 0, sizeof(angleData));
.z = update.value[2] * kDegreesToRadians}; *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) { void HAL_GetIMUEulerAnglesPortrait(HAL_EulerAngles3d* angles, int32_t* status) {
auto update = imu->eulerPortraitSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 3) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*angles = HAL_EulerAngles3d{.timestamp = update.time,
.x = update.value[0] * kDegreesToRadians, MRC_EulerAngles3d angleData;
.y = update.value[1] * kDegreesToRadians, std::memset(&angleData, 0, sizeof(angleData));
.z = update.value[2] * kDegreesToRadians}; *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) { void HAL_GetIMUQuaternion(HAL_Quaternion* quat, int32_t* status) {
auto update = imu->quatSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
if (update.value.size() != 4) { if (initStatus != MRC_STATUS_SUCCESS) {
*status = HAL_INCOMPATIBLE_STATE; *status = initStatus;
return; return;
} }
*quat = HAL_Quaternion{.timestamp = update.time,
.w = update.value[0], MRC_Quaternion quatData;
.x = update.value[1], std::memset(&quatData, 0, sizeof(quatData));
.y = update.value[2], *status = MRC_IMU_GetQuaternion(&quatData);
.z = update.value[3]}; *quat = HAL_Quaternion{.timestamp = quatData.timestamp,
.w = quatData.w,
.x = quatData.x,
.y = quatData.y,
.z = quatData.z};
} }
double HAL_GetIMUYawFlat(int64_t* timestamp) { double HAL_GetIMUYawFlat(int64_t* timestamp) {
auto update = imu->yawFlatSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
*timestamp = update.time; if (initStatus != MRC_STATUS_SUCCESS) {
return update.value * kDegreesToRadians; *timestamp = 0;
return 0;
}
double ret = 0;
*timestamp = 0;
MRC_IMU_GetYawFlat(&ret, timestamp);
return ret;
} }
double HAL_GetIMUYawLandscape(int64_t* timestamp) { double HAL_GetIMUYawLandscape(int64_t* timestamp) {
auto update = imu->yawLandscapeSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
*timestamp = update.time; if (initStatus != MRC_STATUS_SUCCESS) {
return update.value * kDegreesToRadians; *timestamp = 0;
return 0;
}
double ret = 0;
*timestamp = 0;
MRC_IMU_GetYawLandscape(&ret, timestamp);
return ret;
} }
double HAL_GetIMUYawPortrait(int64_t* timestamp) { double HAL_GetIMUYawPortrait(int64_t* timestamp) {
auto update = imu->yawPortraitSub.GetAtomic(); MRC_Status initStatus = InitMrcImu();
*timestamp = update.time; if (initStatus != MRC_STATUS_SUCCESS) {
return update.value * kDegreesToRadians; *timestamp = 0;
return 0;
}
double ret = 0;
*timestamp = 0;
MRC_IMU_GetYawPortrait(&ret, timestamp);
return ret;
} }
} // extern "C" } // extern "C"