Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-29 02:21:44 +00:00
Code Issues Packages Projects Releases Wiki Activity

[hal, wpilib] Remove SPI support (#7678)

Browse Source
This commit is contained in:
Thad House
2025-01-17 00:22:29 -08:00
committed by GitHub
parent dc335ddedb
commit 92f0a3c961
84 changed files with 12 additions and 12763 deletions
Download Patch File Download Diff File Expand all files Collapse all files

496
wpilibc/src/main/native/include/frc/ADIS16448_IMU.h
View File

@@ -1,496 +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.
/*----------------------------------------------------------------------------*/
/* 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 */
/*----------------------------------------------------------------------------*/
#pragma once
#include <stdint.h>
#include <atomic>
#include <thread>
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <units/magnetic_field_strength.h>
#include <units/pressure.h>
#include <units/temperature.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/DigitalInput.h"
#include "frc/DigitalOutput.h"
#include "frc/SPI.h"
namespace frc {
/**
* Use DMA SPI to read rate, acceleration, and magnetometer data from the
* ADIS16448 IMU and return the robots heading relative to a starting position,
* AHRS, and instant measurements
*
* The ADIS16448 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16448 IMU connected via the SPI port available on
* the RoboRIO MXP port.
*/
class ADIS16448_IMU : public wpi::Sendable,
public wpi::SendableHelper<ADIS16448_IMU> {
public:
/**
* ADIS16448 calibration times.
*/
enum class CalibrationTime {
/// 32 ms calibration time.
_32ms = 0,
/// 64 ms calibration time.
_64ms = 1,
/// 128 ms calibration time.
_128ms = 2,
/// 256 ms calibration time.
_256ms = 3,
/// 512 ms calibration time.
_512ms = 4,
/// 1 s calibration time.
_1s = 5,
/// 2 s calibration time.
_2s = 6,
/// 4 s calibration time.
_4s = 7,
/// 8 s calibration time.
_8s = 8,
/// 16 s calibration time.
_16s = 9,
/// 32 s calibration time.
_32s = 10,
/// 64 s calibration time.
_64s = 11
};
/**
* IMU axes.
*/
enum IMUAxis {
/// The IMU's X axis.
kX,
/// The IMU's Y axis.
kY,
/// The IMU's Z axis.
kZ
};
/**
* IMU constructor on onboard MXP CS0, Z-up orientation, and complementary
* AHRS computation.
*/
ADIS16448_IMU();
/**
* IMU constructor on the specified MXP port and orientation.
*
* @param yaw_axis The axis where gravity is present. Valid options are kX,
* kY, and kZ
* @param port The SPI port where the IMU is connected.
* @param cal_time The calibration time that should be used on start-up.
*/
explicit ADIS16448_IMU(IMUAxis yaw_axis, SPI::Port port,
CalibrationTime cal_time);
~ADIS16448_IMU() override;
ADIS16448_IMU(ADIS16448_IMU&&);
ADIS16448_IMU& operator=(ADIS16448_IMU&&);
/**
* Initialize the IMU.
*
* Perform gyro offset calibration by collecting data for a number of seconds
* and computing the center value. The center value is subtracted from
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the match
* starts.
*
* The calibration routine can be triggered by the user during runtime.
*/
void Calibrate();
/**
* Configures the calibration time used for the next calibrate.
*
* @param new_cal_time The calibration time that should be used
*/
int ConfigCalTime(CalibrationTime new_cal_time);
/**
* Reset the gyro.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* there is significant drift in the gyro and it needs to be recalibrated
* after running.
*/
void Reset();
/**
* Returns the yaw axis angle in degrees (CCW positive).
*/
units::degree_t GetAngle() const;
/**
* Returns the yaw axis angular rate in degrees per second (CCW positive).
*/
units::degrees_per_second_t GetRate() const;
/**
* Returns the accumulated gyro angle in the X axis.
*/
units::degree_t GetGyroAngleX() const;
/**
* Returns the accumulated gyro angle in the Y axis.
*/
units::degree_t GetGyroAngleY() const;
/**
* Returns the accumulated gyro angle in the Z axis.
*/
units::degree_t GetGyroAngleZ() const;
/**
* Returns the angular rate in the X axis.
*/
units::degrees_per_second_t GetGyroRateX() const;
/**
* Returns the angular rate in the Y axis.
*/
units::degrees_per_second_t GetGyroRateY() const;
/**
* Returns the angular rate in the Z axis.
*/
units::degrees_per_second_t GetGyroRateZ() const;
/**
* Returns the acceleration in the X axis.
*/
units::meters_per_second_squared_t GetAccelX() const;
/**
* Returns the acceleration in the Y axis.
*/
units::meters_per_second_squared_t GetAccelY() const;
/**
* Returns the acceleration in the Z axis.
*/
units::meters_per_second_squared_t GetAccelZ() const;
/**
* Returns the complementary angle around the X axis computed from
* accelerometer and gyro rate measurements.
*/
units::degree_t GetXComplementaryAngle() const;
/**
* Returns the complementary angle around the Y axis computed from
* accelerometer and gyro rate measurements.
*/
units::degree_t GetYComplementaryAngle() const;
/**
* Returns the X-axis filtered acceleration angle.
*/
units::degree_t GetXFilteredAccelAngle() const;
/**
* Returns the Y-axis filtered acceleration angle.
*/
units::degree_t GetYFilteredAccelAngle() const;
/**
* Returns the magnetic field strength in the X axis.
*/
units::tesla_t GetMagneticFieldX() const;
/**
* Returns the magnetic field strength in the Y axis.
*/
units::tesla_t GetMagneticFieldY() const;
/**
* Returns the magnetic field strength in the Z axis.
*/
units::tesla_t GetMagneticFieldZ() const;
/**
* Returns the barometric pressure.
*/
units::pounds_per_square_inch_t GetBarometricPressure() const;
/**
* Returns the temperature.
*/
units::celsius_t GetTemperature() const;
IMUAxis GetYawAxis() const;
int SetYawAxis(IMUAxis yaw_axis);
/**
* Checks the connection status of the IMU.
*
* @return True if the IMU is connected, false otherwise.
*/
bool IsConnected() const;
/**
* Configures the decimation rate of the IMU.
*
* @param decimationRate The new decimation value.
* @return 0 if success, 1 if no change, 2 if error.
*/
int ConfigDecRate(uint16_t decimationRate);
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
int GetPort() const;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
// ADIS16448 Register Map Declaration
static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
static constexpr uint8_t XGYRO_OUT = 0x04; // X-axis gyroscope output
static constexpr uint8_t YGYRO_OUT = 0x06; // Y-axis gyroscope output
static constexpr uint8_t ZGYRO_OUT = 0x08; // Z-axis gyroscope output
static constexpr uint8_t XACCL_OUT = 0x0A; // X-axis accelerometer output
static constexpr uint8_t YACCL_OUT = 0x0C; // Y-axis accelerometer output
static constexpr uint8_t ZACCL_OUT = 0x0E; // Z-axis accelerometer output
static constexpr uint8_t XMAGN_OUT = 0x10; // X-axis magnetometer output
static constexpr uint8_t YMAGN_OUT = 0x12; // Y-axis magnetometer output
static constexpr uint8_t ZMAGN_OUT = 0x14; // Z-axis magnetometer output
static constexpr uint8_t BARO_OUT =
0x16; // Barometer pressure measurement, high word
static constexpr uint8_t TEMP_OUT = 0x18; // Temperature output
static constexpr uint8_t XGYRO_OFF =
0x1A; // X-axis gyroscope bias offset factor
static constexpr uint8_t YGYRO_OFF =
0x1C; // Y-axis gyroscope bias offset factor
static constexpr uint8_t ZGYRO_OFF =
0x1E; // Z-axis gyroscope bias offset factor
static constexpr uint8_t XACCL_OFF =
0x20; // X-axis acceleration bias offset factor
static constexpr uint8_t YACCL_OFF =
0x22; // Y-axis acceleration bias offset factor
static constexpr uint8_t ZACCL_OFF =
0x24; // Z-axis acceleration bias offset factor
static constexpr uint8_t XMAGN_HIC =
0x26; // X-axis magnetometer, hard iron factor
static constexpr uint8_t YMAGN_HIC =
0x28; // Y-axis magnetometer, hard iron factor
static constexpr uint8_t ZMAGN_HIC =
0x2A; // Z-axis magnetometer, hard iron factor
static constexpr uint8_t XMAGN_SIC =
0x2C; // X-axis magnetometer, soft iron factor
static constexpr uint8_t YMAGN_SIC =
0x2E; // Y-axis magnetometer, soft iron factor
static constexpr uint8_t ZMAGN_SIC =
0x30; // Z-axis magnetometer, soft iron factor
static constexpr uint8_t GPIO_CTRL = 0x32; // GPIO control
static constexpr uint8_t MSC_CTRL = 0x34; // MISC control
static constexpr uint8_t SMPL_PRD =
0x36; // Sample clock/Decimation filter control
static constexpr uint8_t SENS_AVG = 0x38; // Digital filter control
static constexpr uint8_t SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter
static constexpr uint8_t DIAG_STAT = 0x3C; // System status
static constexpr uint8_t GLOB_CMD = 0x3E; // System command
static constexpr uint8_t ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold
static constexpr uint8_t ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold
static constexpr uint8_t ALM_SMPL1 = 0x44; // Alarm 1 sample size
static constexpr uint8_t ALM_SMPL2 = 0x46; // Alarm 2 sample size
static constexpr uint8_t ALM_CTRL = 0x48; // Alarm control
static constexpr uint8_t LOT_ID1 = 0x52; // Lot identification number
static constexpr uint8_t LOT_ID2 = 0x54; // Lot identification number
static constexpr uint8_t PROD_ID = 0x56; // Product identifier
static constexpr uint8_t SERIAL_NUM = 0x58; // Lot-specific serial number
/** @brief ADIS16448 Static Constants */
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
/** @brief struct to store offset data */
struct OffsetData {
double gyro_rate_x = 0.0;
double gyro_rate_y = 0.0;
double gyro_rate_z = 0.0;
};
/** @brief Internal Resources **/
DigitalInput* m_reset_in = nullptr;
DigitalOutput* m_status_led = nullptr;
bool SwitchToStandardSPI();
bool SwitchToAutoSPI();
uint16_t ReadRegister(uint8_t reg);
void WriteRegister(uint8_t reg, uint16_t val);
void Acquire();
void Close();
// User-specified yaw axis
IMUAxis m_yaw_axis;
// Last read values (post-scaling)
double m_gyro_rate_x = 0.0;
double m_gyro_rate_y = 0.0;
double m_gyro_rate_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
double m_mag_x = 0.0;
double m_mag_y = 0.0;
double m_mag_z = 0.0;
double m_baro = 0.0;
double m_temp = 0.0;
// Complementary filter variables
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 0.5;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// vector for storing most recent imu values
OffsetData* m_offset_buffer = nullptr;
double m_gyro_rate_offset_x = 0.0;
double m_gyro_rate_offset_y = 0.0;
double m_gyro_rate_offset_z = 0.0;
// function to re-init offset buffer
void InitOffsetBuffer(int size);
// Accumulated gyro values (for offset calculation)
int m_avg_size = 0;
int m_accum_count = 0;
// Integrated gyro values
double m_integ_gyro_angle_x = 0.0;
double m_integ_gyro_angle_y = 0.0;
double m_integ_gyro_angle_z = 0.0;
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);
// State and resource variables
std::atomic<bool> m_thread_active = false;
std::atomic<bool> m_first_run = true;
std::atomic<bool> m_thread_idle = false;
std::atomic<bool> m_start_up_mode = true;
bool m_auto_configured = false;
SPI::Port m_spi_port;
CalibrationTime m_calibration_time{0};
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
bool m_connected{false};
std::thread m_acquire_task;
hal::SimDevice m_simDevice;
hal::SimBoolean m_simConnected;
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
// CRC-16 Look-Up Table
static constexpr uint16_t m_adiscrc[256] = {
0x0000, 0x17CE, 0x0FDF, 0x1811, 0x1FBE, 0x0870, 0x1061, 0x07AF, 0x1F3F,
0x08F1, 0x10E0, 0x072E, 0x0081, 0x174F, 0x0F5E, 0x1890, 0x1E3D, 0x09F3,
0x11E2, 0x062C, 0x0183, 0x164D, 0x0E5C, 0x1992, 0x0102, 0x16CC, 0x0EDD,
0x1913, 0x1EBC, 0x0972, 0x1163, 0x06AD, 0x1C39, 0x0BF7, 0x13E6, 0x0428,
0x0387, 0x1449, 0x0C58, 0x1B96, 0x0306, 0x14C8, 0x0CD9, 0x1B17, 0x1CB8,
0x0B76, 0x1367, 0x04A9, 0x0204, 0x15CA, 0x0DDB, 0x1A15, 0x1DBA, 0x0A74,
0x1265, 0x05AB, 0x1D3B, 0x0AF5, 0x12E4, 0x052A, 0x0285, 0x154B, 0x0D5A,
0x1A94, 0x1831, 0x0FFF, 0x17EE, 0x0020, 0x078F, 0x1041, 0x0850, 0x1F9E,
0x070E, 0x10C0, 0x08D1, 0x1F1F, 0x18B0, 0x0F7E, 0x176F, 0x00A1, 0x060C,
0x11C2, 0x09D3, 0x1E1D, 0x19B2, 0x0E7C, 0x166D, 0x01A3, 0x1933, 0x0EFD,
0x16EC, 0x0122, 0x068D, 0x1143, 0x0952, 0x1E9C, 0x0408, 0x13C6, 0x0BD7,
0x1C19, 0x1BB6, 0x0C78, 0x1469, 0x03A7, 0x1B37, 0x0CF9, 0x14E8, 0x0326,
0x0489, 0x1347, 0x0B56, 0x1C98, 0x1A35, 0x0DFB, 0x15EA, 0x0224, 0x058B,
0x1245, 0x0A54, 0x1D9A, 0x050A, 0x12C4, 0x0AD5, 0x1D1B, 0x1AB4, 0x0D7A,
0x156B, 0x02A5, 0x1021, 0x07EF, 0x1FFE, 0x0830, 0x0F9F, 0x1851, 0x0040,
0x178E, 0x0F1E, 0x18D0, 0x00C1, 0x170F, 0x10A0, 0x076E, 0x1F7F, 0x08B1,
0x0E1C, 0x19D2, 0x01C3, 0x160D, 0x11A2, 0x066C, 0x1E7D, 0x09B3, 0x1123,
0x06ED, 0x1EFC, 0x0932, 0x0E9D, 0x1953, 0x0142, 0x168C, 0x0C18, 0x1BD6,
0x03C7, 0x1409, 0x13A6, 0x0468, 0x1C79, 0x0BB7, 0x1327, 0x04E9, 0x1CF8,
0x0B36, 0x0C99, 0x1B57, 0x0346, 0x1488, 0x1225, 0x05EB, 0x1DFA, 0x0A34,
0x0D9B, 0x1A55, 0x0244, 0x158A, 0x0D1A, 0x1AD4, 0x02C5, 0x150B, 0x12A4,
0x056A, 0x1D7B, 0x0AB5, 0x0810, 0x1FDE, 0x07CF, 0x1001, 0x17AE, 0x0060,
0x1871, 0x0FBF, 0x172F, 0x00E1, 0x18F0, 0x0F3E, 0x0891, 0x1F5F, 0x074E,
0x1080, 0x162D, 0x01E3, 0x19F2, 0x0E3C, 0x0993, 0x1E5D, 0x064C, 0x1182,
0x0912, 0x1EDC, 0x06CD, 0x1103, 0x16AC, 0x0162, 0x1973, 0x0EBD, 0x1429,
0x03E7, 0x1BF6, 0x0C38, 0x0B97, 0x1C59, 0x0448, 0x1386, 0x0B16, 0x1CD8,
0x04C9, 0x1307, 0x14A8, 0x0366, 0x1B77, 0x0CB9, 0x0A14, 0x1DDA, 0x05CB,
0x1205, 0x15AA, 0x0264, 0x1A75, 0x0DBB, 0x152B, 0x02E5, 0x1AF4, 0x0D3A,
0x0A95, 0x1D5B, 0x054A, 0x1284};
};
} // namespace frc

