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 <cstring>
#include <numbers>
#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"