Squashed 'third_party/allwpilib/' changes from e473a00f97..e4b91005cf
e4b91005cf [examples] Update SwerveModule constructor doc (NFC) (#4042)
a260bfd83b [examples] Remove "this" keyword from SwerveModule (#4043)
18e262a100 [examples] Fix multiple doc typos in SwerveControllerCommand example (NFC) (#4044)
4bd1f526ab [wpilibc] Prevent StopMotor from terminating robot during MotorSafety check (#4038)
27847d7eb2 [sim] Expose GUI control functions via HAL_RegisterExtension (#4034)
b2a8d3f0f3 [wpilibc] Add mechanism to reset MotorSafety list (#4037)
49adac9564 [wpilib] Check for signedness in ArcadeDriveIK() (#4028)
a19d1133b1 [wpiutil] libuv: Fix sign compare warnings in gcc 11.2 (#4031)
dde91717e4 [build] cmake: Add ability to customize target warnings (#4032)
e9050afd67 [sim] Update sim match time to match real robot (#4024)
165d2837cf [wpilib] Preferences: Set Persistent in Init methods (#4025)
ac7549edca [glass] Fix snprintf truncation warning (#4029)
4d96bc72e0 [wpilibj] Fix typos in error messages for non-null assertions (#4014)
3411eee20f [hal] Replace hardcoded sim array sizes with constants (#4015)
74de97eeca [wpilibc] Add mechanism to reset various global structures (#4007)
4e3cc25012 [examples] Fix periodic function rate comment (NFC) (#4013)
90c1db393e [sim] Add exported functions to control the sim GUI (#3995)
2f43274aa4 [wpilibj] MechanismRoot2d: Add flush to setPosition (#4011)
aeca09db09 [glass] Support remapping of Enter key (#3994)
c107f22c67 [sim] Sim GUI: don't force-show Timing and Other Devices (#4001)
68fe51e8da [wpigui] Update PFD to latest, fix kdialog multiselect (#4005)
8d08d67cf1 [wpigui] PFD: Add console warning if file chooser unavailable (#4003)
4f1782f66e [wpilibc] Only call HAL_Report when initializing SmartDashboard (#4006)
3f77725cd3 Remove uses of iostream (#4004)
5635f33a32 [glass] Increase plot depth to 20K points (#3993)
bca4b7111b [glass] Fix PlotSeries::SetSource() (#3991)
6a6366b0d6 [commands] Add until() as alias for withInterrupt() (#3981)
16bf2c70c5 [wpilib] Fix joystick out of range error messages (#3988)
4b3edb742c [wpilib] Fix ADIS16448 IMU default constructor not working in Java (#3989)
fcf23fc9e9 [hal] Fix potential gamedata out of bounds read (#3983)
af5ef510c5 [wpilibc] Fix REV PH pulse duration units (#3982)
05401e2b81 [wpilib] Write REV PH firmware version to roboRIO to display on driver station (#3977)
9fde0110b6 Update to 2022 v4.0 image (#3944)
b03f8ddb2e [examples] fix incorrect variable in Arm Simulation Pref (#3980)
a26df2a022 [examples] Update ArmSimulation example to use Preferences (#3976)
d68d6674e8 [examples] Armbot: rename kCos to kG (#3975)
a8f0f6bb90 [wpilibj] Fix ADIS16448 getRate to return rate instead of angle (#3974)
dd9c92d5bf [build] Remove debug info from examples (#3971)
84df14dd70 [rtns] Fix icons (#3972)
560094ad92 [examples] Correct Mecanum example axes (#3955)
7ea1be9c01 [wpilibc] Fix typo in hardware version for REV PDH (#3969)
700f13bffd [wpilibj] Make methods public for Java REV PDH (#3970)
b6aa7c1aa9 [wpilibj] Make methods public for Java REVPH (#3968)
eb4d183e48 [wpimath] Fix clang-tidy bugprone-integer-division warning (#3966)
77e4e81e1e [wpilib] Add Field widget to BuiltInWidgets in shuffleboard (#3961)
88f5cb6eb0 [build] Publish PDBs with C++ tools (#3960)
efae552f3e [wpimath] Remove DifferentialDriveKinematics include from odometry (#3958)
46b277421a [glass] Update Speed Controller Type name for 2022 WPILib (#3952)
42908126b9 [wpilib] Add DCMotorSim (#3910)
a467392cbd [wpiutil] StackTrace: Add ability to override default implementation (#3951)
78d0bcf49d [templates] Add SimulationInit()/SimulationPeriodic() to robot templates (#3943)
02a0ced9b0 [wpilib] MecanumDrive: update docs for axis to match implementation (NFC) (#3942)
4ccfe1c9f2 [wpilib] Added docs clarification on units for drive class WheelSpeeds (NFC) (#3939)
830c0c5c2f [wpilib] MechanismLigament2d: Add getters for color and line weight (#3947)
5548a37465 [wpilib] PowerDistribution: Add module type getter (#3948)
2f9a600de2 [hal] Fix PCM one shot (#3949)
559db11a20 [myRobot] Skip deploying debug libraries for myRobot deploys (#3950)
76c78e295b [examples] Reorder SwerveModules in SwerveControllerCommand example odometry update (#3934)
debbd5ff4b [wpilib] Improve PowerDistribution docs (NFC) (#3925)
841174f302 [commands] Change command vendordep JSON version number to 1.0.0 (#3938)
8c55844f91 [wpilib] Remove comment about Mecanum right side inverted (NFC) (#3929)
0b990bf0f5 [hal] Fix PCM sticky faults clear function crashing (#3932)
104d7e2abc [hal] Don't throw exceptions in PCM JNI (#3933)
5ba69e1af1 [examples] Updated type in Java SwerveModule (#3928)
f3a0b5c7d7 [wpimath] Fix Java SimpleMotorFeedforward Docs (NFC) (#3926)
7f4265facc [wpimath] Add LinearFilter::FiniteDifference() (#3900)
63d1fb3bed [wpiutil] Modify fmt to not throw on write failure (#3919)
36af6d25a5 [wpimath] Fix input vector in pose estimator docs (NFC) (#3923)
8f387f7255 [wpilibj] Switch ControlWord mutex to actual reentrant mutex (#3922)
792e735e08 [wpimath] Move TrajectoryGenerator::SetErrorHandler definition to .cpp (#3920)
3b76de83eb [commands] Fix ProfiledPIDCommand use-after-free (#3904)
ad9f738cfa [fieldimages] Fix maven publishing (#3897)
49455199e5 [examples] Use left/rightGroup.Get() for simulator inputs to fix inversions (#3908)
64426502ea [wpimath] Fix arm -> flywheel typo (NFC) (#3911)
8cc112d196 [wpiutil] Fix wpi::array for move-only types (#3917)
e78cd49861 [build] Upgrade Java formatter plugins (#3894)
cfb4f756d6 [build] Upgrade to shadow 7.1.2 (#3893)
ba0908216c [wpimath] Fix crash in KF latency compensator (#3888)
a3a0334fad [build] cmake: Move fieldImages to WITH_GUI (#3885)
cf7460c3a8 [fieldImages] Add 2022 field (#3883)
db0fbb6448 [wpimath] Fix LQR matrix constructor overload for Q, R, and N (#3884)
8ac45f20bb [commands] Update Command documentation (NFC) (#3881)
b3707cca0b [wpiutil] Upgrade to fmt 8.1.1 (#3879)
a69ee3ece9 [wpimath] Const-qualify Twist2d scalar multiply (#3882)
750d9a30c9 [examples] Fix Eigen out of range error when running example (#3877)
41c5b2b5ac [rtns] Add cmake build (#3866)
6cf3f9b28e [build] Upgrade to Gradle 7.3.3 (#3878)
269cf03472 [examples] Add communication examples (e.g. arduino) (#2500)
5ccfc4adbd [oldcommands] Deprecate PIDWrappers, since they use deprecated interfaces (#3868)
b6f44f98be [hal] Add warning about onboard I2C (#3871)
0dca57e9ec [templates] romieducational: Invert drivetrain and disable motor safety (#3869)
22c4da152e [wpilib] Add GetRate() to ADIS classes (#3864)
05d66f862d [templates] Change the template ordering to put command based first (#3863)
b09f5b2cf2 [wpilibc] Add virtual dtor for LinearSystemSim (#3861)
a2510aaa0e [wpilib] Make ADIS IMU classes unit-safe (#3860)
947f589916 [wpilibc] Rename ADIS_16470_IMU.cpp to match class name (#3859)
bbd8980a20 [myRobot] Fix cameraserver library order (#3858)
831052f118 [wpilib] Add simulation support to ADIS classes (#3857)
c137569f91 [wpilib] Throw exception if the REV Pneumatic Hub firmware version is older than 22.0.0 (#3853)
dae61226fa Fix Maven Artifacts readme (#3856)
3ad4594a88 Update Maven artifacts readme for 2022 (#3855)
112acb9a62 [wpilibc] Move ADIS IMU constants to inside class (#3852)
ecee224e81 [wpilib] Allow SendableCameraWrappers to take arbitrary URLs (#3850)
a3645dea34 LICENSE: Bump year range to include 2022 (#3854)
7c09f44898 [wpilib] Use PSI for compressor config and sensor reading (#3847)
f401ea9aae [wpigui] Remove wpiutil linkage (#3851)
bf8517f1e6 [wpimath] TimeInterpolatableBufferTest: Fix lint warnings (#3849)
528087e308 [hal] Use enums with fixed underlying type in clang C (#3297)
1f59ff72f9 [wpilib] Add ADIS IMUs (#3777)
315be873c4 [wpimath] Add TimeInterpolatableBuffer (#2695)
b8d019cdb4 [wpilib] Rename NormalizeWheelSpeeds to DesaturateWheelSpeeds (#3791)
102f23bbdb [wpilibj] DriverStation: Set thread interrupted state (#3846)
b85c24a79c [wpilib] Add warning about onboard I2C (#3842)
eee29daaf9 [newCommands] Trigger: Allow override of debounce type (#3845)
aa9dfabde2 [wpimath] Move debouncer to filters (#3838)
5999a26fba [wpiutil] Add GetSystemTime() (#3840)
1e82595ffb [examples] Fix arcade inversions (#3841)
e373fa476b [wpiutil] Add disableMockTime to JNI (#3839)
dceb5364f4 [examples] Ensure right side motors are inverted (#3836)
baacbc8e24 [wpilib] Tachometer: Add function to return RPS (#3833)
84b15f0883 [templates] Add Java Romi Educational template (#3837)
c0da9d2d35 [examples] Invert Right Motor in Romi Java examples (#3828)
0fe0be2733 [build] Change project year to intellisense (#3835)
eafa947338 [wpimath] Make copies of trajectory constraint arguments (#3832)
9d13ae8d01 [wpilib] Add notes for Servo get that it only returns cmd (NFC) (#3820)
2a64e4bae5 [wpimath] Give drivetrain a more realistic width in TrajectoryJsonTest.java (#3822)
c3fd20db59 [wpilib] Fix trajectory sampling in DifferentialDriveSim test (#3821)
6f91f37cd0 [examples] Fix SwerveControllerCommand order of Module States (#3815)
5158730b81 [wpigui] Upgrade to imgui 1.86, GLFW 3.3.6 (#3817)
2ad2d2ca96 [wpiutil] MulticastServiceResolver: Fix C array returning functions (#3816)
b5fd29774f [wpilibj] Trigger: implement BooleanSupplier interface (#3811)
9f8f330e96 [wpilib] Fix Mecanum and SwerveControllerCommand when desired rotation passed (#3808)
1ad3b1b333 [hal] Don't copy byte to where null terminator goes (#3807)
dfc24425c3 [build] Fix gazebo gradle IDE warnings (#3806)
c02577bb51 [glass] Configure delay loading for windows camera server support (#3803)
c9e6a96a61 [wpilib] Document range of Servo angle (NFC) (#3796)
9778626f34 [wpilib, hal] Add support for getting faults and versions from power distribution (#3794)
34b2d0dae1 [wpilib, hal] High Level REV PH changes (#3792)
59a7528fd6 [cscore] Fix crash when usbcamera is deleted before message pump thread fully starts (#3804)
11d9859ef1 [build] Update plugins to remove log4j vulnerabilities (#3805)
e44ed752ad [glass] Fix CollapsingHeader in Encoder, PCM, and DeviceTree (#3797)
52b2dd5b89 [build] Bump native utils to remove log4j (#3802)
c46636f218 [wpilib] Improve new counter classes documentation (NFC) (#3801)
dc531462e1 [build] Update to gradle 7.3.2 (#3800)
92ba98621c [wpimath] Add helper variable templates for units type traits (#3790)
d41d051f1b [wpilibc] Fix Mecanum & Swerve ControllerCommand lambda capture (#3795)
c5ae0effac OtherVersions.md: Add one missing case of useLocal (#3788)
b3974c6ed3 [wpimath] Upgrade to Drake v0.37.0 (#3786)
589a00e379 [wpilibc] Start DriverStation thread from RobotBase (#3785)
8d9836ca02 [wpilib] Improve curvature drive documentation (NFC) (#3783)
8b5bf8632e [myRobot] Add wpimath and wpiutil JNI (#3784)
1846114491 [examples] Update references from characterization to SysId (NFC) (#3782)
2c461c794e [build] Update to gradle 7.3 (#3778)
109363daa4 [hal] Add remaining driver functions for REVPH (#3776)
41d26bee8d [hal] Refactor REV PDH (#3775)
7269a170fb Upgrade maven deps to latest versions and fix new linter errors (#3772)
441f2ed9b0 [build] actions: use fixed image versions instead latest (#3761)
15275433d4 [examples] Fix duplicate port allocations in C++ SwerveBot/SwerveDrivePoseEstimator/RomiReference (#3773)
1ac02d2f58 [examples] Fix drive Joystick axes in several examples (#3769)
8ee6257e92 [wpilib] DifferentialDrivetrainSim.KitbotMotor: Add NEO and Falcon 500 (#3762)
d81ef2bc5c [wpilib] Fix deadlocks in Mechanism2d et al. (#3770)
acb64dff97 [wpimath] Make RamseteController::Calculate() more concise (#3763)
3f6cf76a8c [hal] Refactor REV PH CAN frames (#3756)
3ef2dab465 [wpilib] DutyCycleEncoder: add setting of duty cycle range (#3759)
a5a56dd067 Readme: Add Visual Studio 2022 (#3760)
04957a6d30 [wpimath] Fix units of RamseteController's b and zeta (#3757)
5da54888f8 [glass] Upgrade imgui to 0.85, implot to HEAD, glfw to 3.3.5 (#3754)
6c93365b0f [wpiutil] MulticastService cleanup (#3750)
1c4a8bfb66 [cscore] Cleanup Windows USB camera impl (#3751)
d51a1d3b3d [rtns] Fix icon (#3749)
aced2e7da6 Add roboRIO Team Number Setter tool (#3744)
fa1ceca83a [wpilibj] Use DS cache for iterative robot control word cache (#3748)
0ea05d34e6 [build] Update to gradle 7.2 (#3746)
09db4f672b [build] Update to native utils 2022.6.1 (#3745)
4ba80a3a8c [wpigui] Don't recursively render frames in size callback (#3743)
ae208d2b17 [wpiutil] StringExtras: Add substr() (#3742)
6f51cb3b98 [wpiutil] MulticastResolver: make event manual reset, change to multiple read (#3736)
f6159ee1a2 [glass] Fix Drive widget handling of negative rotation (#3739)
7f401ae895 [build] Update NI libraries to 2022.2.3 (#3738)
0587b7043a [glass] Use JSON files for storage instead of imgui ini
0bbf51d566 [wpigui] Change maximized to bool
92c6eae6b0 [wpigui] PFD: Add explicit to constructors
141354cd79 [wpigui] Add hooks for custom load/save settings
f6e9fc7d71 [wpiutil] Handle multicast service collision on linux (#3734)
d8418be7d1 [glass, outlineviewer] Return 0 from WinMain (#3735)
82066946e5 [wpiutil] Add mDNS resolver and announcer (#3733)
4b1defc8d8 [wpilib] Remove automatic PD type from module type enum (#3732)
da90c1cd2c [wpilib] Add bang-bang controller (#3676)
3aa54fa027 [wpilib] Add new counter implementations (#2447)
b156db400d [hal, wpilib] Incorporate pneumatic control type into wpilibc/j (#3728)
9aba2b7583 [oldCommands] Add wrappers for WPILib objects to work with old PID Controller (#3710)
a9931223f0 [hal] Add REV PH faults (#3729)
aacf9442e4 [wpimath] Fix units typo in LinearSystemId source comment (#3730)
7db10ecf00 [wpilibc] Make SPI destructor virtual since SPI contains virtual functions (#3727)
a0a5b2aea5 [wpimath] Upgrade to EJML 0.41 (#3726)
eb835598a4 [hal] Add HAL functions for compressor config modes on REV PH (#3724)
f0ab6df5b6 [wpimath] Upgrade to Drake v0.36.0 (#3722)
075144faa3 [docs] Parse files without extensions with Doxygen (#3721)
32468a40cb [hal] Remove use of getDmaDescriptor from autospi (#3717)
38611e9dd7 [hal] Fix REVPH Analog Pressure channel selection (#3716)
4d78def31e [wpilib] Add DeadbandElimination forwarding to PWMMotorController (#3714)
3be0c1217a [wpilibcExamples] Make GearsBot use idiomatic C++ (#3711)
42d3a50aa2 [hal] Check error codes during serial port initialization (#3712)
52f1464029 Add project with field images and their json config files (#3668)
68ce62e2e9 [hal] Add autodetect for power modules (#3706)
3dd41c0d37 [wpilib] Don't print PD errors for LiveWindow reads (#3708)
7699a1f827 [hal] Fix sim not working with automatic PD type and default module (#3707)
Change-Id: I6b0b8fa8b2d2a24071240f624db9ec6d127f6648
git-subtree-dir: third_party/allwpilib
git-subtree-split: e4b91005cf69161f1cb3d934f6526232e6b9169e
Signed-off-by: Austin Schuh <austin.linux@gmail.com>
diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
new file mode 100644
index 0000000..25d674f
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
@@ -0,0 +1,874 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/* */
+/* Modified by Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16448_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalOutput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/SPI.h>
+#include <frc/Timer.h>
+
+#include <algorithm>
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline uint16_t BuffToUShort(const uint32_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint8_t BuffToUByte(const uint32_t* buf) {
+ return static_cast<uint8_t>(buf[0]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked<Args...>(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+ADIS16448_IMU::ADIS16448_IMU()
+ : ADIS16448_IMU(kZ, SPI::Port::kMXP, CalibrationTime::_512ms) {}
+
+ADIS16448_IMU::ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
+ CalibrationTime cal_time)
+ : 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);
+ }
+
+ 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);
+ }
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16448_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Dequeue 200 at a time, or the remainder of the buffer if less than
+ * 200 */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(200, data_count),
+ 0_s);
+ /* Update remaining buffer count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(1000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16448) {
+ REPORT_ERROR("Could not find ADIS16448!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+void ADIS16448_IMU::InitOffsetBuffer(int size) {
+ // avoid exceptions in the case of bad arguments
+ if (size < 1) {
+ size = 1;
+ }
+
+ // set average size to size (correct bad values)
+ m_avg_size = size;
+
+ // resize vector
+ if (m_offset_buffer != nullptr) {
+ delete[] m_offset_buffer;
+ }
+ m_offset_buffer = new OffsetData[size];
+
+ // set accumulate count to 0
+ m_accum_count = 0;
+}
+
+/**
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16448 IMU.
+ **/
+bool ADIS16448_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(10);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ m_auto_configured = true;
+ }
+ // Set auto SPI packet data and size
+ m_spi->SetAutoTransmitData({{GLOB_CMD}}, 27);
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kMXP, 100, 1000, 255);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ // Set up circular buffer
+ InitOffsetBuffer(m_avg_size);
+ // Kick off acquire thread
+ m_acquire_task = std::thread(&ADIS16448_IMU::Acquire, this);
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ *
+ **/
+int ADIS16448_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+ if (m_calibration_time == new_cal_time) {
+ return 1;
+ } else {
+ m_calibration_time = new_cal_time;
+ m_avg_size = static_cast<uint16_t>(m_calibration_time) * 819;
+ InitOffsetBuffer(m_avg_size);
+ return 0;
+ }
+}
+
+/**
+ *
+ **/
+void ADIS16448_IMU::Calibrate() {
+ std::scoped_lock sync(m_mutex);
+ // Calculate the running average
+ int gyroAverageSize = std::min(m_accum_count, m_avg_size);
+ double accum_gyro_rate_x = 0.0;
+ double accum_gyro_rate_y = 0.0;
+ double accum_gyro_rate_z = 0.0;
+ for (int i = 0; i < gyroAverageSize; i++) {
+ accum_gyro_rate_x += m_offset_buffer[i].gyro_rate_x;
+ accum_gyro_rate_y += m_offset_buffer[i].gyro_rate_y;
+ accum_gyro_rate_z += m_offset_buffer[i].gyro_rate_z;
+ }
+ m_gyro_rate_offset_x = accum_gyro_rate_x / gyroAverageSize;
+ m_gyro_rate_offset_y = accum_gyro_rate_y / gyroAverageSize;
+ m_gyro_rate_offset_z = accum_gyro_rate_z / gyroAverageSize;
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+}
+
+/**
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16448_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16448_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16448_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+}
+
+void ADIS16448_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+ delete[] m_offset_buffer;
+}
+
+ADIS16448_IMU::~ADIS16448_IMU() {
+ Close();
+}
+
+void ADIS16448_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 29; // 18 data points + timestamp
+
+ const int BUFFER_SIZE = 4000;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ int bufferAvgIndex = 0;
+ uint32_t previous_timestamp = 0;
+ double gyro_rate_x = 0.0;
+ double gyro_rate_y = 0.0;
+ double gyro_rate_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double mag_x = 0.0;
+ double mag_y = 0.0;
+ double mag_z = 0.0;
+ double baro = 0.0;
+ double temp = 0.0;
+ double gyro_rate_x_si = 0.0;
+ double gyro_rate_y_si = 0.0;
+ // double gyro_rate_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16448 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(buffer, data_to_read,
+ 0_s); // Read data from DMA buffer
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Calculate CRC-16 on each data packet
+ uint16_t calc_crc = 0xFFFF; // Starting word
+ uint8_t byte = 0;
+ uint16_t imu_crc = 0;
+ for (int k = 5; k < 27;
+ k += 2) // Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP
+ // (Ignore Status & CRC)
+ {
+ byte = BuffToUByte(&buffer[i + k + 1]); // Process LSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ byte = BuffToUByte(&buffer[i + k]); // Process MSB
+ calc_crc = (calc_crc >> 8) ^ adiscrc[(calc_crc & 0x00FF) ^ byte];
+ }
+ calc_crc = ~calc_crc; // Complement
+ calc_crc = static_cast<uint16_t>((calc_crc << 8) |
+ (calc_crc >> 8)); // Flip LSB & MSB
+ imu_crc =
+ BuffToUShort(&buffer[i + 27]); // Extract DUT CRC from data buffer
+
+ // Compare calculated vs read CRC. Don't update outputs or dt if CRC-16
+ // is bad
+ if (calc_crc == imu_crc) {
+ // Timestamp is at buffer[i]
+ m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+ // Split array and scale data
+ gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
+ gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
+ gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
+ accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
+ accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
+ accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
+ mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
+ mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
+ mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
+ baro = BuffToShort(&buffer[i + 23]) * 0.02;
+ temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
+
+ // Convert scaled sensor data to SI units
+ gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+ gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+ // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+ // Calculate alpha for use with the complementary filter
+ m_alpha = m_tau / (m_tau + m_dt);
+ // Calculate complementary filter
+ if (m_first_run) {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ accelAngleX = atan2f(
+ -accel_x_si,
+ sqrtf((accel_y_si * accel_y_si) + (-accel_z_si * -accel_z_si)));
+ accelAngleY =
+ atan2f(accel_y_si, sqrtf((-accel_x_si * -accel_x_si) +
+ (-accel_z_si * -accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, -accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, -accel_z_si);
+ compAngleX =
+ CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+ compAngleY =
+ CompFilterProcess(compAngleY, accelAngleY, -gyro_rate_x_si);
+ }
+
+ // Update global variables and state
+ {
+ std::scoped_lock sync(m_mutex);
+ // Ignore first, integrated sample
+ if (m_first_run) {
+ m_integ_gyro_angle_x = 0.0;
+ m_integ_gyro_angle_y = 0.0;
+ m_integ_gyro_angle_z = 0.0;
+ } else {
+ // Accumulate gyro for offset calibration
+ // Add most recent sample data to buffer
+ bufferAvgIndex = m_accum_count % m_avg_size;
+ m_offset_buffer[bufferAvgIndex] =
+ OffsetData{gyro_rate_x, gyro_rate_y, gyro_rate_z};
+ // Increment counter
+ m_accum_count++;
+ }
+ // Don't post accumulated data to the global variables until an
+ // initial gyro offset has been calculated
+ if (!m_start_up_mode) {
+ m_gyro_rate_x = gyro_rate_x;
+ m_gyro_rate_y = gyro_rate_y;
+ m_gyro_rate_z = gyro_rate_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_mag_x = mag_x;
+ m_mag_y = mag_y;
+ m_mag_z = mag_z;
+ m_baro = baro;
+ m_temp = temp;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ // Accumulate gyro for angle integration and publish to global
+ // variables
+ m_integ_gyro_angle_x +=
+ (gyro_rate_x - m_gyro_rate_offset_x) * m_dt;
+ m_integ_gyro_angle_y +=
+ (gyro_rate_y - m_gyro_rate_offset_y) * m_dt;
+ m_integ_gyro_angle_z +=
+ (gyro_rate_z - m_gyro_rate_offset_z) * m_dt;
+ }
+ }
+ m_first_run = false;
+ }
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ gyro_rate_x = 0.0;
+ gyro_rate_y = 0.0;
+ gyro_rate_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ mag_x = 0.0;
+ mag_y = 0.0;
+ mag_z = 0.0;
+ baro = 0.0;
+ temp = 0.0;
+ gyro_rate_x_si = 0.0;
+ gyro_rate_y_si = 0.0;
+ // gyro_rate_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16448_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+int ADIS16448_IMU::ConfigDecRate(uint16_t DecimationSetting) {
+ uint16_t writeValue = DecimationSetting;
+ uint16_t readbackValue;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+
+ /* Check max */
+ if (DecimationSetting > 9) {
+ REPORT_ERROR("Attemted to write an invalid decimation value. Capping at 9");
+ DecimationSetting = 9;
+ }
+
+ /* Shift decimation setting to correct position and select internal sync */
+ writeValue = (DecimationSetting << 8) | 0x1;
+
+ /* Apply to IMU */
+ WriteRegister(SMPL_PRD, writeValue);
+
+ /* Perform read back to verify write */
+ readbackValue = ReadRegister(SMPL_PRD);
+
+ /* Throw error for invalid write */
+ if (readbackValue != writeValue) {
+ REPORT_ERROR("ADIS16448 SMPL_PRD write failed.");
+ }
+
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+units::degree_t ADIS16448_IMU::GetAngle() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroAngleX();
+ case kY:
+ return GetGyroAngleY();
+ case kZ:
+ return GetGyroAngleZ();
+ default:
+ return 0_deg;
+ }
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetRate() const {
+ switch (m_yaw_axis) {
+ case kX:
+ return GetGyroRateX();
+ case kY:
+ return GetGyroRateY();
+ case kZ:
+ return GetGyroRateZ();
+ default:
+ return 0_deg_per_s;
+ }
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleX() const {
+ if (m_simGyroAngleX) {
+ return units::degree_t{m_simGyroAngleX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_x};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleY() const {
+ if (m_simGyroAngleY) {
+ return units::degree_t{m_simGyroAngleY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_y};
+}
+
+units::degree_t ADIS16448_IMU::GetGyroAngleZ() const {
+ if (m_simGyroAngleZ) {
+ return units::degree_t{m_simGyroAngleZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_gyro_angle_z};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateX() const {
+ if (m_simGyroRateX) {
+ return units::degrees_per_second_t{m_simGyroRateX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_x};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateY() const {
+ if (m_simGyroRateY) {
+ return units::degrees_per_second_t{m_simGyroRateY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_y};
+}
+
+units::degrees_per_second_t ADIS16448_IMU::GetGyroRateZ() const {
+ if (m_simGyroRateZ) {
+ return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_z};
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelX() const {
+ if (m_simAccelX) {
+ return units::meters_per_second_squared_t{m_simAccelX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_x * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelY() const {
+ if (m_simAccelY) {
+ return units::meters_per_second_squared_t{m_simAccelY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_y * 9.81_mps_sq;
+}
+
+units::meters_per_second_squared_t ADIS16448_IMU::GetAccelZ() const {
+ if (m_simAccelZ) {
+ return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return m_accel_z * 9.81_mps_sq;
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldX() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_x * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldY() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_y * 1e-3};
+}
+
+units::tesla_t ADIS16448_IMU::GetMagneticFieldZ() const {
+ std::scoped_lock sync(m_mutex);
+ return units::gauss_t{m_mag_z * 1e-3};
+}
+
+units::degree_t ADIS16448_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16448_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16448_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleY};
+}
+
+units::pounds_per_square_inch_t ADIS16448_IMU::GetBarometricPressure() const {
+ std::scoped_lock sync(m_mutex);
+ return units::mbar_t{m_baro};
+}
+
+units::celsius_t ADIS16448_IMU::GetTemperature() const {
+ std::scoped_lock sync(m_mutex);
+ return units::celsius_t{m_temp};
+}
+
+ADIS16448_IMU::IMUAxis ADIS16448_IMU::GetYawAxis() const {
+ return m_yaw_axis;
+}
+
+int ADIS16448_IMU::SetYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ } else {
+ m_yaw_axis = yaw_axis;
+ Reset();
+ return 0;
+ }
+}
+
+int ADIS16448_IMU::GetPort() const {
+ return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16448_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16448 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable([=]() {
+ nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
new file mode 100644
index 0000000..f48d827
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
@@ -0,0 +1,809 @@
+// 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.
+
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2016-2020 Analog Devices Inc. All Rights Reserved. */
+/* Open Source Software - may be modified and shared by FRC teams. The code */
+/* must be accompanied by the FIRST BSD license file in the root directory of */
+/* the project. */
+/* */
+/* Juan Chong - frcsupport@analog.com */
+/*----------------------------------------------------------------------------*/
+
+#include "frc/ADIS16470_IMU.h"
+
+#include <frc/DigitalInput.h>
+#include <frc/DigitalSource.h>
+#include <frc/DriverStation.h>
+#include <frc/Timer.h>
+
+#include <cmath>
+#include <string>
+
+#include <hal/HAL.h>
+#include <networktables/NTSendableBuilder.h>
+#include <wpi/numbers>
+#include <wpi/sendable/SendableRegistry.h>
+
+#include "frc/Errors.h"
+
+/* Helpful conversion functions */
+static inline int32_t ToInt(const uint32_t* buf) {
+ return static_cast<int32_t>((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) |
+ buf[3]);
+}
+
+static inline int16_t BuffToShort(const uint32_t* buf) {
+ return (static_cast<int16_t>(buf[0]) << 8) | buf[1];
+}
+
+static inline uint16_t ToUShort(const uint8_t* buf) {
+ return (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
+}
+
+using namespace frc;
+
+namespace {
+template <typename S, typename... Args>
+inline void ADISReportError(int32_t status, const char* file, int line,
+ const char* function, const S& format,
+ Args&&... args) {
+ frc::ReportErrorV(status, file, line, function, format,
+ fmt::make_args_checked<Args...>(format, args...));
+}
+} // namespace
+
+#define REPORT_WARNING(msg) \
+ ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, "{}", msg);
+#define REPORT_ERROR(msg) \
+ ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, "{}", msg)
+
+/**
+ * Constructor.
+ */
+ADIS16470_IMU::ADIS16470_IMU()
+ : ADIS16470_IMU(kZ, SPI::Port::kOnboardCS0, CalibrationTime::_4s) {}
+
+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)),
+ 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);
+ }
+
+ 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
+
+ // Configure standard SPI
+ if (!SwitchToStandardSPI()) {
+ return;
+ }
+
+ // 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);
+
+ // Notify DS that IMU calibration delay is active
+ REPORT_WARNING(
+ "ADIS16470 IMU Detected. Starting initial calibration delay.");
+
+ // 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);
+ }
+
+ // Report usage and post data to DS
+ HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
+
+ wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
+}
+
+/**
+ * @brief Switches to standard SPI operation. Primarily used when exiting auto
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in standard SPI mode.
+ *
+ * This function switches the active SPI port to standard SPI and is used
+ *primarily when exiting auto SPI. Exiting auto SPI is required to read or write
+ *using SPI since the auto SPI configuration, once active, locks the SPI message
+ *being transacted. This function also verifies that the SPI port is operating
+ *in standard SPI mode by reading back the IMU product ID.
+ **/
+bool ADIS16470_IMU::SwitchToStandardSPI() {
+ // Check to see whether the acquire thread is active. If so, wait for it to
+ // stop producing data.
+ if (m_thread_active) {
+ m_thread_active = false;
+ while (!m_thread_idle) {
+ Wait(10_ms);
+ }
+ // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI.
+ if (m_spi != nullptr && m_auto_configured) {
+ m_spi->StopAuto();
+ // We need to get rid of all the garbage left in the auto SPI buffer after
+ // stopping it. Sometimes data magically reappears, so we have to check
+ // the buffer size a couple of times
+ // to be sure we got it all. Yuck.
+ uint32_t trashBuffer[200];
+ Wait(100_ms);
+ int data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ while (data_count > 0) {
+ /* Receive data, max of 200 words at a time (prevent potential segfault)
+ */
+ m_spi->ReadAutoReceivedData(trashBuffer, std::min(data_count, 200),
+ 0_s);
+ /*Get the reamining data count */
+ data_count = m_spi->ReadAutoReceivedData(trashBuffer, 0, 0_s);
+ }
+ }
+ }
+ // There doesn't seem to be a SPI port active. Let's try to set one up
+ if (m_spi == nullptr) {
+ m_spi = new SPI(m_spi_port);
+ m_spi->SetClockRate(2000000);
+ m_spi->SetMSBFirst();
+ m_spi->SetSampleDataOnTrailingEdge();
+ m_spi->SetClockActiveLow();
+ m_spi->SetChipSelectActiveLow();
+ ReadRegister(PROD_ID); // Dummy read
+
+ // Validate the product ID
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ }
+ return true;
+ } else {
+ // Maybe the SPI port is active, but not in auto SPI mode? Try to read the
+ // product ID.
+ ReadRegister(PROD_ID); // Dummy read
+ uint16_t prod_id = ReadRegister(PROD_ID);
+ if (prod_id != 16982 && prod_id != 16470) {
+ REPORT_ERROR("Could not find ADIS16470!");
+ Close();
+ return false;
+ } else {
+ return true;
+ }
+ }
+}
+
+/**
+ * @brief Switches to auto SPI operation. Primarily used when exiting standard
+ *SPI mode.
+ *
+ * @return A boolean indicating the success or failure of setting up the SPI
+ *peripheral in auto SPI mode.
+ *
+ * This function switches the active SPI port to auto SPI and is used primarily
+ *when exiting standard SPI. Auto SPI is required to asynchronously read data
+ *over SPI as it utilizes special FPGA hardware to react to an external data
+ *ready (GPIO) input. Data captured using auto SPI is buffered in the FPGA and
+ *can be read by the CPU asynchronously. Standard SPI transactions are
+ * impossible on the selected SPI port once auto SPI is enabled. The stall
+ *settings, GPIO interrupt pin, and data packet settings used in this function
+ *are hard-coded to work only with the ADIS16470 IMU.
+ **/
+bool ADIS16470_IMU::SwitchToAutoSPI() {
+ // No SPI port has been set up. Go set one up first.
+ if (m_spi == nullptr) {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to start/restart auto SPI");
+ return false;
+ }
+ }
+ // Only set up the interrupt if needed.
+ if (m_auto_interrupt == nullptr) {
+ m_auto_interrupt = new DigitalInput(26);
+ }
+ // The auto SPI controller gets angry if you try to set up two instances on
+ // one bus.
+ if (!m_auto_configured) {
+ m_spi->InitAuto(8200);
+ m_auto_configured = true;
+ }
+ // Do we need to change auto SPI settings?
+ switch (m_yaw_axis) {
+ case kX:
+ m_spi->SetAutoTransmitData(m_autospi_x_packet, 2);
+ break;
+ case kY:
+ m_spi->SetAutoTransmitData(m_autospi_y_packet, 2);
+ break;
+ default:
+ m_spi->SetAutoTransmitData(m_autospi_z_packet, 2);
+ break;
+ }
+ // Configure auto stall time
+ m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
+ // Kick off DMA SPI (Note: Device configration impossible after SPI DMA is
+ // activated) DR High = Data good (data capture should be triggered on the
+ // rising edge)
+ m_spi->StartAutoTrigger(*m_auto_interrupt, true, false);
+ // Check to see if the acquire thread is running. If not, kick one off.
+ if (!m_thread_idle) {
+ m_first_run = true;
+ m_thread_active = true;
+ m_acquire_task = std::thread(&ADIS16470_IMU::Acquire, this);
+ } else {
+ m_first_run = true;
+ m_thread_active = true;
+ }
+ // Looks like the thread didn't start for some reason. Abort.
+ /*
+ if(!m_thread_idle) {
+ REPORT_ERROR("Failed to start/restart the acquire() thread.");
+ Close();
+ return false;
+ }
+ */
+ return true;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the NULL_CNFG register in the IMU, and re-enables auto SPI.
+ *
+ * @param new_cal_time Calibration time to be set.
+ *
+ * @return An int indicating the success or failure of writing the new NULL_CNFG
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new NULL_CNFG setting to the
+ *IMU, and re-enters auto SPI mode. This function does not include a blocking
+ *sleep, so the user must keep track of the elapsed offset calibration time
+ * themselves. After waiting the configured calibration time, the Calibrate()
+ *function should be called to activate the new offset calibration.
+ **/
+int ADIS16470_IMU::ConfigCalTime(CalibrationTime new_cal_time) {
+ if (m_calibration_time == static_cast<uint16_t>(new_cal_time)) {
+ return 1;
+ }
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ m_calibration_time = static_cast<uint16_t>(new_cal_time);
+ WriteRegister(NULL_CNFG, m_calibration_time | 0x700);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes a new value
+ *to the DECIMATE register in the IMU, and re-enables auto SPI.
+ *
+ * @param reg Decimation value to be set.
+ *
+ * @return An int indicating the success or failure of writing the new DECIMATE
+ *setting and returning to auto SPI mode. 0 = Success, 1 = No Change, 2 =
+ *Failure
+ *
+ * This function enters standard SPI mode, writes a new DECIMATE setting to the
+ *IMU, adjusts the sample scale factor, and re-enters auto SPI mode.
+ **/
+int ADIS16470_IMU::ConfigDecRate(uint16_t reg) {
+ uint16_t m_reg = reg;
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ if (m_reg > 1999) {
+ REPORT_ERROR("Attempted to write an invalid decimation value.");
+ m_reg = 1999;
+ }
+ m_scaled_sample_rate = (((m_reg + 1.0) / 2000.0) * 1000000.0);
+ WriteRegister(DEC_RATE, m_reg);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+/**
+ * @brief Switches the active SPI port to standard SPI mode, writes the command
+ *to activate the new null configuration, and re-enables auto SPI.
+ *
+ * This function enters standard SPI mode, writes 0x0001 to the GLOB_CMD
+ *register (thus making the new offset active in the IMU), and re-enters auto
+ *SPI mode. This function does not include a blocking sleep, so the user must
+ *keep track of the elapsed offset calibration time themselves.
+ **/
+void ADIS16470_IMU::Calibrate() {
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ }
+ WriteRegister(GLOB_CMD, 0x0001);
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ }
+}
+
+/**
+ * @brief Reads the contents of a specified register location over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @return An unsigned, 16-bit number representing the contents of the requested
+ *register location.
+ *
+ * This function reads the contents of an 8-bit register location by
+ *transmitting the register location byte along with a null (0x00) byte using
+ *the standard WPILib API. The response (two bytes) is read back using the
+ *WPILib API and joined using a helper function. This function assumes the
+ *controller is set to standard SPI mode.
+ **/
+uint16_t ADIS16470_IMU::ReadRegister(uint8_t reg) {
+ uint8_t buf[2];
+ buf[0] = reg & 0x7f;
+ buf[1] = 0;
+
+ m_spi->Write(buf, 2);
+ m_spi->Read(false, buf, 2);
+
+ return ToUShort(buf);
+}
+
+/**
+ * @brief Writes an unsigned, 16-bit value to two adjacent, 8-bit register
+ *locations over SPI.
+ *
+ * @param reg An unsigned, 8-bit register location.
+ *
+ * @param val An unsigned, 16-bit value to be written to the specified register
+ *location.
+ *
+ * This function writes an unsigned, 16-bit value into adjacent 8-bit addresses
+ *via SPI. The upper and lower bytes that make up the 16-bit value are split
+ *into two unsined, 8-bit values and written to the upper and lower addresses of
+ *the specified register value. Only the lower (base) address must be specified.
+ *This function assumes the controller is set to standard SPI mode.
+ **/
+void ADIS16470_IMU::WriteRegister(uint8_t reg, uint16_t val) {
+ uint8_t buf[2];
+ buf[0] = 0x80 | reg;
+ buf[1] = val & 0xff;
+ m_spi->Write(buf, 2);
+ buf[0] = 0x81 | reg;
+ buf[1] = val >> 8;
+ m_spi->Write(buf, 2);
+}
+
+/**
+ * @brief Resets (zeros) the xgyro, ygyro, and zgyro angle integrations.
+ *
+ * This function resets (zeros) the accumulated (integrated) angle estimates for
+ *the xgyro, ygyro, and zgyro outputs.
+ **/
+void ADIS16470_IMU::Reset() {
+ std::scoped_lock sync(m_mutex);
+ m_integ_angle = 0.0;
+}
+
+void ADIS16470_IMU::Close() {
+ if (m_thread_active) {
+ m_thread_active = false;
+ if (m_acquire_task.joinable()) {
+ m_acquire_task.join();
+ }
+ }
+ if (m_spi != nullptr) {
+ if (m_auto_configured) {
+ m_spi->StopAuto();
+ }
+ delete m_spi;
+ m_auto_configured = false;
+ if (m_auto_interrupt != nullptr) {
+ delete m_auto_interrupt;
+ m_auto_interrupt = nullptr;
+ }
+ m_spi = nullptr;
+ }
+}
+
+ADIS16470_IMU::~ADIS16470_IMU() {
+ Close();
+}
+
+/**
+ * @brief Main acquisition loop. Typically called asynchronously and free-wheels
+ *while the robot code is active.
+ *
+ * This is the main acquisiton loop for the IMU. During each iteration, data
+ *read using auto SPI is extracted from the FPGA FIFO, split, scaled, and
+ *integrated. Each X, Y, and Z value is 32-bits split across 4 indices (bytes)
+ *in the buffer. Auto SPI puts one byte in each index location. Each index is
+ *32-bits wide because the timestamp is an unsigned 32-bit int. The timestamp is
+ *always located at the beginning of the frame. Two indices (request_1 and
+ *request_2 below) are always invalid (garbage) and can be disregarded.
+ *
+ * Data order: [timestamp, request_1, request_2, x_1, x_2, x_3, x_4, y_1, y_2,
+ *y_3, y_4, z_1, z_2, z_3, z_4] x = delta x (32-bit x_1 = highest bit) y = delta
+ *y (32-bit y_1 = highest bit) z = delta z (32-bit z_1 = highest bit)
+ *
+ * Complementary filter code was borrowed from
+ *https://github.com/tcleg/Six_Axis_Complementary_Filter
+ **/
+void ADIS16470_IMU::Acquire() {
+ // Set data packet length
+ const int dataset_len = 19; // 18 data points + timestamp
+
+ /* Fixed buffer size */
+ const int BUFFER_SIZE = 4000;
+
+ // This buffer can contain many datasets
+ uint32_t buffer[BUFFER_SIZE];
+ int data_count = 0;
+ int data_remainder = 0;
+ int data_to_read = 0;
+ uint32_t previous_timestamp = 0;
+ double delta_angle = 0.0;
+ double gyro_rate_x = 0.0;
+ double gyro_rate_y = 0.0;
+ double gyro_rate_z = 0.0;
+ double accel_x = 0.0;
+ double accel_y = 0.0;
+ double accel_z = 0.0;
+ double gyro_rate_x_si = 0.0;
+ double gyro_rate_y_si = 0.0;
+ // double gyro_rate_z_si = 0.0;
+ double accel_x_si = 0.0;
+ double accel_y_si = 0.0;
+ double accel_z_si = 0.0;
+ double compAngleX = 0.0;
+ double compAngleY = 0.0;
+ double accelAngleX = 0.0;
+ double accelAngleY = 0.0;
+
+ while (true) {
+ // Sleep loop for 10ms (wait for data)
+ Wait(10_ms);
+
+ if (m_thread_active) {
+ m_thread_idle = false;
+
+ data_count = m_spi->ReadAutoReceivedData(
+ buffer, 0,
+ 0_s); // Read number of bytes currently stored in the buffer
+ data_remainder =
+ data_count % dataset_len; // Check if frame is incomplete. Add 1
+ // because of timestamp
+ data_to_read = data_count -
+ data_remainder; // Remove incomplete data from read count
+ /* Want to cap the data to read in a single read at the buffer size */
+ if (data_to_read > BUFFER_SIZE) {
+ REPORT_WARNING(
+ "ADIS16470 data processing thread overrun has occurred!");
+ data_to_read = BUFFER_SIZE - (BUFFER_SIZE % dataset_len);
+ }
+ m_spi->ReadAutoReceivedData(
+ buffer, data_to_read,
+ 0_s); // Read data from DMA buffer (only complete sets)
+
+ // Could be multiple data sets in the buffer. Handle each one.
+ for (int i = 0; i < data_to_read; i += dataset_len) {
+ // Timestamp is at buffer[i]
+ m_dt = (buffer[i] - previous_timestamp) / 1000000.0;
+ /* Get delta angle value for selected yaw axis and scale by the elapsed
+ * time (based on timestamp) */
+ delta_angle = (ToInt(&buffer[i + 3]) * delta_angle_sf) /
+ (m_scaled_sample_rate / (buffer[i] - previous_timestamp));
+ gyro_rate_x = (BuffToShort(&buffer[i + 7]) / 10.0);
+ gyro_rate_y = (BuffToShort(&buffer[i + 9]) / 10.0);
+ gyro_rate_z = (BuffToShort(&buffer[i + 11]) / 10.0);
+ accel_x = (BuffToShort(&buffer[i + 13]) / 800.0);
+ accel_y = (BuffToShort(&buffer[i + 15]) / 800.0);
+ accel_z = (BuffToShort(&buffer[i + 17]) / 800.0);
+
+ // Convert scaled sensor data to SI units
+ gyro_rate_x_si = gyro_rate_x * deg_to_rad;
+ gyro_rate_y_si = gyro_rate_y * deg_to_rad;
+ // gyro_rate_z_si = gyro_rate_z * deg_to_rad;
+ accel_x_si = accel_x * grav;
+ accel_y_si = accel_y * grav;
+ accel_z_si = accel_z * grav;
+
+ // Store timestamp for next iteration
+ previous_timestamp = buffer[i];
+
+ m_alpha = m_tau / (m_tau + m_dt);
+
+ if (m_first_run) {
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ compAngleX = accelAngleX;
+ compAngleY = accelAngleY;
+ } else {
+ // Process X angle
+ accelAngleX = atan2f(accel_x_si, sqrtf((accel_y_si * accel_y_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleY = atan2f(accel_y_si, sqrtf((accel_x_si * accel_x_si) +
+ (accel_z_si * accel_z_si)));
+ accelAngleX = FormatAccelRange(accelAngleX, accel_z_si);
+ accelAngleY = FormatAccelRange(accelAngleY, accel_z_si);
+ compAngleX =
+ CompFilterProcess(compAngleX, accelAngleX, -gyro_rate_y_si);
+ compAngleY =
+ CompFilterProcess(compAngleY, accelAngleY, gyro_rate_x_si);
+ }
+
+ {
+ std::scoped_lock sync(m_mutex);
+ /* Push data to global variables */
+ if (m_first_run) {
+ /* Don't accumulate first run. previous_timestamp will be "very" old
+ * and the integration will end up way off */
+ m_integ_angle = 0.0;
+ } else {
+ m_integ_angle += delta_angle;
+ }
+ m_gyro_rate_x = gyro_rate_x;
+ m_gyro_rate_y = gyro_rate_y;
+ m_gyro_rate_z = gyro_rate_z;
+ m_accel_x = accel_x;
+ m_accel_y = accel_y;
+ m_accel_z = accel_z;
+ m_compAngleX = compAngleX * rad_to_deg;
+ m_compAngleY = compAngleY * rad_to_deg;
+ m_accelAngleX = accelAngleX * rad_to_deg;
+ m_accelAngleY = accelAngleY * rad_to_deg;
+ }
+ m_first_run = false;
+ }
+ } else {
+ m_thread_idle = true;
+ data_count = 0;
+ data_remainder = 0;
+ data_to_read = 0;
+ previous_timestamp = 0.0;
+ delta_angle = 0.0;
+ gyro_rate_x = 0.0;
+ gyro_rate_y = 0.0;
+ gyro_rate_z = 0.0;
+ accel_x = 0.0;
+ accel_y = 0.0;
+ accel_z = 0.0;
+ gyro_rate_x_si = 0.0;
+ gyro_rate_y_si = 0.0;
+ // gyro_rate_z_si = 0.0;
+ accel_x_si = 0.0;
+ accel_y_si = 0.0;
+ accel_z_si = 0.0;
+ compAngleX = 0.0;
+ compAngleY = 0.0;
+ accelAngleX = 0.0;
+ accelAngleY = 0.0;
+ }
+ }
+}
+
+/* Complementary filter functions */
+double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
+ if (compAngle > accAngle + wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ } else if (accAngle > compAngle + wpi::numbers::pi) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatRange0to2PI(double compAngle) {
+ while (compAngle >= 2 * wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ while (compAngle < 0.0) {
+ compAngle = compAngle + 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
+ if (accelZ < 0.0) {
+ accelAngle = wpi::numbers::pi - accelAngle;
+ } else if (accelZ > 0.0 && accelAngle < 0.0) {
+ accelAngle = 2.0 * wpi::numbers::pi + accelAngle;
+ }
+ return accelAngle;
+}
+
+double ADIS16470_IMU::CompFilterProcess(double compAngle, double accelAngle,
+ double omega) {
+ compAngle = FormatFastConverge(compAngle, accelAngle);
+ compAngle =
+ m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle;
+ compAngle = FormatRange0to2PI(compAngle);
+ if (compAngle > wpi::numbers::pi) {
+ compAngle = compAngle - 2.0 * wpi::numbers::pi;
+ }
+ return compAngle;
+}
+
+units::degree_t ADIS16470_IMU::GetAngle() const {
+ switch (m_yaw_axis) {
+ case kX:
+ if (m_simGyroAngleX) {
+ return units::degree_t{m_simGyroAngleX.Get()};
+ }
+ break;
+ case kY:
+ if (m_simGyroAngleY) {
+ return units::degree_t{m_simGyroAngleY.Get()};
+ }
+ break;
+ case kZ:
+ if (m_simGyroAngleZ) {
+ return units::degree_t{m_simGyroAngleZ.Get()};
+ }
+ break;
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_integ_angle};
+}
+
+units::degrees_per_second_t ADIS16470_IMU::GetRate() const {
+ if (m_yaw_axis == kX) {
+ if (m_simGyroRateX) {
+ return units::degrees_per_second_t{m_simGyroRateX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_x};
+ } else if (m_yaw_axis == kY) {
+ if (m_simGyroRateY) {
+ return units::degrees_per_second_t{m_simGyroRateY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_y};
+ } else if (m_yaw_axis == kZ) {
+ if (m_simGyroRateZ) {
+ return units::degrees_per_second_t{m_simGyroRateZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::degrees_per_second_t{m_gyro_rate_z};
+ } else {
+ return 0_deg_per_s;
+ }
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelX() const {
+ if (m_simAccelX) {
+ return units::meters_per_second_squared_t{m_simAccelX.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_x};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelY() const {
+ if (m_simAccelY) {
+ return units::meters_per_second_squared_t{m_simAccelY.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_y};
+}
+
+units::meters_per_second_squared_t ADIS16470_IMU::GetAccelZ() const {
+ if (m_simAccelZ) {
+ return units::meters_per_second_squared_t{m_simAccelZ.Get()};
+ }
+ std::scoped_lock sync(m_mutex);
+ return units::meters_per_second_squared_t{m_accel_z};
+}
+
+units::degree_t ADIS16470_IMU::GetXComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYComplementaryAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_compAngleY};
+}
+
+units::degree_t ADIS16470_IMU::GetXFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleX};
+}
+
+units::degree_t ADIS16470_IMU::GetYFilteredAccelAngle() const {
+ std::scoped_lock sync(m_mutex);
+ return units::degree_t{m_accelAngleY};
+}
+
+ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetYawAxis() const {
+ return m_yaw_axis;
+}
+
+int ADIS16470_IMU::SetYawAxis(IMUAxis yaw_axis) {
+ if (m_yaw_axis == yaw_axis) {
+ return 1;
+ }
+ if (!SwitchToStandardSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
+ return 2;
+ }
+ m_yaw_axis = yaw_axis;
+ if (!SwitchToAutoSPI()) {
+ REPORT_ERROR("Failed to configure/reconfigure auto SPI.");
+ return 2;
+ }
+ return 0;
+}
+
+int ADIS16470_IMU::GetPort() const {
+ return m_spi_port;
+}
+
+/**
+ * @brief Builds a Sendable object to push IMU data to the driver station.
+ *
+ * This function pushes the most recent angle estimates for all axes to the
+ *driver station.
+ **/
+void ADIS16470_IMU::InitSendable(nt::NTSendableBuilder& builder) {
+ builder.SetSmartDashboardType("ADIS16470 IMU");
+ auto yaw_angle = builder.GetEntry("Yaw Angle").GetHandle();
+ builder.SetUpdateTable([=]() {
+ nt::NetworkTableEntry(yaw_angle).SetDouble(GetAngle().value());
+ });
+}
diff --git a/wpilibc/src/main/native/cpp/Compressor.cpp b/wpilibc/src/main/native/cpp/Compressor.cpp
index 0302acb..d994b14 100644
--- a/wpilibc/src/main/native/cpp/Compressor.cpp
+++ b/wpilibc/src/main/native/cpp/Compressor.cpp
@@ -19,7 +19,7 @@
throw FRC_MakeError(err::ResourceAlreadyAllocated, "{}", module);
}
- SetClosedLoopControl(true);
+ m_module->EnableCompressorDigital();
HAL_Report(HALUsageReporting::kResourceType_Compressor, module + 1);
wpi::SendableRegistry::AddLW(this, "Compressor", module);
@@ -33,11 +33,11 @@
}
void Compressor::Start() {
- SetClosedLoopControl(true);
+ EnableDigital();
}
void Compressor::Stop() {
- SetClosedLoopControl(false);
+ Disable();
}
bool Compressor::Enabled() const {
@@ -48,24 +48,43 @@
return m_module->GetPressureSwitch();
}
-double Compressor::GetCompressorCurrent() const {
+units::ampere_t Compressor::GetCurrent() const {
return m_module->GetCompressorCurrent();
}
-void Compressor::SetClosedLoopControl(bool on) {
- m_module->SetClosedLoopControl(on);
+units::volt_t Compressor::GetAnalogVoltage() const {
+ return m_module->GetAnalogVoltage(0);
}
-bool Compressor::GetClosedLoopControl() const {
- return m_module->GetClosedLoopControl();
+units::pounds_per_square_inch_t Compressor::GetPressure() const {
+ return m_module->GetPressure(0);
+}
+
+void Compressor::Disable() {
+ m_module->DisableCompressor();
+}
+
+void Compressor::EnableDigital() {
+ m_module->EnableCompressorDigital();
+}
+
+void Compressor::EnableAnalog(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ m_module->EnableCompressorAnalog(minPressure, maxPressure);
+}
+
+void Compressor::EnableHybrid(units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ m_module->EnableCompressorHybrid(minPressure, maxPressure);
+}
+
+CompressorConfigType Compressor::GetConfigType() const {
+ return m_module->GetCompressorConfigType();
}
void Compressor::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Compressor");
builder.AddBooleanProperty(
- "Closed Loop Control", [=]() { return GetClosedLoopControl(); },
- [=](bool value) { SetClosedLoopControl(value); });
- builder.AddBooleanProperty(
"Enabled", [=] { return Enabled(); }, nullptr);
builder.AddBooleanProperty(
"Pressure switch", [=]() { return GetPressureSwitchValue(); }, nullptr);
diff --git a/wpilibc/src/main/native/cpp/Debouncer.cpp b/wpilibc/src/main/native/cpp/Debouncer.cpp
deleted file mode 100644
index eb402cd..0000000
--- a/wpilibc/src/main/native/cpp/Debouncer.cpp
+++ /dev/null
@@ -1,37 +0,0 @@
-// 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/Debouncer.h"
-
-using namespace frc;
-
-Debouncer::Debouncer(units::second_t debounceTime, DebounceType type)
- : m_debounceTime(debounceTime), m_debounceType(type) {
- switch (type) {
- case DebounceType::kBoth: // fall-through
- case DebounceType::kRising:
- m_baseline = false;
- break;
- case DebounceType::kFalling:
- m_baseline = true;
- break;
- }
- m_timer.Start();
-}
-
-bool Debouncer::Calculate(bool input) {
- if (input == m_baseline) {
- m_timer.Reset();
- }
-
- if (m_timer.HasElapsed(m_debounceTime)) {
- if (m_debounceType == DebounceType::kBoth) {
- m_baseline = input;
- m_timer.Reset();
- }
- return input;
- } else {
- return m_baseline;
- }
-}
diff --git a/wpilibc/src/main/native/cpp/DigitalInput.cpp b/wpilibc/src/main/native/cpp/DigitalInput.cpp
index 7bce921..86f1c3a 100644
--- a/wpilibc/src/main/native/cpp/DigitalInput.cpp
+++ b/wpilibc/src/main/native/cpp/DigitalInput.cpp
@@ -4,7 +4,6 @@
#include "frc/DigitalInput.h"
-#include <iostream>
#include <limits>
#include <hal/DIO.h>
diff --git a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
index 8e994e9..452f5cd 100644
--- a/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
+++ b/wpilibc/src/main/native/cpp/DutyCycleEncoder.cpp
@@ -89,6 +89,14 @@
auto counter2 = m_counter->Get();
auto pos2 = m_dutyCycle->GetOutput();
if (counter == counter2 && pos == pos2) {
+ // map sensor range
+ if (pos < m_sensorMin) {
+ pos = m_sensorMin;
+ }
+ if (pos > m_sensorMax) {
+ pos = m_sensorMax;
+ }
+ pos = (pos - m_sensorMin) / (m_sensorMax - m_sensorMin);
units::turn_t turns{counter + pos - m_positionOffset};
m_lastPosition = turns;
return turns;
@@ -102,6 +110,11 @@
return m_lastPosition;
}
+void DutyCycleEncoder::SetDutyCycleRange(double min, double max) {
+ m_sensorMin = std::clamp(min, 0.0, 1.0);
+ m_sensorMax = std::clamp(max, 0.0, 1.0);
+}
+
void DutyCycleEncoder::SetDistancePerRotation(double distancePerRotation) {
m_distancePerRotation = distancePerRotation;
m_simDistancePerRotation.Set(distancePerRotation);
diff --git a/wpilibc/src/main/native/cpp/I2C.cpp b/wpilibc/src/main/native/cpp/I2C.cpp
index adf1c54..dcfb2b8 100644
--- a/wpilibc/src/main/native/cpp/I2C.cpp
+++ b/wpilibc/src/main/native/cpp/I2C.cpp
@@ -16,6 +16,14 @@
I2C::I2C(Port port, int deviceAddress)
: m_port(static_cast<HAL_I2CPort>(port)), m_deviceAddress(deviceAddress) {
int32_t status = 0;
+
+ if (port == I2C::Port::kOnboard) {
+ FRC_ReportError(warn::Warning, "{}",
+ "Onboard I2C port is subject to system lockups. See Known "
+ "Issues page for "
+ "details");
+ }
+
HAL_InitializeI2C(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", port);
diff --git a/wpilibc/src/main/native/cpp/MotorSafety.cpp b/wpilibc/src/main/native/cpp/MotorSafety.cpp
index 9cb28cd..388a887 100644
--- a/wpilibc/src/main/native/cpp/MotorSafety.cpp
+++ b/wpilibc/src/main/native/cpp/MotorSafety.cpp
@@ -17,6 +17,15 @@
static wpi::SmallPtrSet<MotorSafety*, 32> instanceList;
static wpi::mutex listMutex;
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetMotorSafety() {
+ std::scoped_lock lock(listMutex);
+ instanceList.clear();
+}
+} // namespace frc::impl
+#endif
+
MotorSafety::MotorSafety() {
std::scoped_lock lock(listMutex);
instanceList.insert(this);
@@ -89,7 +98,15 @@
if (stopTime < Timer::GetFPGATimestamp()) {
FRC_ReportError(err::Timeout, "{}... Output not updated often enough",
GetDescription());
- StopMotor();
+
+ try {
+ StopMotor();
+ } catch (frc::RuntimeError& e) {
+ e.Report();
+ } catch (std::exception& e) {
+ FRC_ReportError(err::Error, "{} StopMotor threw unexpected exception: {}",
+ GetDescription(), e.what());
+ }
}
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticHub.cpp b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
index 8217e86..0f32e1e 100644
--- a/wpilibc/src/main/native/cpp/PneumaticHub.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticHub.cpp
@@ -4,6 +4,7 @@
#include "frc/PneumaticHub.h"
+#include <fmt/format.h>
#include <hal/REVPH.h>
#include <wpi/NullDeleter.h>
#include <wpi/StackTrace.h>
@@ -11,11 +12,27 @@
#include "frc/Compressor.h"
#include "frc/DoubleSolenoid.h"
#include "frc/Errors.h"
+#include "frc/RobotBase.h"
#include "frc/SensorUtil.h"
#include "frc/Solenoid.h"
+#include "frc/fmt/Units.h"
using namespace frc;
+/** Converts volts to PSI per the REV Analog Pressure Sensor datasheet. */
+units::pounds_per_square_inch_t VoltsToPSI(units::volt_t sensorVoltage,
+ units::volt_t supplyVoltage) {
+ auto pressure = 250 * (sensorVoltage.value() / supplyVoltage.value()) - 25;
+ return units::pounds_per_square_inch_t{pressure};
+}
+
+/** Converts PSI to volts per the REV Analog Pressure Sensor datasheet. */
+units::volt_t PSIToVolts(units::pounds_per_square_inch_t pressure,
+ units::volt_t supplyVoltage) {
+ auto voltage = supplyVoltage.value() * (0.004 * pressure.value() + 0.1);
+ return units::volt_t{voltage};
+}
+
wpi::mutex PneumaticHub::m_handleLock;
std::unique_ptr<wpi::DenseMap<int, std::weak_ptr<PneumaticHub::DataStore>>>
PneumaticHub::m_handleMap = nullptr;
@@ -39,6 +56,38 @@
m_moduleObject = PneumaticHub{handle, module};
m_moduleObject.m_dataStore =
std::shared_ptr<DataStore>{this, wpi::NullDeleter<DataStore>()};
+
+ auto version = m_moduleObject.GetVersion();
+
+ if (version.FirmwareMajor > 0 && RobotBase::IsReal()) {
+ // Write PH firmware version to roboRIO
+ std::FILE* file = nullptr;
+ file = std::fopen(
+ fmt::format("/tmp/frc_versions/REV_PH_{:0>2}_WPILib_Version.ini",
+ module)
+ .c_str(),
+ "w");
+ if (file != nullptr) {
+ std::fputs("[Version]\n", file);
+ std::fputs(fmt::format("model=REV PH\n").c_str(), file);
+ std::fputs(fmt::format("deviceID={:x}\n", (0x9052600 | module)).c_str(),
+ file);
+ std::fputs(fmt::format("currentVersion={}.{}.{}", version.FirmwareMajor,
+ version.FirmwareMinor, version.FirmwareFix)
+ .c_str(),
+ file);
+ std::fclose(file);
+ }
+ }
+
+ // Check PH firmware version
+ if (version.FirmwareMajor > 0 && version.FirmwareMajor < 22) {
+ throw FRC_MakeError(
+ err::AssertionFailure,
+ "The Pneumatic Hub has firmware version {}.{}.{}, and must be "
+ "updated to version 2022.0.0 or later using the REV Hardware Client",
+ version.FirmwareMajor, version.FirmwareMinor, version.FirmwareFix);
+ }
}
~DataStore() noexcept { HAL_FreeREVPH(m_moduleObject.m_handle); }
@@ -52,6 +101,7 @@
bool m_compressorReserved{false};
wpi::mutex m_reservedLock;
PneumaticHub m_moduleObject{HAL_kInvalidHandle, 0};
+ std::array<units::millisecond_t, 16> m_oneShotDurMs{0_ms};
};
PneumaticHub::PneumaticHub()
@@ -76,47 +126,103 @@
bool PneumaticHub::GetCompressor() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressor(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-void PneumaticHub::SetClosedLoopControl(bool enabled) {
+void PneumaticHub::DisableCompressor() {
int32_t status = 0;
- HAL_SetREVPHClosedLoopControl(m_handle, enabled, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ HAL_SetREVPHClosedLoopControlDisabled(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
-bool PneumaticHub::GetClosedLoopControl() const {
+void PneumaticHub::EnableCompressorDigital() {
int32_t status = 0;
- auto result = HAL_GetREVPHClosedLoopControl(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ HAL_SetREVPHClosedLoopControlDigital(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ if (minPressure >= maxPressure) {
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ "maxPressure must be greater than minPresure");
+ }
+ if (minPressure < 0_psi || minPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "minPressure must be between 0 and 120 PSI, got {}",
+ minPressure);
+ }
+ if (maxPressure < 0_psi || maxPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "maxPressure must be between 0 and 120 PSI, got {}",
+ maxPressure);
+ }
+ int32_t status = 0;
+ units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+ units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+ HAL_SetREVPHClosedLoopControlAnalog(m_handle, minAnalogVoltage.value(),
+ maxAnalogVoltage.value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticHub::EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ if (minPressure >= maxPressure) {
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ "maxPressure must be greater than minPresure");
+ }
+ if (minPressure < 0_psi || minPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "minPressure must be between 0 and 120 PSI, got {}",
+ minPressure);
+ }
+ if (maxPressure < 0_psi || maxPressure > 120_psi) {
+ throw FRC_MakeError(err::ParameterOutOfRange,
+ "maxPressure must be between 0 and 120 PSI, got {}",
+ maxPressure);
+ }
+ int32_t status = 0;
+ units::volt_t minAnalogVoltage = PSIToVolts(minPressure, 5_V);
+ units::volt_t maxAnalogVoltage = PSIToVolts(maxPressure, 5_V);
+ HAL_SetREVPHClosedLoopControlHybrid(m_handle, minAnalogVoltage.value(),
+ maxAnalogVoltage.value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticHub::GetCompressorConfigType() const {
+ int32_t status = 0;
+ auto result = HAL_GetREVPHCompressorConfig(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return static_cast<CompressorConfigType>(result);
}
bool PneumaticHub::GetPressureSwitch() const {
int32_t status = 0;
auto result = HAL_GetREVPHPressureSwitch(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-double PneumaticHub::GetCompressorCurrent() const {
+units::ampere_t PneumaticHub::GetCompressorCurrent() const {
int32_t status = 0;
auto result = HAL_GetREVPHCompressorCurrent(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{result};
}
void PneumaticHub::SetSolenoids(int mask, int values) {
int32_t status = 0;
HAL_SetREVPHSolenoids(m_handle, mask, values, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
int PneumaticHub::GetSolenoids() const {
int32_t status = 0;
auto result = HAL_GetREVPHSolenoids(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
@@ -125,27 +231,26 @@
}
int PneumaticHub::GetSolenoidDisabledList() const {
- return 0;
- // TODO Fix me
- // int32_t status = 0;
- // auto result = HAL_GetREVPHSolenoidDisabledList(m_handle, &status);
- // FRC_CheckErrorStatus(status, "Module {}", m_module);
- // return result;
+ int32_t status = 0;
+ HAL_REVPHStickyFaults faults;
+ std::memset(&faults, 0, sizeof(faults));
+ HAL_GetREVPHStickyFaults(m_handle, &faults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ uint32_t intFaults = 0;
+ static_assert(sizeof(faults) == sizeof(intFaults));
+ std::memcpy(&intFaults, &faults, sizeof(faults));
+ return intFaults & 0xFFFF;
}
void PneumaticHub::FireOneShot(int index) {
- // TODO Fix me
- // int32_t status = 0;
- // HAL_FireREVPHOneShot(m_handle, index, &status);
- // FRC_CheckErrorStatus(status, "Module {}", m_module);
+ int32_t status = 0;
+ HAL_FireREVPHOneShot(m_handle, index,
+ m_dataStore->m_oneShotDurMs[index].value(), &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticHub::SetOneShotDuration(int index, units::second_t duration) {
- // TODO Fix me
- // int32_t status = 0;
- // units::millisecond_t millis = duration;
- // HAL_SetREVPHOneShotDuration(m_handle, index, millis.to<int32_t>(),
- // &status); FRC_CheckErrorStatus(status, "Module {}", m_module);
+ m_dataStore->m_oneShotDurMs[index] = duration;
}
bool PneumaticHub::CheckSolenoidChannel(int channel) const {
@@ -181,6 +286,98 @@
m_dataStore->m_compressorReserved = false;
}
+PneumaticHub::Version PneumaticHub::GetVersion() const {
+ int32_t status = 0;
+ HAL_REVPHVersion halVersions;
+ std::memset(&halVersions, 0, sizeof(halVersions));
+ HAL_GetREVPHVersion(m_handle, &halVersions, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::Version versions;
+ static_assert(sizeof(halVersions) == sizeof(versions));
+ static_assert(std::is_standard_layout_v<decltype(versions)>);
+ static_assert(std::is_trivial_v<decltype(versions)>);
+ std::memcpy(&versions, &halVersions, sizeof(versions));
+ return versions;
+}
+
+PneumaticHub::Faults PneumaticHub::GetFaults() const {
+ int32_t status = 0;
+ HAL_REVPHFaults halFaults;
+ std::memset(&halFaults, 0, sizeof(halFaults));
+ HAL_GetREVPHFaults(m_handle, &halFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::Faults faults;
+ static_assert(sizeof(halFaults) == sizeof(faults));
+ static_assert(std::is_standard_layout_v<decltype(faults)>);
+ static_assert(std::is_trivial_v<decltype(faults)>);
+ std::memcpy(&faults, &halFaults, sizeof(faults));
+ return faults;
+}
+
+PneumaticHub::StickyFaults PneumaticHub::GetStickyFaults() const {
+ int32_t status = 0;
+ HAL_REVPHStickyFaults halStickyFaults;
+ std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+ HAL_GetREVPHStickyFaults(m_handle, &halStickyFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PneumaticHub::StickyFaults stickyFaults;
+ static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+ static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+ static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+ std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+ return stickyFaults;
+}
+
+void PneumaticHub::ClearStickyFaults() {
+ int32_t status = 0;
+ HAL_ClearREVPHStickyFaults(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+units::volt_t PneumaticHub::GetInputVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::Get5VRegulatedVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::ampere_t PneumaticHub::GetSolenoidsTotalCurrent() const {
+ int32_t status = 0;
+ auto current = HAL_GetREVPHSolenoidCurrent(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{current};
+}
+
+units::volt_t PneumaticHub::GetSolenoidsVoltage() const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHSolenoidVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::volt_t PneumaticHub::GetAnalogVoltage(int channel) const {
+ int32_t status = 0;
+ auto voltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::volt_t{voltage};
+}
+
+units::pounds_per_square_inch_t PneumaticHub::GetPressure(int channel) const {
+ int32_t status = 0;
+ auto sensorVoltage = HAL_GetREVPHAnalogVoltage(m_handle, channel, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ auto supplyVoltage = HAL_GetREVPH5VVoltage(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return VoltsToPSI(units::volt_t{sensorVoltage}, units::volt_t{supplyVoltage});
+}
+
Solenoid PneumaticHub::MakeSolenoid(int channel) {
return Solenoid{m_module, PneumaticsModuleType::REVPH, channel};
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
index 9d96651..bd9e033 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsBase.cpp
@@ -4,6 +4,8 @@
#include "frc/PneumaticsBase.h"
+#include <hal/REVPH.h>
+
#include "frc/Errors.h"
#include "frc/PneumaticHub.h"
#include "frc/PneumaticsControlModule.h"
@@ -11,6 +13,19 @@
using namespace frc;
+static_assert(
+ static_cast<int>(CompressorConfigType::Disabled) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDisabled);
+static_assert(
+ static_cast<int>(CompressorConfigType::Digital) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kDigital);
+static_assert(
+ static_cast<int>(CompressorConfigType::Analog) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kAnalog);
+static_assert(
+ static_cast<int>(CompressorConfigType::Hybrid) ==
+ HAL_REVPHCompressorConfigType::HAL_REVPHCompressorConfigType_kHybrid);
+
std::shared_ptr<PneumaticsBase> PneumaticsBase::GetForType(
int module, PneumaticsModuleType moduleType) {
if (moduleType == PneumaticsModuleType::CTREPCM) {
@@ -18,7 +33,8 @@
} else if (moduleType == PneumaticsModuleType::REVPH) {
return PneumaticHub::GetForModule(module);
}
- throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ static_cast<int>(moduleType));
}
int PneumaticsBase::GetDefaultForType(PneumaticsModuleType moduleType) {
@@ -27,5 +43,6 @@
} else if (moduleType == PneumaticsModuleType::REVPH) {
return SensorUtil::GetDefaultREVPHModule();
}
- throw FRC_MakeError(err::InvalidParameter, "{}", moduleType);
+ throw FRC_MakeError(err::InvalidParameter, "{}",
+ static_cast<int>(moduleType));
}
diff --git a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
index 32de5ca..60cd84d 100644
--- a/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
+++ b/wpilibc/src/main/native/cpp/PneumaticsControlModule.cpp
@@ -80,105 +80,128 @@
bool PneumaticsControlModule::GetCompressor() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressor(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-void PneumaticsControlModule::SetClosedLoopControl(bool enabled) {
+void PneumaticsControlModule::DisableCompressor() {
int32_t status = 0;
- HAL_SetCTREPCMClosedLoopControl(m_handle, enabled, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ HAL_SetCTREPCMClosedLoopControl(m_handle, false, &status);
+ FRC_ReportError(status, "Module {}", m_module);
}
-bool PneumaticsControlModule::GetClosedLoopControl() const {
+void PneumaticsControlModule::EnableCompressorDigital() {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorAnalog(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+void PneumaticsControlModule::EnableCompressorHybrid(
+ units::pounds_per_square_inch_t minPressure,
+ units::pounds_per_square_inch_t maxPressure) {
+ int32_t status = 0;
+ HAL_SetCTREPCMClosedLoopControl(m_handle, true, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+}
+
+CompressorConfigType PneumaticsControlModule::GetCompressorConfigType() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMClosedLoopControl(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return result ? CompressorConfigType::Digital
+ : CompressorConfigType::Disabled;
}
bool PneumaticsControlModule::GetPressureSwitch() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMPressureSwitch(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
-double PneumaticsControlModule::GetCompressorCurrent() const {
+units::ampere_t PneumaticsControlModule::GetCompressorCurrent() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorCurrent(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
- return result;
+ FRC_ReportError(status, "Module {}", m_module);
+ return units::ampere_t{result};
}
bool PneumaticsControlModule::GetCompressorCurrentTooHighFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorCurrentTooHighFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorCurrentTooHighStickyFault() const {
int32_t status = 0;
auto result =
HAL_GetCTREPCMCompressorCurrentTooHighStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorShortedFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorShortedFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorShortedStickyFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorShortedStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorNotConnectedFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMCompressorNotConnectedFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetCompressorNotConnectedStickyFault() const {
int32_t status = 0;
auto result =
HAL_GetCTREPCMCompressorNotConnectedStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetSolenoidVoltageFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidVoltageFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
bool PneumaticsControlModule::GetSolenoidVoltageStickyFault() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidVoltageStickyFault(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
void PneumaticsControlModule::ClearAllStickyFaults() {
int32_t status = 0;
HAL_ClearAllCTREPCMStickyFaults(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticsControlModule::SetSolenoids(int mask, int values) {
int32_t status = 0;
HAL_SetCTREPCMSolenoids(m_handle, mask, values, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
int PneumaticsControlModule::GetSolenoids() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoids(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
@@ -189,14 +212,14 @@
int PneumaticsControlModule::GetSolenoidDisabledList() const {
int32_t status = 0;
auto result = HAL_GetCTREPCMSolenoidDisabledList(m_handle, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
return result;
}
void PneumaticsControlModule::FireOneShot(int index) {
int32_t status = 0;
HAL_FireCTREPCMOneShot(m_handle, index, &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
void PneumaticsControlModule::SetOneShotDuration(int index,
@@ -204,7 +227,7 @@
int32_t status = 0;
units::millisecond_t millis = duration;
HAL_SetCTREPCMOneShotDuration(m_handle, index, millis.to<int32_t>(), &status);
- FRC_CheckErrorStatus(status, "Module {}", m_module);
+ FRC_ReportError(status, "Module {}", m_module);
}
bool PneumaticsControlModule::CheckSolenoidChannel(int channel) const {
@@ -240,6 +263,15 @@
m_dataStore->m_compressorReserved = false;
}
+units::volt_t PneumaticsControlModule::GetAnalogVoltage(int channel) const {
+ return units::volt_t{0};
+}
+
+units::pounds_per_square_inch_t PneumaticsControlModule::GetPressure(
+ int channel) const {
+ return 0_psi;
+}
+
Solenoid PneumaticsControlModule::MakeSolenoid(int channel) {
return Solenoid{m_module, PneumaticsModuleType::CTREPCM, channel};
}
diff --git a/wpilibc/src/main/native/cpp/PowerDistribution.cpp b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
index 4c2acfc..21101f2 100644
--- a/wpilibc/src/main/native/cpp/PowerDistribution.cpp
+++ b/wpilibc/src/main/native/cpp/PowerDistribution.cpp
@@ -16,9 +16,6 @@
#include "frc/SensorUtil.h"
static_assert(static_cast<HAL_PowerDistributionType>(
- frc::PowerDistribution::ModuleType::kAutomatic) ==
- HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic);
-static_assert(static_cast<HAL_PowerDistributionType>(
frc::PowerDistribution::ModuleType::kCTRE) ==
HAL_PowerDistributionType::HAL_PowerDistributionType_kCTRE);
static_assert(static_cast<HAL_PowerDistributionType>(
@@ -29,8 +26,21 @@
using namespace frc;
-PowerDistribution::PowerDistribution()
- : PowerDistribution(-1, ModuleType::kAutomatic) {}
+PowerDistribution::PowerDistribution() {
+ auto stack = wpi::GetStackTrace(1);
+
+ int32_t status = 0;
+ m_handle = HAL_InitializePowerDistribution(
+ kDefaultModule,
+ HAL_PowerDistributionType::HAL_PowerDistributionType_kAutomatic,
+ stack.c_str(), &status);
+ FRC_CheckErrorStatus(status, "Module {}", kDefaultModule);
+ m_module = HAL_GetPowerDistributionModuleNumber(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+
+ HAL_Report(HALUsageReporting::kResourceType_PDP, m_module + 1);
+ wpi::SendableRegistry::AddLW(this, "PowerDistribution", m_module);
+}
PowerDistribution::PowerDistribution(int module, ModuleType moduleType) {
auto stack = wpi::GetStackTrace(1);
@@ -114,6 +124,13 @@
return m_module;
}
+PowerDistribution::ModuleType PowerDistribution::GetType() const {
+ int32_t status = 0;
+ auto type = HAL_GetPowerDistributionType(m_handle, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ return static_cast<ModuleType>(type);
+}
+
bool PowerDistribution::GetSwitchableChannel() const {
int32_t status = 0;
bool state = HAL_GetPowerDistributionSwitchableChannel(m_handle, &status);
@@ -127,20 +144,85 @@
FRC_ReportError(status, "Module {}", m_module);
}
+PowerDistribution::Version PowerDistribution::GetVersion() const {
+ int32_t status = 0;
+ HAL_PowerDistributionVersion halVersion;
+ std::memset(&halVersion, 0, sizeof(halVersion));
+ HAL_GetPowerDistributionVersion(m_handle, &halVersion, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::Version version;
+ static_assert(sizeof(halVersion) == sizeof(version));
+ static_assert(std::is_standard_layout_v<decltype(version)>);
+ static_assert(std::is_trivial_v<decltype(version)>);
+ std::memcpy(&version, &halVersion, sizeof(version));
+ return version;
+}
+
+PowerDistribution::Faults PowerDistribution::GetFaults() const {
+ int32_t status = 0;
+ HAL_PowerDistributionFaults halFaults;
+ std::memset(&halFaults, 0, sizeof(halFaults));
+ HAL_GetPowerDistributionFaults(m_handle, &halFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::Faults faults;
+ static_assert(sizeof(halFaults) == sizeof(faults));
+ static_assert(std::is_standard_layout_v<decltype(faults)>);
+ static_assert(std::is_trivial_v<decltype(faults)>);
+ std::memcpy(&faults, &halFaults, sizeof(faults));
+ return faults;
+}
+
+PowerDistribution::StickyFaults PowerDistribution::GetStickyFaults() const {
+ int32_t status = 0;
+ HAL_PowerDistributionStickyFaults halStickyFaults;
+ std::memset(&halStickyFaults, 0, sizeof(halStickyFaults));
+ HAL_GetPowerDistributionStickyFaults(m_handle, &halStickyFaults, &status);
+ FRC_ReportError(status, "Module {}", m_module);
+ PowerDistribution::StickyFaults stickyFaults;
+ static_assert(sizeof(halStickyFaults) == sizeof(stickyFaults));
+ static_assert(std::is_standard_layout_v<decltype(stickyFaults)>);
+ static_assert(std::is_trivial_v<decltype(stickyFaults)>);
+ std::memcpy(&stickyFaults, &halStickyFaults, sizeof(stickyFaults));
+ return stickyFaults;
+}
+
void PowerDistribution::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("PowerDistribution");
int32_t status = 0;
int numChannels = HAL_GetPowerDistributionNumChannels(m_handle, &status);
FRC_ReportError(status, "Module {}", m_module);
+ // Use manual reads to avoid printing errors
for (int i = 0; i < numChannels; ++i) {
builder.AddDoubleProperty(
- fmt::format("Chan{}", i), [=] { return GetCurrent(i); }, nullptr);
+ fmt::format("Chan{}", i),
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionChannelCurrent(m_handle, i, &lStatus);
+ },
+ nullptr);
}
builder.AddDoubleProperty(
- "Voltage", [=] { return GetVoltage(); }, nullptr);
+ "Voltage",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionVoltage(m_handle, &lStatus);
+ },
+ nullptr);
builder.AddDoubleProperty(
- "TotalCurrent", [=] { return GetTotalCurrent(); }, nullptr);
+ "TotalCurrent",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionTotalCurrent(m_handle, &lStatus);
+ },
+ nullptr);
builder.AddBooleanProperty(
- "SwitchableChannel", [=] { return GetSwitchableChannel(); },
- [=](bool value) { SetSwitchableChannel(value); });
+ "SwitchableChannel",
+ [=] {
+ int32_t lStatus = 0;
+ return HAL_GetPowerDistributionSwitchableChannel(m_handle, &lStatus);
+ },
+ [=](bool value) {
+ int32_t lStatus = 0;
+ HAL_SetPowerDistributionSwitchableChannel(m_handle, value, &lStatus);
+ });
}
diff --git a/wpilibc/src/main/native/cpp/Preferences.cpp b/wpilibc/src/main/native/cpp/Preferences.cpp
index 0c82f54..4663875 100644
--- a/wpilibc/src/main/native/cpp/Preferences.cpp
+++ b/wpilibc/src/main/native/cpp/Preferences.cpp
@@ -30,6 +30,14 @@
return instance;
}
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetPreferencesInstance() {
+ GetInstance() = Instance();
+}
+} // namespace frc::impl
+#endif
+
Preferences* Preferences::GetInstance() {
::GetInstance();
static Preferences instance;
@@ -79,6 +87,7 @@
void Preferences::InitString(std::string_view key, std::string_view value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultString(value);
+ entry.SetPersistent();
}
void Preferences::SetInt(std::string_view key, int value) {
@@ -94,6 +103,7 @@
void Preferences::InitInt(std::string_view key, int value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetDouble(std::string_view key, double value) {
@@ -109,6 +119,7 @@
void Preferences::InitDouble(std::string_view key, double value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetFloat(std::string_view key, float value) {
@@ -124,6 +135,7 @@
void Preferences::InitFloat(std::string_view key, float value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
void Preferences::SetBoolean(std::string_view key, bool value) {
@@ -139,6 +151,7 @@
void Preferences::InitBoolean(std::string_view key, bool value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultBoolean(value);
+ entry.SetPersistent();
}
void Preferences::SetLong(std::string_view key, int64_t value) {
@@ -154,6 +167,7 @@
void Preferences::InitLong(std::string_view key, int64_t value) {
auto entry = ::GetInstance().table->GetEntry(key);
entry.SetDefaultDouble(value);
+ entry.SetPersistent();
}
bool Preferences::ContainsKey(std::string_view key) {
diff --git a/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
new file mode 100644
index 0000000..8c7bb58
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/ExternalDirectionCounter.cpp
@@ -0,0 +1,103 @@
+// 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/counter/ExternalDirectionCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+ DigitalSource& countSource, DigitalSource& directionSource)
+ : ExternalDirectionCounter(
+ {&countSource, wpi::NullDeleter<DigitalSource>()},
+ {&directionSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+ExternalDirectionCounter::ExternalDirectionCounter(
+ std::shared_ptr<DigitalSource> countSource,
+ std::shared_ptr<DigitalSource> directionSource) {
+ if (countSource == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "countSource");
+ }
+ if (directionSource == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "directionSource");
+ }
+
+ m_countSource = countSource;
+ m_directionSource = directionSource;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeCounter(
+ HAL_Counter_Mode::HAL_Counter_kExternalDirection, &m_index, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_SetCounterUpSource(m_handle, m_countSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_countSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_SetCounterDownSource(
+ m_handle, m_directionSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_directionSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterDownSourceEdge(m_handle, false, true, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ Reset();
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "External Direction Counter", m_index);
+}
+
+ExternalDirectionCounter::~ExternalDirectionCounter() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+int ExternalDirectionCounter::GetCount() const {
+ int32_t status = 0;
+ int val = HAL_GetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ return val;
+}
+
+void ExternalDirectionCounter::SetReverseDirection(bool reverseDirection) {
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::Reset() {
+ int32_t status = 0;
+ HAL_ResetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::SetEdgeConfiguration(
+ EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void ExternalDirectionCounter::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("External Direction Counter");
+ builder.AddDoubleProperty(
+ "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/Tachometer.cpp b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp
new file mode 100644
index 0000000..a52bea5
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/Tachometer.cpp
@@ -0,0 +1,122 @@
+// 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/counter/Tachometer.h"
+
+#include <frc/DigitalSource.h>
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/Errors.h"
+
+using namespace frc;
+
+Tachometer::Tachometer(DigitalSource& source)
+ : Tachometer({&source, wpi::NullDeleter<DigitalSource>()}) {}
+Tachometer::Tachometer(std::shared_ptr<DigitalSource> source) {
+ if (source == nullptr) {
+ throw FRC_MakeError(err::NullParameter, "{}", "source");
+ }
+
+ m_source = source;
+
+ int32_t status = 0;
+ HAL_SetCounterUpSource(m_handle, m_source->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_source->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "Tachometer", m_index);
+}
+
+Tachometer::~Tachometer() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+units::hertz_t Tachometer::GetFrequency() const {
+ auto period = GetPeriod();
+ if (period.to<double>() == 0) {
+ return units::hertz_t{0.0};
+ }
+ return 1 / period;
+}
+
+units::second_t Tachometer::GetPeriod() const {
+ int32_t status = 0;
+ double period = HAL_GetCounterPeriod(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return units::second_t{period};
+}
+
+int Tachometer::GetEdgesPerRevolution() const {
+ return m_edgesPerRevolution;
+}
+void Tachometer::SetEdgesPerRevolution(int edges) {
+ m_edgesPerRevolution = edges;
+}
+
+units::turns_per_second_t Tachometer::GetRevolutionsPerSecond() const {
+ auto period = GetPeriod();
+ if (period.to<double>() == 0) {
+ return units::turns_per_second_t{0.0};
+ }
+ int edgesPerRevolution = GetEdgesPerRevolution();
+ if (edgesPerRevolution == 0) {
+ return units::turns_per_second_t{0.0};
+ }
+ auto rotationHz = ((1.0 / edgesPerRevolution) / period);
+ return units::turns_per_second_t{rotationHz.to<double>()};
+}
+
+units::revolutions_per_minute_t Tachometer::GetRevolutionsPerMinute() const {
+ return units::revolutions_per_minute_t{GetRevolutionsPerSecond()};
+}
+
+bool Tachometer::GetStopped() const {
+ int32_t status = 0;
+ bool stopped = HAL_GetCounterStopped(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return stopped;
+}
+
+int Tachometer::GetSamplesToAverage() const {
+ int32_t status = 0;
+ int32_t samplesToAverage = HAL_GetCounterSamplesToAverage(m_handle, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+ return samplesToAverage;
+}
+
+void Tachometer::SetSamplesToAverage(int samples) {
+ int32_t status = 0;
+ HAL_SetCounterSamplesToAverage(m_handle, samples, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetMaxPeriod(units::second_t maxPeriod) {
+ int32_t status = 0;
+ HAL_SetCounterMaxPeriod(m_handle, maxPeriod.to<double>(), &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::SetUpdateWhenEmpty(bool updateWhenEmpty) {
+ int32_t status = 0;
+ HAL_SetCounterUpdateWhenEmpty(m_handle, updateWhenEmpty, &status);
+ FRC_CheckErrorStatus(status, "Channel {}", m_source->GetChannel());
+}
+
+void Tachometer::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("Tachometer");
+ builder.AddDoubleProperty(
+ "RPS", [&] { return GetRevolutionsPerSecond().to<double>(); }, nullptr);
+ builder.AddDoubleProperty(
+ "RPM", [&] { return GetRevolutionsPerMinute().to<double>(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
new file mode 100644
index 0000000..62537ee
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/counter/UpDownCounter.cpp
@@ -0,0 +1,106 @@
+// 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/counter/UpDownCounter.h"
+
+#include <hal/Counter.h>
+#include <hal/FRCUsageReporting.h>
+#include <wpi/NullDeleter.h>
+#include <wpi/sendable/SendableBuilder.h>
+
+#include "frc/DigitalSource.h"
+#include "frc/Errors.h"
+
+using namespace frc;
+
+UpDownCounter::UpDownCounter(DigitalSource& upSource, DigitalSource& downSource)
+ : UpDownCounter({&upSource, wpi::NullDeleter<DigitalSource>()},
+ {&downSource, wpi::NullDeleter<DigitalSource>()}) {}
+
+UpDownCounter::UpDownCounter(std::shared_ptr<DigitalSource> upSource,
+ std::shared_ptr<DigitalSource> downSource) {
+ m_upSource = upSource;
+ m_downSource = downSource;
+
+ int32_t status = 0;
+ m_handle = HAL_InitializeCounter(HAL_Counter_Mode::HAL_Counter_kTwoPulse,
+ &m_index, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+
+ if (m_upSource) {
+ HAL_SetCounterUpSource(m_handle, m_upSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_upSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterUpSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ }
+
+ if (m_downSource) {
+ HAL_SetCounterDownSource(
+ m_handle, m_downSource->GetPortHandleForRouting(),
+ static_cast<HAL_AnalogTriggerType>(
+ m_downSource->GetAnalogTriggerTypeForRouting()),
+ &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ HAL_SetCounterDownSourceEdge(m_handle, true, false, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ }
+
+ Reset();
+
+ HAL_Report(HALUsageReporting::kResourceType_Counter, m_index + 1);
+ wpi::SendableRegistry::AddLW(this, "UpDown Counter", m_index);
+}
+
+UpDownCounter::~UpDownCounter() {
+ int32_t status = 0;
+ HAL_FreeCounter(m_handle, &status);
+}
+
+int UpDownCounter::GetCount() const {
+ int32_t status = 0;
+ int val = HAL_GetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+ return val;
+}
+
+void UpDownCounter::SetReverseDirection(bool reverseDirection) {
+ int32_t status = 0;
+ HAL_SetCounterReverseDirection(m_handle, reverseDirection, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::Reset() {
+ int32_t status = 0;
+ HAL_ResetCounter(m_handle, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetUpEdgeConfiguration(EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterUpSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::SetDownEdgeConfiguration(EdgeConfiguration configuration) {
+ int32_t status = 0;
+ bool rising = configuration == EdgeConfiguration::kRisingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ bool falling = configuration == EdgeConfiguration::kFallingEdge ||
+ configuration == EdgeConfiguration::kBoth;
+ HAL_SetCounterDownSourceEdge(m_handle, rising, falling, &status);
+ FRC_CheckErrorStatus(status, "{}", m_index);
+}
+
+void UpDownCounter::InitSendable(wpi::SendableBuilder& builder) {
+ builder.SetSmartDashboardType("UpDown Counter");
+ builder.AddDoubleProperty(
+ "Count", [&] { return GetCount(); }, nullptr);
+}
diff --git a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
index b54b607..eed9495 100644
--- a/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/DifferentialDrive.cpp
@@ -112,9 +112,10 @@
double maxInput =
std::copysign(std::max(std::abs(xSpeed), std::abs(zRotation)), xSpeed);
- if (xSpeed >= 0.0) {
+ // Sign is used because `xSpeed >= 0.0` succeeds for -0.0
+ if (!std::signbit(xSpeed)) {
// First quadrant, else second quadrant
- if (zRotation >= 0.0) {
+ if (!std::signbit(zRotation)) {
leftSpeed = maxInput;
rightSpeed = xSpeed - zRotation;
} else {
@@ -123,7 +124,7 @@
}
} else {
// Third quadrant, else fourth quadrant
- if (zRotation >= 0.0) {
+ if (!std::signbit(zRotation)) {
leftSpeed = xSpeed + zRotation;
rightSpeed = maxInput;
} else {
diff --git a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
index c847678..268ae64 100644
--- a/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/KilloughDrive.cpp
@@ -102,7 +102,7 @@
wheelSpeeds[kRight] = input.ScalarProject(m_rightVec) + zRotation;
wheelSpeeds[kBack] = input.ScalarProject(m_backVec) + zRotation;
- Normalize(wheelSpeeds);
+ Desaturate(wheelSpeeds);
return {wheelSpeeds[kLeft], wheelSpeeds[kRight], wheelSpeeds[kBack]};
}
diff --git a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
index b74c611..70e90d4 100644
--- a/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
+++ b/wpilibc/src/main/native/cpp/drive/MecanumDrive.cpp
@@ -103,7 +103,7 @@
wheelSpeeds[kRearLeft] = input.x - input.y + zRotation;
wheelSpeeds[kRearRight] = input.x + input.y - zRotation;
- Normalize(wheelSpeeds);
+ Desaturate(wheelSpeeds);
return {wheelSpeeds[kFrontLeft], wheelSpeeds[kFrontRight],
wheelSpeeds[kRearLeft], wheelSpeeds[kRearRight]};
diff --git a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
index 2159958..97ea0ee 100644
--- a/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
+++ b/wpilibc/src/main/native/cpp/drive/RobotDriveBase.cpp
@@ -35,7 +35,7 @@
return frc::ApplyDeadband(value, deadband);
}
-void RobotDriveBase::Normalize(wpi::span<double> wheelSpeeds) {
+void RobotDriveBase::Desaturate(wpi::span<double> wheelSpeeds) {
double maxMagnitude = std::abs(wheelSpeeds[0]);
for (size_t i = 1; i < wheelSpeeds.size(); i++) {
double temp = std::abs(wheelSpeeds[i]);
diff --git a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
index f8be7de..ac1c8ad 100644
--- a/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
+++ b/wpilibc/src/main/native/cpp/livewindow/LiveWindow.cpp
@@ -48,11 +48,23 @@
};
} // namespace
-static Instance& GetInstance() {
- static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+ static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
return instance;
}
+static Instance& GetInstance() {
+ return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetLiveWindow() {
+ std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+} // namespace frc::impl
+#endif
+
std::shared_ptr<Component> Instance::GetOrAdd(wpi::Sendable* sendable) {
auto data = std::static_pointer_cast<Component>(
wpi::SendableRegistry::GetData(sendable, dataHandle));
diff --git a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
index 9829a75..f9ff4b8 100644
--- a/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
+++ b/wpilibc/src/main/native/cpp/motorcontrol/PWMMotorController.cpp
@@ -42,6 +42,10 @@
return m_pwm.GetChannel();
}
+void PWMMotorController::EnableDeadbandElimination(bool eliminateDeadband) {
+ m_pwm.EnableDeadbandElimination(eliminateDeadband);
+}
+
PWMMotorController::PWMMotorController(std::string_view name, int channel)
: m_pwm(channel, false) {
wpi::SendableRegistry::AddLW(this, name, channel);
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
index 1675c64..d5e076a 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/SendableCameraWrapper.cpp
@@ -8,19 +8,19 @@
#include <memory>
#include <string>
-#include <wpi/DenseMap.h>
+#include <wpi/StringMap.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
namespace frc {
namespace detail {
std::shared_ptr<SendableCameraWrapper>& GetSendableCameraWrapper(
- CS_Source source) {
- static wpi::DenseMap<int, std::shared_ptr<SendableCameraWrapper>> wrappers;
- return wrappers[static_cast<int>(source)];
+ std::string_view cameraName) {
+ static wpi::StringMap<std::shared_ptr<SendableCameraWrapper>> wrappers;
+ return wrappers[cameraName];
}
-void AddToSendableRegistry(wpi::Sendable* sendable, std::string name) {
+void AddToSendableRegistry(wpi::Sendable* sendable, std::string_view name) {
wpi::SendableRegistry::Add(sendable, name);
}
} // namespace detail
diff --git a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
index 81f6671..31e4b11 100644
--- a/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
+++ b/wpilibc/src/main/native/cpp/shuffleboard/ShuffleboardWidget.cpp
@@ -31,6 +31,7 @@
"Differential Drivebase",
"Mecanum Drivebase",
"Camera Stream",
+ "Field",
};
const char* detail::GetStringForWidgetType(BuiltInWidgets type) {
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
new file mode 100644
index 0000000..46c412a
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
@@ -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 angularRate) {
+ m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+ m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+ m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+ m_simAccelX.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+ m_simAccelY.Set(accel.value());
+}
+
+void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+ m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
new file mode 100644
index 0000000..40c09c3
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
@@ -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 angularRate) {
+ m_simGyroRateX.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) {
+ m_simGyroRateY.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) {
+ m_simGyroRateZ.Set(angularRate.value());
+}
+
+void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) {
+ m_simAccelX.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) {
+ m_simAccelY.Set(accel.value());
+}
+
+void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) {
+ m_simAccelZ.Set(accel.value());
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
new file mode 100644
index 0000000..7860e32
--- /dev/null
+++ b/wpilibc/src/main/native/cpp/simulation/DCMotorSim.cpp
@@ -0,0 +1,46 @@
+// 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/DCMotorSim.h"
+
+#include <wpi/MathExtras.h>
+
+#include "frc/system/plant/LinearSystemId.h"
+
+using namespace frc;
+using namespace frc::sim;
+
+DCMotorSim::DCMotorSim(const LinearSystem<2, 1, 2>& plant,
+ const DCMotor& gearbox, double gearing,
+ const std::array<double, 2>& measurementStdDevs)
+ : LinearSystemSim<2, 1, 2>(plant, measurementStdDevs),
+ m_gearbox(gearbox),
+ m_gearing(gearing) {}
+
+DCMotorSim::DCMotorSim(const DCMotor& gearbox, double gearing,
+ units::kilogram_square_meter_t moi,
+ const std::array<double, 2>& measurementStdDevs)
+ : DCMotorSim(LinearSystemId::DCMotorSystem(gearbox, moi, gearing), gearbox,
+ gearing, measurementStdDevs) {}
+
+units::radian_t DCMotorSim::GetAngularPosition() const {
+ return units::radian_t{GetOutput(0)};
+}
+
+units::radians_per_second_t DCMotorSim::GetAngularVelocity() const {
+ return units::radians_per_second_t{GetOutput(1)};
+}
+
+units::ampere_t DCMotorSim::GetCurrentDraw() const {
+ // I = V / R - omega / (Kv * R)
+ // Reductions are greater than 1, so a reduction of 10:1 would mean the motor
+ // is spinning 10x faster than the output.
+ return m_gearbox.Current(GetAngularVelocity() * m_gearing,
+ units::volt_t{m_u(0)}) *
+ wpi::sgn(m_u(0));
+}
+
+void DCMotorSim::SetInputVoltage(units::volt_t voltage) {
+ SetInput(Eigen::Vector<double, 1>{voltage.value()});
+}
diff --git a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
index 0bbf9c3..78b9dc7 100644
--- a/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/DifferentialDrivetrainSim.cpp
@@ -43,8 +43,8 @@
Eigen::Vector<double, 2> DifferentialDrivetrainSim::ClampInput(
const Eigen::Vector<double, 2>& u) {
- return frc::NormalizeInputVector<2>(u,
- frc::RobotController::GetInputVoltage());
+ return frc::DesaturateInputVector<2>(u,
+ frc::RobotController::GetInputVoltage());
}
void DifferentialDrivetrainSim::SetInputs(units::volt_t leftVoltage,
diff --git a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
index dd6dba8..ca7384a 100644
--- a/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
+++ b/wpilibc/src/main/native/cpp/simulation/REVPHSim.cpp
@@ -73,21 +73,23 @@
HALSIM_SetREVPHCompressorOn(m_index, compressorOn);
}
-std::unique_ptr<CallbackStore> REVPHSim::RegisterClosedLoopEnabledCallback(
+std::unique_ptr<CallbackStore> REVPHSim::RegisterCompressorConfigTypeCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
- m_index, -1, callback, &HALSIM_CancelREVPHClosedLoopEnabledCallback);
- store->SetUid(HALSIM_RegisterREVPHClosedLoopEnabledCallback(
+ m_index, -1, callback, &HALSIM_CancelREVPHCompressorConfigTypeCallback);
+ store->SetUid(HALSIM_RegisterREVPHCompressorConfigTypeCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
-bool REVPHSim::GetClosedLoopEnabled() const {
- return HALSIM_GetREVPHClosedLoopEnabled(m_index);
+int REVPHSim::GetCompressorConfigType() const {
+ return HALSIM_GetREVPHCompressorConfigType(m_index);
}
-void REVPHSim::SetClosedLoopEnabled(bool closedLoopEnabled) {
- HALSIM_SetREVPHClosedLoopEnabled(m_index, closedLoopEnabled);
+void REVPHSim::SetCompressorConfigType(int compressorConfigType) {
+ HALSIM_SetREVPHCompressorConfigType(
+ m_index,
+ static_cast<HAL_REVPHCompressorConfigType>(compressorConfigType));
}
std::unique_ptr<CallbackStore> REVPHSim::RegisterPressureSwitchCallback(
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
index 175db0d..b49bc99 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismLigament2d.cpp
@@ -21,7 +21,6 @@
void MechanismLigament2d::UpdateEntries(
std::shared_ptr<nt::NetworkTable> table) {
- std::scoped_lock lock(m_mutex);
table->GetEntry(".type").SetString("line");
m_colorEntry = table->GetEntry("color");
@@ -50,7 +49,20 @@
Flush();
}
+Color8Bit MechanismLigament2d::GetColor() {
+ std::scoped_lock lock(m_mutex);
+ if (m_colorEntry) {
+ auto color = m_colorEntry.GetString("");
+ std::strncpy(m_color, color.c_str(), sizeof(m_color));
+ m_color[sizeof(m_color) - 1] = '\0';
+ }
+ unsigned int r = 0, g = 0, b = 0;
+ std::sscanf(m_color, "#%02X%02X%02X", &r, &g, &b);
+ return {static_cast<int>(r), static_cast<int>(g), static_cast<int>(b)};
+}
+
double MechanismLigament2d::GetAngle() {
+ std::scoped_lock lock(m_mutex);
if (m_angleEntry) {
m_angle = m_angleEntry.GetDouble(0.0);
}
@@ -58,12 +70,21 @@
}
double MechanismLigament2d::GetLength() {
+ std::scoped_lock lock(m_mutex);
if (m_lengthEntry) {
m_length = m_lengthEntry.GetDouble(0.0);
}
return m_length;
}
+double MechanismLigament2d::GetLineWeight() {
+ std::scoped_lock lock(m_mutex);
+ if (m_weightEntry) {
+ m_weight = m_weightEntry.GetDouble(0.0);
+ }
+ return m_weight;
+}
+
void MechanismLigament2d::SetLength(double length) {
std::scoped_lock lock(m_mutex);
m_length = length;
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
index 07d15f0..637b24b 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/MechanismRoot2d.cpp
@@ -20,14 +20,12 @@
}
void MechanismRoot2d::UpdateEntries(std::shared_ptr<nt::NetworkTable> table) {
- std::scoped_lock lock(m_mutex);
m_xEntry = table->GetEntry("x");
m_yEntry = table->GetEntry("y");
Flush();
}
inline void MechanismRoot2d::Flush() {
- std::scoped_lock lock(m_mutex);
if (m_xEntry) {
m_xEntry.SetDouble(m_x);
}
diff --git a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
index 25bb61f..5088394 100644
--- a/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
+++ b/wpilibc/src/main/native/cpp/smartdashboard/SmartDashboard.cpp
@@ -19,6 +19,8 @@
namespace {
struct Instance {
+ Instance() { HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0); }
+
detail::ListenerExecutor listenerExecutor;
std::shared_ptr<nt::NetworkTable> table =
nt::NetworkTableInstance::GetDefault().GetTable("SmartDashboard");
@@ -27,12 +29,23 @@
};
} // namespace
-static Instance& GetInstance() {
- HAL_Report(HALUsageReporting::kResourceType_SmartDashboard, 0);
- static Instance instance;
+static std::unique_ptr<Instance>& GetInstanceHolder() {
+ static std::unique_ptr<Instance> instance = std::make_unique<Instance>();
return instance;
}
+static Instance& GetInstance() {
+ return *GetInstanceHolder();
+}
+
+#ifndef __FRC_ROBORIO__
+namespace frc::impl {
+void ResetSmartDashboardInstance() {
+ std::make_unique<Instance>().swap(GetInstanceHolder());
+}
+} // namespace frc::impl
+#endif
+
void SmartDashboard::init() {
GetInstance();
}