551
wpilibc/src/main/native/include/frc/ADIS16470_IMU.h
View File

@@ -1,551 +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.
/*----------------------------------------------------------------------------*/
/* 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 */
/*----------------------------------------------------------------------------*/
#pragma once
#include <stdint.h>
#include <atomic>
#include <thread>
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/DigitalInput.h"
#include "frc/DigitalOutput.h"
#include "frc/SPI.h"
namespace frc {
/**
* Use DMA SPI to read rate and acceleration data from the ADIS16470 IMU and
* return the robot's heading relative to a starting position and instant
* measurements
*
* The ADIS16470 gyro angle outputs track the robot's heading based on the
* starting position. As the robot rotates the new heading is computed by
* integrating the rate of rotation returned by the IMU. When the class is
* instantiated, a short calibration routine is performed where the IMU samples
* the gyros while at rest to determine the initial offset. This is subtracted
* from each sample to determine the heading.
*
* This class is for the ADIS16470 IMU connected via the primary SPI port
* available on the RoboRIO.
*/
class ADIS16470_IMU : public wpi::Sendable,
public wpi::SendableHelper<ADIS16470_IMU> {
public:
/**
* ADIS16470 calibration times.
*/
enum class CalibrationTime {
/// 32 ms calibration time.
_32ms = 0,
/// 64 ms calibration time.
_64ms = 1,
/// 128 ms calibration time.
_128ms = 2,
/// 256 ms calibration time.
_256ms = 3,
/// 512 ms calibration time.
_512ms = 4,
/// 1 s calibration time.
_1s = 5,
/// 2 s calibration time.
_2s = 6,
/// 4 s calibration time.
_4s = 7,
/// 8 s calibration time.
_8s = 8,
/// 16 s calibration time.
_16s = 9,
/// 32 s calibration time.
_32s = 10,
/// 64 s calibration time.
_64s = 11
};
/**
* IMU axes.
*
* kX, kY, and kZ refer to the IMU's X, Y, and Z axes respectively. kYaw,
* kPitch, and kRoll are configured by the user to refer to an X, Y, or Z
* axis.
*/
enum IMUAxis {
/// The IMU's X axis.
kX,
/// The IMU's Y axis.
kY,
/// The IMU's Z axis.
kZ,
/// The user-configured yaw axis.
kYaw,
/// The user-configured pitch axis.
kPitch,
/// The user-configured roll axis.
kRoll
};
/**
* Creates a new ADIS16740 IMU object.
*
* The default setup is the onboard SPI port with a calibration time of 1
* second. Yaw, pitch, and roll are kZ, kX, and kY respectively.
*/
ADIS16470_IMU();
/**
* Creates a new ADIS16740 IMU object.
*
* The default setup is the onboard SPI port with a calibration time of 1
* second.
*
* <b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll
* will result in an error.</i></b>
*
* @param yaw_axis The axis that measures the yaw
* @param pitch_axis The axis that measures the pitch
* @param roll_axis The axis that measures the roll
*/
ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis);
/**
* Creates a new ADIS16740 IMU object.
*
* <b><i>Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch, or
* kRoll will result in an error.</i></b>
*
* @param yaw_axis The axis that measures the yaw
* @param pitch_axis The axis that measures the pitch
* @param roll_axis The axis that measures the roll
* @param port The SPI Port the gyro is plugged into
* @param cal_time Calibration time
*/
explicit ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis, frc::SPI::Port port,
CalibrationTime cal_time);
~ADIS16470_IMU() override;
ADIS16470_IMU(ADIS16470_IMU&& other);
ADIS16470_IMU& operator=(ADIS16470_IMU&& other);
/**
* Configures the decimation rate of the IMU.
*
* @param decimationRate The new decimation value.
* @return 0 if success, 1 if no change, 2 if error.
*/
int ConfigDecRate(uint16_t decimationRate);
/**
* @brief Switches the active SPI port to standard SPI mode, writes the
* command to activate the new null configuration, and re-enables auto SPI.
*/
void Calibrate();
/**
* Configures calibration time.
*
* @param new_cal_time New calibration time
* @return 0 if success, 1 if no change, 2 if error.
*/
int ConfigCalTime(CalibrationTime new_cal_time);
/**
* Reset the gyro.
*
* Resets the gyro accumulations to a heading of zero. This can be used if
* there is significant drift in the gyro and it needs to be recalibrated
* after running.
*/
void Reset();
/**
* Allow the designated gyro angle to be set to a given value. This may happen
* with unread values in the buffer, it is suggested that the IMU is not
* moving when this method is run.
*
* @param axis IMUAxis that will be changed
* @param angle The new angle (CCW positive)
*/
void SetGyroAngle(IMUAxis axis, units::degree_t angle);
/**
* Allow the gyro angle X to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
void SetGyroAngleX(units::degree_t angle);
/**
* Allow the gyro angle Y to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Allow the gyro angle Z to be set to a given value. This may happen with
* unread values in the buffer, it is suggested that the IMU is not moving
* when this method is run.
*
* @param angle The new angle (CCW positive)
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Returns the axis angle (CCW positive).
*
* @param axis The IMUAxis whose angle to return. Defaults to user configured
* Yaw.
* @return The axis angle (CCW positive).
*/
units::degree_t GetAngle(IMUAxis axis = IMUAxis::kYaw) const;
/**
* Returns the axis angular rate (CCW positive).
*
* @param axis The IMUAxis whose rate to return. Defaults to user configured
* Yaw.
* @return Axis angular rate (CCW positive).
*/
units::degrees_per_second_t GetRate(IMUAxis axis = IMUAxis::kYaw) const;
/**
* Returns the acceleration in the X axis.
*/
units::meters_per_second_squared_t GetAccelX() const;
/**
* Returns the acceleration in the Y axis.
*/
units::meters_per_second_squared_t GetAccelY() const;
/**
* Returns the acceleration in the Z axis.
*/
units::meters_per_second_squared_t GetAccelZ() const;
/**
* Returns the X-axis complementary angle.
*/
units::degree_t GetXComplementaryAngle() const;
/**
* Returns the Y-axis complementary angle.
*/
units::degree_t GetYComplementaryAngle() const;
/**
* Returns the X-axis filtered acceleration angle.
*/
units::degree_t GetXFilteredAccelAngle() const;
/**
* Returns the Y-axis filtered acceleration angle.
*/
units::degree_t GetYFilteredAccelAngle() const;
/**
* Returns which axis, kX, kY, or kZ, is set to the yaw axis.
*
* @return IMUAxis Yaw Axis
*/
IMUAxis GetYawAxis() const;
/**
* Returns which axis, kX, kY, or kZ, is set to the pitch axis.
*
* @return IMUAxis Pitch Axis
*/
IMUAxis GetPitchAxis() const;
/**
* Returns which axis, kX, kY, or kZ, is set to the roll axis.
*
* @return IMUAxis Roll Axis
*/
IMUAxis GetRollAxis() const;
/**
* Checks the connection status of the IMU.
*
* @return True if the IMU is connected, false otherwise.
*/
bool IsConnected() const;
IMUAxis m_yaw_axis;
IMUAxis m_pitch_axis;
IMUAxis m_roll_axis;
/**
* Gets the SPI port number.
*
* @return The SPI port number.
*/
int GetPort() const;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
// Register Map Declaration
static constexpr uint8_t FLASH_CNT = 0x00; // Flash memory write count
static constexpr uint8_t DIAG_STAT =
0x02; // Diagnostic and operational status
static constexpr uint8_t X_GYRO_LOW =
0x04; // X-axis gyroscope output, lower word
static constexpr uint8_t X_GYRO_OUT =
0x06; // X-axis gyroscope output, upper word
static constexpr uint8_t Y_GYRO_LOW =
0x08; // Y-axis gyroscope output, lower word
static constexpr uint8_t Y_GYRO_OUT =
0x0A; // Y-axis gyroscope output, upper word
static constexpr uint8_t Z_GYRO_LOW =
0x0C; // Z-axis gyroscope output, lower word
static constexpr uint8_t Z_GYRO_OUT =
0x0E; // Z-axis gyroscope output, upper word
static constexpr uint8_t X_ACCL_LOW =
0x10; // X-axis accelerometer output, lower word
static constexpr uint8_t X_ACCL_OUT =
0x12; // X-axis accelerometer output, upper word
static constexpr uint8_t Y_ACCL_LOW =
0x14; // Y-axis accelerometer output, lower word
static constexpr uint8_t Y_ACCL_OUT =
0x16; // Y-axis accelerometer output, upper word
static constexpr uint8_t Z_ACCL_LOW =
0x18; // Z-axis accelerometer output, lower word
static constexpr uint8_t Z_ACCL_OUT =
0x1A; // Z-axis accelerometer output, upper word
static constexpr uint8_t TEMP_OUT =
0x1C; // Temperature output (internal, not calibrated)
static constexpr uint8_t TIME_STAMP = 0x1E; // PPS mode time stamp
static constexpr uint8_t X_DELTANG_LOW =
0x24; // X-axis delta angle output, lower word
static constexpr uint8_t X_DELTANG_OUT =
0x26; // X-axis delta angle output, upper word
static constexpr uint8_t Y_DELTANG_LOW =
0x28; // Y-axis delta angle output, lower word
static constexpr uint8_t Y_DELTANG_OUT =
0x2A; // Y-axis delta angle output, upper word
static constexpr uint8_t Z_DELTANG_LOW =
0x2C; // Z-axis delta angle output, lower word
static constexpr uint8_t Z_DELTANG_OUT =
0x2E; // Z-axis delta angle output, upper word
static constexpr uint8_t X_DELTVEL_LOW =
0x30; // X-axis delta velocity output, lower word
static constexpr uint8_t X_DELTVEL_OUT =
0x32; // X-axis delta velocity output, upper word
static constexpr uint8_t Y_DELTVEL_LOW =
0x34; // Y-axis delta velocity output, lower word
static constexpr uint8_t Y_DELTVEL_OUT =
0x36; // Y-axis delta velocity output, upper word
static constexpr uint8_t Z_DELTVEL_LOW =
0x38; // Z-axis delta velocity output, lower word
static constexpr uint8_t Z_DELTVEL_OUT =
0x3A; // Z-axis delta velocity output, upper word
static constexpr uint8_t XG_BIAS_LOW =
0x40; // X-axis gyroscope bias offset correction, lower word
static constexpr uint8_t XG_BIAS_HIGH =
0x42; // X-axis gyroscope bias offset correction, upper word
static constexpr uint8_t YG_BIAS_LOW =
0x44; // Y-axis gyroscope bias offset correction, lower word
static constexpr uint8_t YG_BIAS_HIGH =
0x46; // Y-axis gyroscope bias offset correction, upper word
static constexpr uint8_t ZG_BIAS_LOW =
0x48; // Z-axis gyroscope bias offset correction, lower word
static constexpr uint8_t ZG_BIAS_HIGH =
0x4A; // Z-axis gyroscope bias offset correction, upper word
static constexpr uint8_t XA_BIAS_LOW =
0x4C; // X-axis accelerometer bias offset correction, lower word
static constexpr uint8_t XA_BIAS_HIGH =
0x4E; // X-axis accelerometer bias offset correction, upper word
static constexpr uint8_t YA_BIAS_LOW =
0x50; // Y-axis accelerometer bias offset correction, lower word
static constexpr uint8_t YA_BIAS_HIGH =
0x52; // Y-axis accelerometer bias offset correction, upper word
static constexpr uint8_t ZA_BIAS_LOW =
0x54; // Z-axis accelerometer bias offset correction, lower word
static constexpr uint8_t ZA_BIAS_HIGH =
0x56; // Z-axis accelerometer bias offset correction, upper word
static constexpr uint8_t FILT_CTRL = 0x5C; // Filter control
static constexpr uint8_t MSC_CTRL = 0x60; // Miscellaneous control
static constexpr uint8_t UP_SCALE = 0x62; // Clock scale factor, PPS mode
static constexpr uint8_t DEC_RATE =
0x64; // Decimation rate control (output data rate)
static constexpr uint8_t NULL_CNFG = 0x66; // Auto-null configuration control
static constexpr uint8_t GLOB_CMD = 0x68; // Global commands
static constexpr uint8_t FIRM_REV = 0x6C; // Firmware revision
static constexpr uint8_t FIRM_DM =
0x6E; // Firmware revision date, month and day
static constexpr uint8_t FIRM_Y = 0x70; // Firmware revision date, year
static constexpr uint8_t PROD_ID = 0x72; // Product identification
static constexpr uint8_t SERIAL_NUM =
0x74; // Serial number (relative to assembly lot)
static constexpr uint8_t USER_SCR1 = 0x76; // User scratch register 1
static constexpr uint8_t USER_SCR2 = 0x78; // User scratch register 2
static constexpr uint8_t USER_SCR3 = 0x7A; // User scratch register 3
static constexpr uint8_t FLSHCNT_LOW =
0x7C; // Flash update count, lower word
static constexpr uint8_t FLSHCNT_HIGH =
0x7E; // Flash update count, upper word
// Auto SPI Data Packet to read all thrre gyro axes.
static constexpr uint8_t m_autospi_allangle_packet[24] = {
X_DELTANG_OUT, FLASH_CNT, X_DELTANG_LOW, FLASH_CNT, Y_DELTANG_OUT,
FLASH_CNT, Y_DELTANG_LOW, FLASH_CNT, Z_DELTANG_OUT, FLASH_CNT,
Z_DELTANG_LOW, FLASH_CNT, X_GYRO_OUT, FLASH_CNT, Y_GYRO_OUT,
FLASH_CNT, Z_GYRO_OUT, FLASH_CNT, X_ACCL_OUT, FLASH_CNT,
Y_ACCL_OUT, FLASH_CNT, Z_ACCL_OUT, FLASH_CNT};
static constexpr double delta_angle_sf = 2160.0 / 2147483648.0;
static constexpr double kRadToDeg = 57.2957795;
static constexpr double kDegToRad = 0.0174532;
static constexpr double kGrav = 9.81;
// Resources
DigitalInput* m_reset_in = nullptr;
DigitalOutput* m_status_led = nullptr;
/**
* @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.
*/
bool SwitchToStandardSPI();
/**
* @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.
*/
bool SwitchToAutoSPI();
/**
* @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.
*/
uint16_t ReadRegister(uint8_t reg);
/**
* @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.
*/
void WriteRegister(uint8_t reg, uint16_t val);
/**
* @brief Main acquisition loop. Typically called asynchronously and
* free-wheels while the robot code is active.
*/
void Acquire();
void Close();
// Integrated gyro angles.
double m_integ_angle_x = 0.0;
double m_integ_angle_y = 0.0;
double m_integ_angle_z = 0.0;
// Instant raw outputs
double m_gyro_rate_x = 0.0;
double m_gyro_rate_y = 0.0;
double m_gyro_rate_z = 0.0;
double m_accel_x = 0.0;
double m_accel_y = 0.0;
double m_accel_z = 0.0;
// Complementary filter variables
double m_dt, m_alpha = 0.0;
static constexpr double kTau = 1.0;
double m_compAngleX, m_compAngleY, m_accelAngleX, m_accelAngleY = 0.0;
// Complementary filter functions
double FormatFastConverge(double compAngle, double accAngle);
double FormatAccelRange(double accelAngle, double accelZ);
double CompFilterProcess(double compAngle, double accelAngle, double omega);
// State and resource variables
std::atomic<bool> m_thread_active = false;
std::atomic<bool> m_first_run = true;
std::atomic<bool> m_thread_idle = false;
bool m_auto_configured = false;
SPI::Port m_spi_port;
uint16_t m_calibration_time = 0;
SPI* m_spi = nullptr;
DigitalInput* m_auto_interrupt = nullptr;
double m_scaled_sample_rate = 2500.0; // Default sample rate setting
bool m_connected{false};
std::thread m_acquire_task;
hal::SimDevice m_simDevice;
hal::SimBoolean m_simConnected;
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;
struct NonMovableMutexWrapper {
wpi::mutex mutex;
NonMovableMutexWrapper() = default;
NonMovableMutexWrapper(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper& operator=(const NonMovableMutexWrapper&) = delete;
NonMovableMutexWrapper(NonMovableMutexWrapper&&) {}
NonMovableMutexWrapper& operator=(NonMovableMutexWrapper&&) {
return *this;
}
void lock() { mutex.lock(); }
void unlock() { mutex.unlock(); }
bool try_lock() noexcept { return mutex.try_lock(); }
};
mutable NonMovableMutexWrapper m_mutex;
};
} // namespace frc

