mirror of
https://github.com/wpilibsuite/allwpilib
synced 2026-06-21 01:01:43 +00:00
[wpilib] Add simulation support to ADIS classes (#3857)
This commit is contained in:
Tyler Veness
@@ -70,55 +70,81 @@ ADIS16448_IMU::ADIS16448_IMU() : ADIS16448_IMU(kZ, SPI::Port::kMXP, 4) {}
|
||||
|
||||
ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
|
||||
uint16_t cal_time)
|
||||
: m_yaw_axis(yaw_axis), m_spi_port(port) {
|
||||
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
|
||||
// Relies on the RIO hardware by default configuring an output as low
|
||||
// and configuring an input as high Z. The 10k pull-up resistor internal to
|
||||
// the IMU then forces the reset line high for normal operation.
|
||||
DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
|
||||
Wait(10_ms); // Wait 10ms
|
||||
delete m_reset_out;
|
||||
new DigitalInput(18); // Set MXP DIO8 high
|
||||
Wait(500_ms); // Wait 500ms for reset to complete
|
||||
|
||||
ConfigCalTime(cal_time);
|
||||
|
||||
// Configure standard SPI
|
||||
if (!SwitchToStandardSPI()) {
|
||||
return;
|
||||
: m_yaw_axis(yaw_axis),
|
||||
m_spi_port(port),
|
||||
m_simDevice("Gyro:ADIS16448", port) {
|
||||
if (m_simDevice) {
|
||||
m_simGyroAngleX =
|
||||
m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroAngleY =
|
||||
m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroAngleZ =
|
||||
m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateX =
|
||||
m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateY =
|
||||
m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateZ =
|
||||
m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelX =
|
||||
m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelY =
|
||||
m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelZ =
|
||||
m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
|
||||
}
|
||||
|
||||
// Set IMU internal decimation to 819.2 SPS
|
||||
WriteRegister(SMPL_PRD, 0x0001);
|
||||
// Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
|
||||
WriteRegister(MSC_CTRL, 0x0016);
|
||||
// Disable IMU internal Bartlett filter
|
||||
WriteRegister(SENS_AVG, 0x0400);
|
||||
// Clear offset registers
|
||||
WriteRegister(XGYRO_OFF, 0x0000);
|
||||
WriteRegister(YGYRO_OFF, 0x0000);
|
||||
WriteRegister(ZGYRO_OFF, 0x0000);
|
||||
// Configure and enable auto SPI
|
||||
if (!SwitchToAutoSPI()) {
|
||||
return;
|
||||
if (!m_simDevice) {
|
||||
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
|
||||
// Relies on the RIO hardware by default configuring an output as low
|
||||
// and configuring an input as high Z. The 10k pull-up resistor internal to
|
||||
// the IMU then forces the reset line high for normal operation.
|
||||
DigitalOutput* m_reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
|
||||
Wait(10_ms); // Wait 10ms
|
||||
delete m_reset_out;
|
||||
new DigitalInput(18); // Set MXP DIO8 high
|
||||
Wait(500_ms); // Wait 500ms for reset to complete
|
||||
|
||||
ConfigCalTime(cal_time);
|
||||
|
||||
// Configure standard SPI
|
||||
if (!SwitchToStandardSPI()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Set IMU internal decimation to 819.2 SPS
|
||||
WriteRegister(SMPL_PRD, 0x0001);
|
||||
// Enable Data Ready (LOW = Good Data) on DIO1 (PWM0 on MXP)
|
||||
WriteRegister(MSC_CTRL, 0x0016);
|
||||
// Disable IMU internal Bartlett filter
|
||||
WriteRegister(SENS_AVG, 0x0400);
|
||||
// Clear offset registers
|
||||
WriteRegister(XGYRO_OFF, 0x0000);
|
||||
WriteRegister(YGYRO_OFF, 0x0000);
|
||||
WriteRegister(ZGYRO_OFF, 0x0000);
|
||||
// Configure and enable auto SPI
|
||||
if (!SwitchToAutoSPI()) {
|
||||
return;
|
||||
}
|
||||
// Notify DS that IMU calibration delay is active
|
||||
REPORT_WARNING(
|
||||
"ADIS16448 IMU Detected. Starting initial calibration delay.");
|
||||
// Wait for whatever time the user set as the start-up delay
|
||||
Wait(static_cast<double>(m_calibration_time) * 1.2_s);
|
||||
// Execute calibration routine
|
||||
Calibrate();
|
||||
// Reset accumulated offsets
|
||||
Reset();
|
||||
// Tell the acquire loop that we're done starting up
|
||||
m_start_up_mode = false;
|
||||
|
||||
// Let the user know the IMU was initiallized successfully
|
||||
REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
|
||||
|
||||
// TODO: Find what the proper pin is to turn this LED
|
||||
// Drive MXP PWM5 (IMU ready LED) low (active low)
|
||||
new DigitalOutput(19);
|
||||
}
|
||||
// Notify DS that IMU calibration delay is active
|
||||
REPORT_WARNING("ADIS16448 IMU Detected. Starting initial calibration delay.");
|
||||
// Wait for whatever time the user set as the start-up delay
|
||||
Wait(static_cast<double>(m_calibration_time) * 1.2_s);
|
||||
// Execute calibration routine
|
||||
Calibrate();
|
||||
// Reset accumulated offsets
|
||||
Reset();
|
||||
// Tell the acquire loop that we're done starting up
|
||||
m_start_up_mode = false;
|
||||
|
||||
// Let the user know the IMU was initiallized successfully
|
||||
REPORT_WARNING("ADIS16448 IMU Successfully Initialized!");
|
||||
|
||||
// TODO: Find what the proper pin is to turn this LED
|
||||
// Drive MXP PWM5 (IMU ready LED) low (active low)
|
||||
new DigitalOutput(19);
|
||||
|
||||
// Report usage and post data to DS
|
||||
HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
|
||||
@@ -739,11 +765,11 @@ double ADIS16448_IMU::GetAngle() const {
|
||||
double ADIS16448_IMU::GetRate() const {
|
||||
switch (m_yaw_axis) {
|
||||
case kX:
|
||||
return GetGyroInstantX();
|
||||
return GetGyroRateX();
|
||||
case kY:
|
||||
return GetGyroInstantY();
|
||||
return GetGyroRateY();
|
||||
case kZ:
|
||||
return GetGyroInstantZ();
|
||||
return GetGyroRateZ();
|
||||
default:
|
||||
return 0.0;
|
||||
}
|
||||
@@ -754,46 +780,73 @@ ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroAngleX() const {
|
||||
if (m_simGyroAngleX) {
|
||||
return m_simGyroAngleX.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_integ_gyro_x;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroAngleY() const {
|
||||
if (m_simGyroAngleY) {
|
||||
return m_simGyroAngleY.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_integ_gyro_y;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroAngleZ() const {
|
||||
if (m_simGyroAngleZ) {
|
||||
return m_simGyroAngleZ.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_integ_gyro_z;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroInstantX() const {
|
||||
double ADIS16448_IMU::GetGyroRateX() const {
|
||||
if (m_simGyroRateX) {
|
||||
return m_simGyroRateX.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_x;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroInstantY() const {
|
||||
double ADIS16448_IMU::GetGyroRateY() const {
|
||||
if (m_simGyroRateY) {
|
||||
return m_simGyroRateY.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_y;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetGyroInstantZ() const {
|
||||
double ADIS16448_IMU::GetGyroRateZ() const {
|
||||
if (m_simGyroRateZ) {
|
||||
return m_simGyroRateZ.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_z;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetAccelInstantX() const {
|
||||
double ADIS16448_IMU::GetAccelX() const {
|
||||
if (m_simAccelX) {
|
||||
return m_simAccelX.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_x;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetAccelInstantY() const {
|
||||
double ADIS16448_IMU::GetAccelY() const {
|
||||
if (m_simAccelY) {
|
||||
return m_simAccelY.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_y;
|
||||
}
|
||||
|
||||
double ADIS16448_IMU::GetAccelInstantZ() const {
|
||||
double ADIS16448_IMU::GetAccelZ() const {
|
||||
if (m_simAccelZ) {
|
||||
return m_simAccelZ.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_z;
|
||||
}
|
||||
@@ -843,6 +896,10 @@ double ADIS16448_IMU::GetTemperature() const {
|
||||
return m_temp;
|
||||
}
|
||||
|
||||
int ADIS16448_IMU::GetPort() const {
|
||||
return m_spi_port;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Builds a Sendable object to push IMU data to the driver station.
|
||||
*
|
||||
|
||||
@@ -69,55 +69,80 @@ ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, SPI::Port port,
|
||||
CalibrationTime cal_time)
|
||||
: m_yaw_axis(yaw_axis),
|
||||
m_spi_port(port),
|
||||
m_calibration_time(static_cast<uint16_t>(cal_time)) {
|
||||
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
|
||||
// Relies on the RIO hardware by default configuring an output as low
|
||||
// and configuring an input as high Z. The 10k pull-up resistor internal to
|
||||
// the IMU then forces the reset line high for normal operation.
|
||||
DigitalOutput* m_reset_out =
|
||||
new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
|
||||
Wait(10_ms); // Wait 10ms
|
||||
delete m_reset_out;
|
||||
new DigitalInput(27); // Set SPI CS2 (IMU RST) high
|
||||
Wait(500_ms); // Wait 500ms for reset to complete
|
||||
|
||||
// Configure standard SPI
|
||||
if (!SwitchToStandardSPI()) {
|
||||
return;
|
||||
m_calibration_time(static_cast<uint16_t>(cal_time)),
|
||||
m_simDevice("Gyro:ADIS16470", port) {
|
||||
if (m_simDevice) {
|
||||
m_simGyroAngleX =
|
||||
m_simDevice.CreateDouble("gyro_angle_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroAngleY =
|
||||
m_simDevice.CreateDouble("gyro_angle_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroAngleZ =
|
||||
m_simDevice.CreateDouble("gyro_angle_z", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateX =
|
||||
m_simDevice.CreateDouble("gyro_rate_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateY =
|
||||
m_simDevice.CreateDouble("gyro_rate_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simGyroRateZ =
|
||||
m_simDevice.CreateDouble("gyro_rate_z", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelX =
|
||||
m_simDevice.CreateDouble("accel_x", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelY =
|
||||
m_simDevice.CreateDouble("accel_y", hal::SimDevice::kInput, 0.0);
|
||||
m_simAccelZ =
|
||||
m_simDevice.CreateDouble("accel_z", hal::SimDevice::kInput, 0.0);
|
||||
}
|
||||
|
||||
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1) =
|
||||
// 400Hz)
|
||||
WriteRegister(DEC_RATE, 0x0004);
|
||||
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and
|
||||
// PoP
|
||||
WriteRegister(MSC_CTRL, 0x0001);
|
||||
// Configure IMU internal Bartlett filter
|
||||
WriteRegister(FILT_CTRL, 0x0000);
|
||||
// Configure continuous bias calibration time based on user setting
|
||||
WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
|
||||
if (!m_simDevice) {
|
||||
// Force the IMU reset pin to toggle on startup (doesn't require DS enable)
|
||||
// Relies on the RIO hardware by default configuring an output as low
|
||||
// and configuring an input as high Z. The 10k pull-up resistor internal to
|
||||
// the IMU then forces the reset line high for normal operation.
|
||||
DigitalOutput* m_reset_out =
|
||||
new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
|
||||
Wait(10_ms); // Wait 10ms
|
||||
delete m_reset_out;
|
||||
new DigitalInput(27); // Set SPI CS2 (IMU RST) high
|
||||
Wait(500_ms); // Wait 500ms for reset to complete
|
||||
|
||||
// Notify DS that IMU calibration delay is active
|
||||
REPORT_WARNING("ADIS16470 IMU Detected. Starting initial calibration delay.");
|
||||
// Configure standard SPI
|
||||
if (!SwitchToStandardSPI()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Wait for samples to accumulate internal to the IMU (110% of user-defined
|
||||
// time)
|
||||
Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
|
||||
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
|
||||
// = 400Hz)
|
||||
WriteRegister(DEC_RATE, 0x0004);
|
||||
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
|
||||
// and PoP
|
||||
WriteRegister(MSC_CTRL, 0x0001);
|
||||
// Configure IMU internal Bartlett filter
|
||||
WriteRegister(FILT_CTRL, 0x0000);
|
||||
// Configure continuous bias calibration time based on user setting
|
||||
WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
|
||||
|
||||
// Write offset calibration command to IMU
|
||||
WriteRegister(GLOB_CMD, 0x0001);
|
||||
// Notify DS that IMU calibration delay is active
|
||||
REPORT_WARNING(
|
||||
"ADIS16470 IMU Detected. Starting initial calibration delay.");
|
||||
|
||||
// Configure and enable auto SPI
|
||||
if (!SwitchToAutoSPI()) {
|
||||
return;
|
||||
// Wait for samples to accumulate internal to the IMU (110% of user-defined
|
||||
// time)
|
||||
Wait(units::second_t{pow(2, m_calibration_time) / 2000 * 64 * 1.1});
|
||||
|
||||
// Write offset calibration command to IMU
|
||||
WriteRegister(GLOB_CMD, 0x0001);
|
||||
|
||||
// Configure and enable auto SPI
|
||||
if (!SwitchToAutoSPI()) {
|
||||
return;
|
||||
}
|
||||
|
||||
// Let the user know the IMU was initiallized successfully
|
||||
REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
|
||||
|
||||
// Drive SPI CS3 (IMU ready LED) low (active low)
|
||||
new DigitalOutput(28);
|
||||
}
|
||||
|
||||
// Let the user know the IMU was initiallized successfully
|
||||
REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
|
||||
|
||||
// Drive SPI CS3 (IMU ready LED) low (active low)
|
||||
new DigitalOutput(28);
|
||||
|
||||
// Report usage and post data to DS
|
||||
HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
|
||||
|
||||
@@ -708,6 +733,23 @@ double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
|
||||
*not coincide with the IMU Z axis.
|
||||
**/
|
||||
double ADIS16470_IMU::GetAngle() const {
|
||||
switch (m_yaw_axis) {
|
||||
case kX:
|
||||
if (m_simGyroAngleX) {
|
||||
return m_simGyroAngleX.Get();
|
||||
}
|
||||
break;
|
||||
case kY:
|
||||
if (m_simGyroAngleY) {
|
||||
return m_simGyroAngleY.Get();
|
||||
}
|
||||
break;
|
||||
case kZ:
|
||||
if (m_simGyroAngleZ) {
|
||||
return m_simGyroAngleZ.Get();
|
||||
}
|
||||
break;
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_integ_angle;
|
||||
}
|
||||
@@ -716,11 +758,11 @@ double ADIS16470_IMU::GetRate() const {
|
||||
std::scoped_lock sync(m_mutex);
|
||||
switch (m_yaw_axis) {
|
||||
case kX:
|
||||
return m_gyro_x;
|
||||
return GetGyroRateX();
|
||||
case kY:
|
||||
return m_gyro_y;
|
||||
return GetGyroRateY();
|
||||
case kZ:
|
||||
return m_gyro_z;
|
||||
return GetGyroRateZ();
|
||||
default:
|
||||
return 0.0;
|
||||
}
|
||||
@@ -730,32 +772,50 @@ ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
|
||||
return m_yaw_axis;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetGyroInstantX() const {
|
||||
double ADIS16470_IMU::GetGyroRateX() const {
|
||||
if (m_simGyroRateX) {
|
||||
return m_simGyroRateX.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_x;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetGyroInstantY() const {
|
||||
double ADIS16470_IMU::GetGyroRateY() const {
|
||||
if (m_simGyroRateY) {
|
||||
return m_simGyroRateY.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_y;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetGyroInstantZ() const {
|
||||
double ADIS16470_IMU::GetGyroRateZ() const {
|
||||
if (m_simGyroRateZ) {
|
||||
return m_simGyroRateZ.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_gyro_z;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetAccelInstantX() const {
|
||||
double ADIS16470_IMU::GetAccelX() const {
|
||||
if (m_simAccelX) {
|
||||
return m_simAccelX.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_x;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetAccelInstantY() const {
|
||||
double ADIS16470_IMU::GetAccelY() const {
|
||||
if (m_simAccelY) {
|
||||
return m_simAccelY.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_y;
|
||||
}
|
||||
|
||||
double ADIS16470_IMU::GetAccelInstantZ() const {
|
||||
double ADIS16470_IMU::GetAccelZ() const {
|
||||
if (m_simAccelZ) {
|
||||
return m_simAccelZ.Get();
|
||||
}
|
||||
std::scoped_lock sync(m_mutex);
|
||||
return m_accel_z;
|
||||
}
|
||||
@@ -780,6 +840,10 @@ double ADIS16470_IMU::GetYFilteredAccelAngle() const {
|
||||
return m_accelAngleY;
|
||||
}
|
||||
|
||||
int ADIS16470_IMU::GetPort() const {
|
||||
return m_spi_port;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Builds a Sendable object to push IMU data to the driver station.
|
||||
*
|
||||
|
||||
59
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
Normal file
59
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
Normal file
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/simulation/ADIS16448_IMUSim.h"
|
||||
|
||||
#include <frc/ADIS16448_IMU.h>
|
||||
#include <frc/simulation/SimDeviceSim.h>
|
||||
|
||||
using namespace frc::sim;
|
||||
|
||||
ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) {
|
||||
frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()};
|
||||
m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
|
||||
m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
|
||||
m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
|
||||
m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
|
||||
m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
|
||||
m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
|
||||
m_simAccelX = deviceSim.GetDouble("accel_x");
|
||||
m_simAccelY = deviceSim.GetDouble("accel_y");
|
||||
m_simAccelZ = deviceSim.GetDouble("accel_z");
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) {
|
||||
m_simGyroAngleX.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) {
|
||||
m_simGyroAngleY.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) {
|
||||
m_simGyroAngleZ.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateX.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateY.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateZ.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelX.Set(accel.value() / 9.81);
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelY.Set(accel.value() / 9.81);
|
||||
}
|
||||
|
||||
void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelZ.Set(accel.value() / 9.81);
|
||||
}
|
||||
59
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
Normal file
59
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
Normal file
@@ -0,0 +1,59 @@
|
||||
// Copyright (c) FIRST and other WPILib contributors.
|
||||
// Open Source Software; you can modify and/or share it under the terms of
|
||||
// the WPILib BSD license file in the root directory of this project.
|
||||
|
||||
#include "frc/simulation/ADIS16470_IMUSim.h"
|
||||
|
||||
#include <frc/ADIS16470_IMU.h>
|
||||
#include <frc/simulation/SimDeviceSim.h>
|
||||
|
||||
using namespace frc::sim;
|
||||
|
||||
ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) {
|
||||
frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()};
|
||||
m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x");
|
||||
m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y");
|
||||
m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z");
|
||||
m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x");
|
||||
m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y");
|
||||
m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z");
|
||||
m_simAccelX = deviceSim.GetDouble("accel_x");
|
||||
m_simAccelY = deviceSim.GetDouble("accel_y");
|
||||
m_simAccelZ = deviceSim.GetDouble("accel_z");
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) {
|
||||
m_simGyroAngleX.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) {
|
||||
m_simGyroAngleY.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) {
|
||||
m_simGyroAngleZ.Set(angle.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateX.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateY.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t rate) {
|
||||
m_simGyroRateZ.Set(rate.value());
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelX.Set(accel.value() / 9.81);
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelY.Set(accel.value() / 9.81);
|
||||
}
|
||||
|
||||
void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
|
||||
m_simAccelZ.Set(accel.value() / 9.81);
|
||||
}
|
||||
Reference in New Issue
Block a user