156
wpilibc/src/main/native/include/frc/ADXL345_SPI.h
View File

@@ -1,156 +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.
#pragma once
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/SPI.h"
namespace frc {
/**
* ADXL345 Accelerometer on SPI.
*
* This class allows access to an Analog Devices ADXL345 3-axis accelerometer
* via SPI. This class assumes the sensor is wired in 4-wire SPI mode.
*/
class ADXL345_SPI : public nt::NTSendable,
public wpi::SendableHelper<ADXL345_SPI> {
public:
/**
* Accelerometer range.
*/
enum Range {
/// 2 Gs max.
kRange_2G = 0,
/// 4 Gs max.
kRange_4G = 1,
/// 8 Gs max.
kRange_8G = 2,
/// 16 Gs max.
kRange_16G = 3
};
/**
* Accelerometer axes.
*/
enum Axes {
/// X axis.
kAxis_X = 0x00,
/// Y axis.
kAxis_Y = 0x02,
/// Z axis.
kAxis_Z = 0x04
};
/**
* Container type for accelerations from all axes.
*/
struct AllAxes {
/// Acceleration along the X axis in g-forces.
double XAxis = 0.0;
/// Acceleration along the Y axis in g-forces.
double YAxis = 0.0;
/// Acceleration along the Z axis in g-forces.
double ZAxis = 0.0;
};
/**
* Constructor.
*
* @param port The SPI port the accelerometer is attached to
* @param range The range (+ or -) that the accelerometer will measure
*/
explicit ADXL345_SPI(SPI::Port port, Range range = kRange_2G);
~ADXL345_SPI() override = default;
ADXL345_SPI(ADXL345_SPI&&) = default;
ADXL345_SPI& operator=(ADXL345_SPI&&) = default;
SPI::Port GetSpiPort() const;
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the
* accelerometer will measure.
*/
void SetRange(Range range);
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
double GetX();
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
double GetY();
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
double GetZ();
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL345 in Gs.
*/
virtual double GetAcceleration(Axes axis);
/**
* Get the acceleration of all axes in Gs.
*
* @return An object containing the acceleration measured on each axis of the
* ADXL345 in Gs.
*/
virtual AllAxes GetAccelerations();
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
SPI m_spi;
hal::SimDevice m_simDevice;
hal::SimEnum m_simRange;
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
static constexpr int kPowerCtlRegister = 0x2D;
static constexpr int kDataFormatRegister = 0x31;
static constexpr int kDataRegister = 0x32;
static constexpr double kGsPerLSB = 0.00390625;
enum SPIAddressFields { kAddress_Read = 0x80, kAddress_MultiByte = 0x40 };
enum PowerCtlFields {
kPowerCtl_Link = 0x20,
kPowerCtl_AutoSleep = 0x10,
kPowerCtl_Measure = 0x08,
kPowerCtl_Sleep = 0x04
};
enum DataFormatFields {
kDataFormat_SelfTest = 0x80,
kDataFormat_SPI = 0x40,
kDataFormat_IntInvert = 0x20,
kDataFormat_FullRes = 0x08,
kDataFormat_Justify = 0x04
};
};
} // namespace frc

138
wpilibc/src/main/native/include/frc/ADXL362.h
View File

@@ -1,138 +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.
#pragma once
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/SPI.h"
namespace frc {
/**
* ADXL362 SPI Accelerometer.
*
* This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
*/
class ADXL362 : public nt::NTSendable, public wpi::SendableHelper<ADXL362> {
public:
/**
* Accelerometer range.
*/
enum Range {
/// 2 Gs max.
kRange_2G = 0,
/// 4 Gs max.
kRange_4G = 1,
/// 8 Gs max.
kRange_8G = 2
};
/**
* Accelerometer axes.
*/
enum Axes {
/// X axis.
kAxis_X = 0x00,
/// Y axis.
kAxis_Y = 0x02,
/// Z axis.
kAxis_Z = 0x04
};
/**
* Container type for accelerations from all axes.
*/
struct AllAxes {
/// Acceleration along the X axis in g-forces.
double XAxis = 0.0;
/// Acceleration along the Y axis in g-forces.
double YAxis = 0.0;
/// Acceleration along the Z axis in g-forces.
double ZAxis = 0.0;
};
public:
/**
* Constructor. Uses the onboard CS1.
*
* @param range The range (+ or -) that the accelerometer will measure.
*/
explicit ADXL362(Range range = kRange_2G);
/**
* Constructor.
*
* @param port The SPI port the accelerometer is attached to
* @param range The range (+ or -) that the accelerometer will measure.
*/
explicit ADXL362(SPI::Port port, Range range = kRange_2G);
~ADXL362() override = default;
ADXL362(ADXL362&&) = default;
ADXL362& operator=(ADXL362&&) = default;
SPI::Port GetSpiPort() const;
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the
* accelerometer will measure.
*/
void SetRange(Range range);
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
double GetX();
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
double GetY();
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
double GetZ();
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL362 in Gs.
*/
virtual double GetAcceleration(Axes axis);
/**
* Get the acceleration of all axes in Gs.
*
* @return An object containing the acceleration measured on each axis of the
* ADXL362 in Gs.
*/
virtual AllAxes GetAccelerations();
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
SPI m_spi;
hal::SimDevice m_simDevice;
hal::SimEnum m_simRange;
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
double m_gsPerLSB = 0.001;
};
} // namespace frc

132
wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h
View File

@@ -1,132 +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.
#pragma once
#include <stdint.h>
#include <hal/SimDevice.h>
#include <wpi/sendable/Sendable.h>
#include <wpi/sendable/SendableHelper.h>
#include "frc/SPI.h"
#include "frc/geometry/Rotation2d.h"
namespace frc {
/**
* Use a rate gyro to return the robots heading relative to a starting position.
*
* The %Gyro class tracks the robots heading based on the starting position. As
* the robot rotates the new heading is computed by integrating the rate of
* rotation returned by the sensor. When the class is instantiated, it does a
* short calibration routine where it samples the gyro while at rest to
* determine the default offset. This is subtracted from each sample to
* determine the heading.
*
* This class is for the digital ADXRS450 gyro sensor that connects via SPI.
* Only one instance of an ADXRS %Gyro is supported.
*/
class ADXRS450_Gyro : public wpi::Sendable,
public wpi::SendableHelper<ADXRS450_Gyro> {
public:
/**
* %Gyro constructor on onboard CS0.
*/
ADXRS450_Gyro();
/**
* %Gyro constructor on the specified SPI port.
*
* @param port The SPI port the gyro is attached to.
*/
explicit ADXRS450_Gyro(SPI::Port port);
~ADXRS450_Gyro() override = default;
ADXRS450_Gyro(ADXRS450_Gyro&&) = default;
ADXRS450_Gyro& operator=(ADXRS450_Gyro&&) = default;
bool IsConnected() const;
/**
* Return the actual angle in degrees that the robot is currently facing.
*
* The angle is based on integration of the returned rate from the gyro.
* The angle is continuous, that is it will continue from 360->361 degrees.
* This allows algorithms that wouldn't want to see a discontinuity in the
* gyro output as it sweeps from 360 to 0 on the second time around.
*
* @return the current heading of the robot in degrees.
*/
double GetAngle() const;
/**
* Return the rate of rotation of the gyro
*
* The rate is based on the most recent reading of the gyro.
*
* @return the current rate in degrees per second
*/
double GetRate() const;
/**
* Reset the gyro.
*
* Resets the gyro to a heading of zero. This can be used if there is
* significant drift in the gyro and it needs to be recalibrated after it has
* been running.
*/
void Reset();
/**
* Calibrate the gyro by running for a number of samples and computing the
* center value. Then use the center value as the Accumulator center value for
* subsequent measurements.
*
* It's important to make sure that the robot is not moving while the
* centering calculations are in progress, this is typically done when the
* robot is first turned on while it's sitting at rest before the competition
* starts.
*/
void Calibrate();
/**
* Return the heading of the robot as a Rotation2d.
*
* The angle is continuous, that is it will continue from 360 to 361 degrees.
* This allows algorithms that wouldn't want to see a discontinuity in the
* gyro output as it sweeps past from 360 to 0 on the second time around.
*
* The angle is expected to increase as the gyro turns counterclockwise when
* looked at from the top. It needs to follow the NWU axis convention.
*
* @return the current heading of the robot as a Rotation2d. This heading is
* based on integration of the returned rate from the gyro.
*/
Rotation2d GetRotation2d() const;
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
int GetPort() const;
void InitSendable(wpi::SendableBuilder& builder) override;
private:
SPI m_spi;
SPI::Port m_port;
bool m_connected{false};
hal::SimDevice m_simDevice;
hal::SimBoolean m_simConnected;
hal::SimDouble m_simAngle;
hal::SimDouble m_simRate;
uint16_t ReadRegister(int reg);
};
} // namespace frc

370
wpilibc/src/main/native/include/frc/SPI.h
View File

@@ -1,370 +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.
#pragma once
#include <stdint.h>
#include <memory>
#include <span>
#include <hal/SPI.h>
#include <hal/SPITypes.h>
#include <units/time.h>
namespace frc {
class DigitalSource;
/**
* SPI bus interface class.
*
* This class is intended to be used by sensor (and other SPI device) drivers.
* It probably should not be used directly.
*
*/
class SPI {
public:
/**
* SPI port.
*/
enum Port {
/// Onboard SPI bus port CS0.
kOnboardCS0 = 0,
/// Onboard SPI bus port CS1.
kOnboardCS1,
/// Onboard SPI bus port CS2.
kOnboardCS2,
/// Onboard SPI bus port CS3.
kOnboardCS3,
/// MXP (roboRIO MXP) SPI bus port.
kMXP
};
/**
* SPI mode.
*/
enum Mode {
/// Clock idle low, data sampled on rising edge.
kMode0 = HAL_SPI_kMode0,
/// Clock idle low, data sampled on falling edge.
kMode1 = HAL_SPI_kMode1,
/// Clock idle high, data sampled on falling edge.
kMode2 = HAL_SPI_kMode2,
/// Clock idle high, data sampled on rising edge.
kMode3 = HAL_SPI_kMode3
};
/**
* Constructor
*
* @param port the physical SPI port
*/
explicit SPI(Port port);
SPI(SPI&&) = default;
SPI& operator=(SPI&&) = default;
~SPI();
/**
* Returns the SPI port.
*
* @return The SPI port.
*/
Port GetPort() const;
/**
* Configure the rate of the generated clock signal.
*
* The default value is 500,000Hz.
* The maximum value is 4,000,000Hz.
*
* @param hz The clock rate in Hertz.
*/
void SetClockRate(int hz);
/**
* Sets the mode for the SPI device.
*
* <p>Mode 0 is Clock idle low, data sampled on rising edge
*
* <p>Mode 1 is Clock idle low, data sampled on falling edge
*
* <p>Mode 2 is Clock idle high, data sampled on falling edge
*
* <p>Mode 3 is Clock idle high, data sampled on rising edge
*
* @param mode The mode to set.
*/
void SetMode(Mode mode);
/**
* Configure the chip select line to be active high.
*/
void SetChipSelectActiveHigh();
/**
* Configure the chip select line to be active low.
*/
void SetChipSelectActiveLow();
/**
* Write data to the peripheral device. Blocks until there is space in the
* output FIFO.
*
* If not running in output only mode, also saves the data received
* on the CIPO input during the transfer into the receive FIFO.
*/
int Write(uint8_t* data, int size);
/**
* Read a word from the receive FIFO.
*
* Waits for the current transfer to complete if the receive FIFO is empty.
*
* If the receive FIFO is empty, there is no active transfer, and initiate
* is false, errors.
*
* @param initiate If true, this function pushes "0" into the transmit
* buffer and initiates a transfer. If false, this
* function assumes that data is already in the receive
* FIFO from a previous write.
* @param dataReceived Buffer to receive data from the device
* @param size The length of the transaction, in bytes
*/
int Read(bool initiate, uint8_t* dataReceived, int size);
/**
* Perform a simultaneous read/write transaction with the device
*
* @param dataToSend The data to be written out to the device
* @param dataReceived Buffer to receive data from the device
* @param size The length of the transaction, in bytes
*/
int Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size);
/**
* Initialize automatic SPI transfer engine.
*
* Only a single engine is available, and use of it blocks use of all other
* chip select usage on the same physical SPI port while it is running.
*
* @param bufferSize buffer size in bytes
*/
void InitAuto(int bufferSize);
/**
* Frees the automatic SPI transfer engine.
*/
void FreeAuto();
/**
* Set the data to be transmitted by the engine.
*
* Up to 16 bytes are configurable, and may be followed by up to 127 zero
* bytes.
*
* @param dataToSend data to send (maximum 16 bytes)
* @param zeroSize number of zeros to send after the data
*/
void SetAutoTransmitData(std::span<const uint8_t> dataToSend, int zeroSize);
/**
* Start running the automatic SPI transfer engine at a periodic rate.
*
* InitAuto() and SetAutoTransmitData() must be called before calling this
* function.
*
* @param period period between transfers (us resolution)
*/
void StartAutoRate(units::second_t period);
/**
* Start running the automatic SPI transfer engine when a trigger occurs.
*
* InitAuto() and SetAutoTransmitData() must be called before calling this
* function.
*
* @param source digital source for the trigger (may be an analog trigger)
* @param rising trigger on the rising edge
* @param falling trigger on the falling edge
*/
void StartAutoTrigger(DigitalSource& source, bool rising, bool falling);
/**
* Stop running the automatic SPI transfer engine.
*/
void StopAuto();
/**
* Force the engine to make a single transfer.
*/
void ForceAutoRead();
/**
* Read data that has been transferred by the automatic SPI transfer engine.
*
* Transfers may be made a byte at a time, so it's necessary for the caller
* to handle cases where an entire transfer has not been completed.
*
* Each received data sequence consists of a timestamp followed by the
* received data bytes, one byte per word (in the least significant byte).
* The length of each received data sequence is the same as the combined
* size of the data and zeroSize set in SetAutoTransmitData().
*
* Blocks until numToRead words have been read or timeout expires.
* May be called with numToRead=0 to retrieve how many words are available.
*
* @param buffer buffer where read words are stored
* @param numToRead number of words to read
* @param timeout timeout (ms resolution)
* @return Number of words remaining to be read
*/
int ReadAutoReceivedData(uint32_t* buffer, int numToRead,
units::second_t timeout);
/**
* Get the number of bytes dropped by the automatic SPI transfer engine due
* to the receive buffer being full.
*
* @return Number of bytes dropped
*/
int GetAutoDroppedCount();
/**
* Configure the Auto SPI Stall time between reads.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP.
* @param csToSclkTicks the number of ticks to wait before asserting the cs
* pin
* @param stallTicks the number of ticks to stall for
* @param pow2BytesPerRead the number of bytes to read before stalling
*/
void ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks, int stallTicks,
int pow2BytesPerRead);
/**
* Initialize the accumulator.
*
* @param period Time between reads
* @param cmd SPI command to send to request data
* @param xferSize SPI transfer size, in bytes
* @param validMask Mask to apply to received data for validity checking
* @param validValue After valid_mask is applied, required matching value for
* validity checking
* @param dataShift Bit shift to apply to received data to get actual data
* value
* @param dataSize Size (in bits) of data field
* @param isSigned Is data field signed?
* @param bigEndian Is device big endian?
*/
void InitAccumulator(units::second_t period, int cmd, int xferSize,
int validMask, int validValue, int dataShift,
int dataSize, bool isSigned, bool bigEndian);
/**
* Frees the accumulator.
*/
void FreeAccumulator();
/**
* Resets the accumulator to zero.
*/
void ResetAccumulator();
/**
* Set the center value of the accumulator.
*
* The center value is subtracted from each value before it is added to the
* accumulator. This is used for the center value of devices like gyros and
* accelerometers to make integration work and to take the device offset into
* account when integrating.
*/
void SetAccumulatorCenter(int center);
/**
* Set the accumulator's deadband.
*/
void SetAccumulatorDeadband(int deadband);
/**
* Read the last value read by the accumulator engine.
*/
int GetAccumulatorLastValue() const;
/**
* Read the accumulated value.
*
* @return The 64-bit value accumulated since the last Reset().
*/
int64_t GetAccumulatorValue() const;
/**
* Read the number of accumulated values.
*
* Read the count of the accumulated values since the accumulator was last
* Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
int64_t GetAccumulatorCount() const;
/**
* Read the average of the accumulated value.
*
* @return The accumulated average value (value / count).
*/
double GetAccumulatorAverage() const;
/**
* Read the accumulated value and the number of accumulated values atomically.
*
* This function reads the value and count atomically.
* This can be used for averaging.
*
* @param value Pointer to the 64-bit accumulated output.
* @param count Pointer to the number of accumulation cycles.
*/
void GetAccumulatorOutput(int64_t& value, int64_t& count) const;
/**
* Set the center value of the accumulator integrator.
*
* The center value is subtracted from each value*dt before it is added to the
* integrated value. This is used for the center value of devices like gyros
* and accelerometers to take the device offset into account when integrating.
*/
void SetAccumulatorIntegratedCenter(double center);
/**
* Read the integrated value. This is the sum of (each value * time between
* values).
*
* @return The integrated value accumulated since the last Reset().
*/
double GetAccumulatorIntegratedValue() const;
/**
* Read the average of the integrated value. This is the sum of (each value
* times the time between values), divided by the count.
*
* @return The average of the integrated value accumulated since the last
* Reset().
*/
double GetAccumulatorIntegratedAverage() const;
protected:
hal::Handle<HAL_SPIPort, HAL_CloseSPI, HAL_SPI_kInvalid> m_port;
HAL_SPIMode m_mode = HAL_SPIMode::HAL_SPI_kMode0;
private:
void Init();
class Accumulator;
std::unique_ptr<Accumulator> m_accum;
};
} // namespace frc

106
wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h
View File

@@ -1,106 +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.
#pragma once
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
namespace frc {
class ADIS16448_IMU;
namespace sim {
/**
* Class to control a simulated ADIS16448 IMU.
*/
class ADIS16448_IMUSim {
public:
/**
* Constructs from a ADIS16448_IMU object.
*
* @param imu ADIS16448_IMU to simulate
*/
explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu);
/**
* Sets the X axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleX(units::degree_t angle);
/**
* Sets the Y axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Sets the Z axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t angularRate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t angularRate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t angularRate);
/**
* Sets the X axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelX(units::meters_per_second_squared_t accel);
/**
* Sets the Y axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelY(units::meters_per_second_squared_t accel);
/**
* Sets the Z axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelZ(units::meters_per_second_squared_t accel);
private:
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;
};
} // namespace sim
} // namespace frc

106
wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h
View File

@@ -1,106 +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.
#pragma once
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
namespace frc {
class ADIS16470_IMU;
namespace sim {
/**
* Class to control a simulated ADIS16470 IMU.
*/
class ADIS16470_IMUSim {
public:
/**
* Constructs from a ADIS16470_IMU object.
*
* @param imu ADIS16470_IMU to simulate
*/
explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu);
/**
* Sets the X axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleX(units::degree_t angle);
/**
* Sets the Y axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Sets the Z axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t angularRate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t angularRate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param angularRate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t angularRate);
/**
* Sets the X axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelX(units::meters_per_second_squared_t accel);
/**
* Sets the Y axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelY(units::meters_per_second_squared_t accel);
/**
* Sets the Z axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelZ(units::meters_per_second_squared_t accel);
private:
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;
};
} // namespace sim
} // namespace frc

8
wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h
View File

@@ -8,7 +8,6 @@
namespace frc {
class ADXL345_SPI;
class ADXL345_I2C;
namespace sim {
@@ -25,13 +24,6 @@ class ADXL345Sim {
*/
explicit ADXL345Sim(const ADXL345_I2C& accel);
/**
* Constructs from a ADXL345_SPI object.
*
* @param accel ADXL345 accel to simulate
*/
explicit ADXL345Sim(const ADXL345_SPI& accel);
/**
* Sets the X acceleration.
*

55
wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h
View File

@@ -1,55 +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.
#pragma once
#include <hal/SimDevice.h>
namespace frc {
class ADXL362;
namespace sim {
/**
* Class to control a simulated ADXL362.
*/
class ADXL362Sim {
public:
/**
* Constructs from a ADXL362 object.
*
* @param accel ADXL362 accel to simulate
*/
explicit ADXL362Sim(const ADXL362& accel);
/**
* Sets the X acceleration.
*
* @param accel The X acceleration.
*/
void SetX(double accel);
/**
* Sets the Y acceleration.
*
* @param accel The Y acceleration.
*/
void SetY(double accel);
/**
* Sets the Z acceleration.
*
* @param accel The Z acceleration.
*/
void SetZ(double accel);
private:
hal::SimDouble m_simX;
hal::SimDouble m_simY;
hal::SimDouble m_simZ;
};
} // namespace sim
} // namespace frc

51
wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h
View File

@@ -1,51 +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.
#pragma once
#include <hal/SimDevice.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
#include "frc/geometry/Rotation2d.h"
namespace frc {
class ADXRS450_Gyro;
namespace sim {
/**
* Class to control a simulated ADXRS450 gyroscope.
*/
class ADXRS450_GyroSim {
public:
/**
* Constructs from a ADXRS450_Gyro object.
*
* @param gyro ADXRS450_Gyro to simulate
*/
explicit ADXRS450_GyroSim(const ADXRS450_Gyro& gyro);
/**
* Sets the angle.
*
* @param angle The angle (clockwise positive).
*/
void SetAngle(units::degree_t angle);
/**
* Sets the angular rate (clockwise positive).
*
* @param rate The angular rate.
*/
void SetRate(units::degrees_per_second_t rate);
private:
hal::SimDouble m_simAngle;
hal::SimDouble m_simRate;
};
} // namespace sim
} // namespace frc

154
wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h
View File

@@ -1,154 +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.
#pragma once
#include <memory>
#include "frc/simulation/CallbackStore.h"
namespace frc::sim {
class SPIAccelerometerSim {
public:
/**
* Construct a new simulation object.
*
* @param index the HAL index of the accelerometer
*/
explicit SPIAccelerometerSim(int index);
/**
* Register a callback to be run when this accelerometer activates.
*
* @param callback the callback
* @param initialNotify whether to run the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
std::unique_ptr<CallbackStore> RegisterActiveCallback(NotifyCallback callback,
bool initialNotify);
/**
* Check whether the accelerometer is active.
*
* @return true if active
*/
bool GetActive() const;
/**
* Define whether this accelerometer is active.
*
* @param active the new state
*/
void SetActive(bool active);
/**
* Register a callback to be run whenever the range changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
std::unique_ptr<CallbackStore> RegisterRangeCallback(NotifyCallback callback,
bool initialNotify);
/**
* Check the range of this accelerometer.
*
* @return the accelerometer range
*/
int GetRange() const;
/**
* Change the range of this accelerometer.
*
* @param range the new accelerometer range
*/
void SetRange(int range);
/**
* Register a callback to be run whenever the X axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
std::unique_ptr<CallbackStore> RegisterXCallback(NotifyCallback callback,
bool initialNotify);
/**
* Measure the X axis value.
*
* @return the X axis measurement
*/
double GetX() const;
/**
* Change the X axis value of the accelerometer.
*
* @param x the new reading of the X axis
*/
void SetX(double x);
/**
* Register a callback to be run whenever the Y axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
std::unique_ptr<CallbackStore> RegisterYCallback(NotifyCallback callback,
bool initialNotify);
/**
* Measure the Y axis value.
*
* @return the Y axis measurement
*/
double GetY() const;
/**
* Change the Y axis value of the accelerometer.
*
* @param y the new reading of the Y axis
*/
void SetY(double y);
/**
* Register a callback to be run whenever the Z axis value changes.
*
* @param callback the callback
* @param initialNotify whether to call the callback with the initial state
* @return the CallbackStore object associated with this callback
*/
[[nodiscard]]
std::unique_ptr<CallbackStore> RegisterZCallback(NotifyCallback callback,
bool initialNotify);
/**
* Measure the Z axis value.
*
* @return the Z axis measurement
*/
double GetZ() const;
/**
* Change the Z axis value of the accelerometer.
*
* @param z the new reading of the Z axis
*/
void SetZ(double z);
/**
* Reset all simulation data of this object.
*/
void ResetData();
private:
int m_index;
};
} // namespace frc::sim