Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-30 02:31: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

943
wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp
View File

@@ -1,943 +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 */
/*----------------------------------------------------------------------------*/
#include "frc/ADIS16448_IMU.h"
#include <algorithm>
#include <cmath>
#include <numbers>
#include <utility>
#include <hal/HAL.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/DigitalInput.h"
#include "frc/DigitalOutput.h"
#include "frc/Errors.h"
#include "frc/MathUtil.h"
#include "frc/SPI.h"
#include "frc/Timer.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 int16_t BuffToShort(const uint32_t* buf) {
return (static_cast<int16_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_format_args(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_connected =
m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
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* reset_out = new DigitalOutput(18); // Drive MXP DIO8 low
Wait(10_ms);
delete reset_out;
m_reset_in = new DigitalInput(18); // Set MXP DIO8 high
Wait(500_ms); // Wait for reset to complete
ConfigCalTime(cal_time);
if (!SwitchToStandardSPI()) {
return;
}
bool needsFlash = false;
// Set IMU internal decimation to 1 (output data rate of 819.2 SPS / (1 + 1)
// = 409.6Hz), output bandwidth = 204.8Hz
if (ReadRegister(SMPL_PRD) != 0x0001) {
WriteRegister(SMPL_PRD, 0x0001);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling "
"flash update.");
}
// Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP)
if (ReadRegister(MSC_CTRL) != 0x0016) {
WriteRegister(MSC_CTRL, 0x0016);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling "
"flash update.");
}
// Disable IMU internal Bartlett filter (204Hz bandwidth is sufficient) and
// set IMU scale factor (range)
if (ReadRegister(SENS_AVG) != 0x0400) {
WriteRegister(SENS_AVG, 0x0400);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: SENS_AVG register configuration inconsistent! Scheduling "
"flash update.");
}
// Clear offset registers
if (ReadRegister(XGYRO_OFF) != 0x0000) {
WriteRegister(XGYRO_OFF, 0x0000);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: XGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
}
if (ReadRegister(YGYRO_OFF) != 0x0000) {
WriteRegister(YGYRO_OFF, 0x0000);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: YGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
}
if (ReadRegister(ZGYRO_OFF) != 0x0000) {
WriteRegister(ZGYRO_OFF, 0x0000);
needsFlash = true;
REPORT_WARNING(
"ADIS16448: ZGYRO_OFF register configuration inconsistent! "
"Scheduling flash update.");
}
// If any registers on the IMU don't match the config, trigger a flash
// update
if (needsFlash) {
REPORT_WARNING(
"ADIS16448: Register configuration changed! Starting IMU flash "
"update.");
WriteRegister(GLOB_CMD, 0x0008);
// Wait long enough for the flash update to finish (75ms minimum as per
// the datasheet)
Wait(0.5_s);
REPORT_WARNING("ADIS16448: Flash update finished!");
} else {
REPORT_WARNING(
"ADIS16448: Flash and RAM configuration consistent. No flash update "
"required!");
}
if (!SwitchToAutoSPI()) {
return;
}
// Notify DS that IMU calibration delay is active
REPORT_WARNING("ADIS16448: 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 initialized 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)
m_status_led = new DigitalOutput(19);
}
// Report usage and post data to DS
HAL_Report(HALUsageReporting::kResourceType_ADIS16448, 0);
wpi::SendableRegistry::AddLW(this, "ADIS16448", port);
m_connected = true;
}
ADIS16448_IMU::ADIS16448_IMU(ADIS16448_IMU&& other)
: m_reset_in{std::move(other.m_reset_in)},
m_status_led{std::move(other.m_status_led)},
m_yaw_axis{std::move(other.m_yaw_axis)},
m_gyro_rate_x{std::move(other.m_gyro_rate_x)},
m_gyro_rate_y{std::move(other.m_gyro_rate_y)},
m_gyro_rate_z{std::move(other.m_gyro_rate_z)},
m_accel_x{std::move(other.m_accel_x)},
m_accel_y{std::move(other.m_accel_y)},
m_accel_z{std::move(other.m_accel_z)},
m_mag_x{std::move(other.m_mag_x)},
m_mag_y{std::move(other.m_mag_y)},
m_mag_z{std::move(other.m_mag_z)},
m_baro{std::move(other.m_baro)},
m_temp{std::move(other.m_temp)},
m_dt{std::move(other.m_dt)},
m_alpha{std::move(other.m_alpha)},
m_compAngleX{std::move(other.m_compAngleX)},
m_compAngleY{std::move(other.m_compAngleY)},
m_accelAngleX{std::move(other.m_accelAngleX)},
m_accelAngleY{std::move(other.m_accelAngleY)},
m_offset_buffer{other.m_offset_buffer},
m_gyro_rate_offset_x{std::move(other.m_gyro_rate_offset_x)},
m_gyro_rate_offset_y{std::move(other.m_gyro_rate_offset_y)},
m_gyro_rate_offset_z{std::move(other.m_gyro_rate_offset_z)},
m_avg_size{std::move(other.m_avg_size)},
m_accum_count{std::move(other.m_accum_count)},
m_integ_gyro_angle_x{std::move(other.m_integ_gyro_angle_x)},
m_integ_gyro_angle_y{std::move(other.m_integ_gyro_angle_y)},
m_integ_gyro_angle_z{std::move(other.m_integ_gyro_angle_z)},
m_thread_active{other.m_thread_active.load()},
m_first_run{other.m_first_run.load()},
m_thread_idle{other.m_thread_idle.load()},
m_start_up_mode{other.m_start_up_mode.load()},
m_auto_configured{std::move(other.m_auto_configured)},
m_spi_port{std::move(other.m_spi_port)},
m_calibration_time{std::move(other.m_calibration_time)},
m_spi{std::move(other.m_spi)},
m_auto_interrupt{std::move(other.m_auto_interrupt)},
m_connected{std::move(other.m_connected)},
m_acquire_task{std::move(other.m_acquire_task)},
m_simDevice{std::move(other.m_simDevice)},
m_simConnected{std::move(other.m_simConnected)},
m_simGyroAngleX{std::move(other.m_simGyroAngleX)},
m_simGyroAngleY{std::move(other.m_simGyroAngleZ)},
m_simGyroAngleZ{std::move(other.m_simGyroAngleZ)},
m_simGyroRateX{std::move(other.m_simGyroRateX)},
m_simGyroRateY{std::move(other.m_simGyroRateY)},
m_simGyroRateZ{std::move(other.m_simGyroRateZ)},
m_simAccelX{std::move(other.m_simAccelX)},
m_simAccelY{std::move(other.m_simAccelY)},
m_simAccelZ{std::move(other.m_simAccelZ)},
m_mutex{std::move(other.m_mutex)} {}
ADIS16448_IMU& ADIS16448_IMU::operator=(ADIS16448_IMU&& other) {
if (this == &other) {
return *this;
}
std::swap(this->m_reset_in, other.m_reset_in);
std::swap(this->m_status_led, other.m_status_led);
std::swap(this->m_yaw_axis, other.m_yaw_axis);
std::swap(this->m_gyro_rate_x, other.m_gyro_rate_x);
std::swap(this->m_gyro_rate_y, other.m_gyro_rate_y);
std::swap(this->m_gyro_rate_z, other.m_gyro_rate_z);
std::swap(this->m_accel_x, other.m_accel_x);
std::swap(this->m_accel_y, other.m_accel_y);
std::swap(this->m_accel_z, other.m_accel_z);
std::swap(this->m_mag_x, other.m_mag_x);
std::swap(this->m_mag_y, other.m_mag_y);
std::swap(this->m_mag_z, other.m_mag_z);
std::swap(this->m_baro, other.m_baro);
std::swap(this->m_temp, other.m_temp);
std::swap(this->m_dt, other.m_dt);
std::swap(this->m_alpha, other.m_alpha);
std::swap(this->m_compAngleX, other.m_compAngleX);
std::swap(this->m_compAngleY, other.m_compAngleY);
std::swap(this->m_accelAngleX, other.m_accelAngleX);
std::swap(this->m_accelAngleY, other.m_accelAngleY);
std::swap(this->m_offset_buffer, other.m_offset_buffer);
std::swap(this->m_gyro_rate_offset_x, other.m_gyro_rate_offset_x);
std::swap(this->m_gyro_rate_offset_y, other.m_gyro_rate_offset_y);
std::swap(this->m_gyro_rate_offset_z, other.m_gyro_rate_offset_z);
std::swap(this->m_avg_size, other.m_avg_size);
std::swap(this->m_accum_count, other.m_accum_count);
std::swap(this->m_integ_gyro_angle_x, other.m_integ_gyro_angle_x);
std::swap(this->m_integ_gyro_angle_y, other.m_integ_gyro_angle_y);
std::swap(this->m_integ_gyro_angle_z, other.m_integ_gyro_angle_z);
this->m_thread_active = other.m_thread_active.load();
this->m_first_run = other.m_first_run.load();
this->m_thread_idle = other.m_thread_idle.load();
this->m_start_up_mode = other.m_start_up_mode.load();
std::swap(this->m_auto_configured, other.m_auto_configured);
std::swap(this->m_spi_port, other.m_spi_port);
std::swap(this->m_calibration_time, other.m_calibration_time);
std::swap(this->m_spi, other.m_spi);
std::swap(this->m_auto_interrupt, other.m_auto_interrupt);
std::swap(this->m_connected, other.m_connected);
std::swap(this->m_acquire_task, other.m_acquire_task);
std::swap(this->m_simDevice, other.m_simDevice);
std::swap(this->m_simConnected, other.m_simConnected);
std::swap(this->m_simGyroAngleX, other.m_simGyroAngleX);
std::swap(this->m_simGyroAngleY, other.m_simGyroAngleY);
std::swap(this->m_simGyroAngleZ, other.m_simGyroAngleZ);
std::swap(this->m_simGyroRateX, other.m_simGyroRateX);
std::swap(this->m_simGyroRateY, other.m_simGyroRateY);
std::swap(this->m_simGyroRateZ, other.m_simGyroRateZ);
std::swap(this->m_simAccelX, other.m_simAccelX);
std::swap(this->m_simAccelY, other.m_simAccelY);
std::swap(this->m_simAccelZ, other.m_simAccelZ);
std::swap(this->m_mutex, other.m_mutex);
return *this;
}
bool ADIS16448_IMU::IsConnected() const {
if (m_simConnected) {
return m_simConnected.Get();
}
return m_connected;
}
/**
* @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);
}
}
}
if (m_spi == nullptr) {
m_spi = new SPI(m_spi_port);
m_spi->SetClockRate(1000000);
m_spi->SetMode(frc::SPI::Mode::kMode3);
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;
}
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 && !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 configuration 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;
}
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 (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
/**
* 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 unsigned, 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);
}
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_reset_in != nullptr) {
delete m_reset_in;
m_reset_in = nullptr;
}
if (m_status_led != nullptr) {
delete m_status_led;
m_status_led = nullptr;
}
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];
uint32_t previous_timestamp = 0;
double compAngleX = 0.0;
double compAngleY = 0.0;
while (true) {
// Wait for data
Wait(10_ms);
if (m_thread_active) {
// Read number of bytes currently stored in the buffer
int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
// 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);
}
// Read data from DMA buffer
m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
// 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
// Cycle through XYZ GYRO, XYZ ACCEL, XYZ MAG, BARO, TEMP (Ignore Status
// & CRC)
for (int k = 5; k < 27; k += 2) {
// Process LSB
uint8_t byte = static_cast<uint8_t>(buffer[i + k + 1]);
calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte];
// Process MSB
byte = static_cast<uint8_t>(buffer[i + k]);
calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte];
}
// Complement
calc_crc = ~calc_crc;
// Flip LSB & MSB
calc_crc = static_cast<uint16_t>((calc_crc << 8) | (calc_crc >> 8));
// Extract DUT CRC from data buffer
uint16_t imu_crc = BuffToUShort(&buffer[i + 27]);
// 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;
// Scale sensor data
double gyro_rate_x = BuffToShort(&buffer[i + 5]) * 0.04;
double gyro_rate_y = BuffToShort(&buffer[i + 7]) * 0.04;
double gyro_rate_z = BuffToShort(&buffer[i + 9]) * 0.04;
double accel_x = BuffToShort(&buffer[i + 11]) * 0.833;
double accel_y = BuffToShort(&buffer[i + 13]) * 0.833;
double accel_z = BuffToShort(&buffer[i + 15]) * 0.833;
double mag_x = BuffToShort(&buffer[i + 17]) * 0.1429;
double mag_y = BuffToShort(&buffer[i + 19]) * 0.1429;
double mag_z = BuffToShort(&buffer[i + 21]) * 0.1429;
double baro = BuffToShort(&buffer[i + 23]) * 0.02;
double temp = BuffToShort(&buffer[i + 25]) * 0.07386 + 31.0;
// Convert scaled sensor data to SI units
double gyro_rate_x_si = gyro_rate_x * kDegToRad;
double gyro_rate_y_si = gyro_rate_y * kDegToRad;
// double gyro_rate_z_si = gyro_rate_z * kDegToRad;
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
// Calculate alpha for use with the complementary filter
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
atan2f(-accel_x_si, std::hypotf(accel_y_si, -accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(-accel_x_si, -accel_z_si));
// Calculate complementary filter
if (m_first_run) {
compAngleX = accelAngleX;
compAngleY = accelAngleY;
} else {
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
int 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 * kRadToDeg;
m_compAngleY = compAngleY * kRadToDeg;
m_accelAngleX = accelAngleX * kRadToDeg;
m_accelAngleY = accelAngleY * kRadToDeg;
// 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;
previous_timestamp = 0.0;
compAngleX = 0.0;
compAngleY = 0.0;
}
}
}
/* Complementary filter functions */
double ADIS16448_IMU::FormatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
} else if (accAngle > compAngle + std::numbers::pi) {
compAngle = compAngle + 2.0 * std::numbers::pi;
}
return compAngle;
}
double ADIS16448_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = std::numbers::pi - accelAngle;
} else if (accelZ > 0.0 && accelAngle < 0.0) {
accelAngle = 2.0 * std::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;
return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
}
int ADIS16448_IMU::ConfigDecRate(uint16_t decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, adjusts the sample scale factor, and
// re-enables auto SPI.
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
// Check max
if (decimationRate > 9) {
REPORT_ERROR(
"Attempted to write an invalid decimation value. Capping at 9");
decimationRate = 9;
}
// Shift decimation setting to correct position and select internal sync
uint16_t writeValue = (decimationRate << 8) | 0x1;
// Apply to IMU
WriteRegister(SMPL_PRD, writeValue);
// Perform read back to verify write
uint16_t 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;
}
void ADIS16448_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16448 IMU");
builder.AddDoubleProperty(
"Yaw Angle", [=, this] { return GetAngle().value(); }, nullptr);
}

944
wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp
View File

@@ -1,944 +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 */
/*----------------------------------------------------------------------------*/
#include "frc/ADIS16470_IMU.h"
#include <cmath>
#include <numbers>
#include <utility>
#include <hal/HAL.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/DigitalInput.h"
#include "frc/Errors.h"
#include "frc/MathUtil.h"
#include "frc/Timer.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];
}
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_format_args(args...));
}
} // namespace
#define REPORT_WARNING(msg) \
ADISReportError(warn::Warning, __FILE__, __LINE__, __FUNCTION__, msg);
#define REPORT_ERROR(msg) \
ADISReportError(err::Error, __FILE__, __LINE__, __FUNCTION__, msg)
ADIS16470_IMU::ADIS16470_IMU()
: ADIS16470_IMU(kZ, kY, kX, SPI::Port::kOnboardCS0, CalibrationTime::_1s) {}
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis)
: ADIS16470_IMU(yaw_axis, pitch_axis, roll_axis, SPI::Port::kOnboardCS0,
CalibrationTime::_1s) {}
ADIS16470_IMU::ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis,
IMUAxis roll_axis, SPI::Port port,
CalibrationTime cal_time)
: m_yaw_axis(yaw_axis),
m_pitch_axis(pitch_axis),
m_roll_axis(roll_axis),
m_spi_port(port),
m_calibration_time(static_cast<uint16_t>(cal_time)),
m_simDevice("Gyro:ADIS16470", port) {
if (yaw_axis == kYaw || yaw_axis == kPitch || yaw_axis == kRoll ||
pitch_axis == kYaw || pitch_axis == kPitch || pitch_axis == kRoll ||
roll_axis == kYaw || roll_axis == kPitch || roll_axis == kRoll) {
REPORT_ERROR(
"ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or "
"IMUAxis.kZ as arguments.");
REPORT_ERROR(
"Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)");
yaw_axis = kZ;
pitch_axis = kY;
roll_axis = kX;
}
if (m_simDevice) {
m_connected =
m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
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* reset_out =
new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low
Wait(10_ms);
delete reset_out;
m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high
Wait(500_ms); // Wait for reset to complete
// Configure standard SPI
if (!SwitchToStandardSPI()) {
return;
}
bool needsFlash = false;
// Set IMU internal decimation to 4 (output data rate of 2000 SPS / (4 + 1)
// = 400Hz)
if (ReadRegister(DEC_RATE) != 0x0004) {
WriteRegister(DEC_RATE, 0x0004);
needsFlash = true;
REPORT_WARNING(
"ADIS16470: DEC_RATE register configuration inconsistent! Scheduling "
"flash update.");
}
// Set data ready polarity (HIGH = Good Data), Disable gSense Compensation
// and PoP
if (ReadRegister(MSC_CTRL) != 0x0001) {
WriteRegister(MSC_CTRL, 0x0001);
needsFlash = true;
REPORT_WARNING(
"ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling "
"flash update.");
}
// Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient)
if (ReadRegister(FILT_CTRL) != 0x0000) {
WriteRegister(FILT_CTRL, 0x0000);
needsFlash = true;
REPORT_WARNING(
"ADIS16470: FILT_CTRL register configuration inconsistent! "
"Scheduling flash update.");
}
// If any registers on the IMU don't match the config, trigger a flash
// update
if (needsFlash) {
REPORT_WARNING(
"ADIS16470: Register configuration changed! Starting IMU flash "
"update.");
WriteRegister(GLOB_CMD, 0x0008);
// Wait long enough for the flash update to finish (72ms minimum as per
// the datasheet)
Wait(0.3_s);
REPORT_WARNING("ADIS16470: Flash update finished!");
} else {
REPORT_WARNING(
"ADIS16470: Flash and RAM configuration consistent. No flash update "
"required!");
}
// 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: 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 initialized successfully
REPORT_WARNING("ADIS16470 IMU Successfully Initialized!");
// Drive SPI CS3 (IMU ready LED) low (active low)
m_status_led = new DigitalOutput(28);
}
// Report usage and post data to DS
HAL_Report(HALUsageReporting::kResourceType_ADIS16470, 0);
wpi::SendableRegistry::AddLW(this, "ADIS16470", port);
m_connected = true;
}
ADIS16470_IMU::ADIS16470_IMU(ADIS16470_IMU&& other)
: m_yaw_axis{std::move(other.m_yaw_axis)},
m_pitch_axis{std::move(other.m_pitch_axis)},
m_roll_axis{std::move(other.m_roll_axis)},
m_reset_in{std::move(other.m_reset_in)},
m_status_led{std::move(other.m_status_led)},
m_integ_angle_x{std::move(other.m_integ_angle_x)},
m_integ_angle_y{std::move(other.m_integ_angle_y)},
m_integ_angle_z{std::move(other.m_integ_angle_z)},
m_gyro_rate_x{std::move(other.m_gyro_rate_x)},
m_gyro_rate_y{std::move(other.m_gyro_rate_y)},
m_gyro_rate_z{std::move(other.m_gyro_rate_z)},
m_accel_x{std::move(other.m_accel_x)},
m_accel_y{std::move(other.m_accel_y)},
m_accel_z{std::move(other.m_accel_z)},
m_dt{std::move(other.m_dt)},
m_alpha{std::move(other.m_alpha)},
m_compAngleX{std::move(other.m_compAngleX)},
m_compAngleY{std::move(other.m_compAngleY)},
m_accelAngleX{std::move(other.m_accelAngleX)},
m_accelAngleY{std::move(other.m_accelAngleY)},
m_thread_active{other.m_thread_active.load()},
m_first_run{other.m_first_run.load()},
m_thread_idle{other.m_thread_idle.load()},
m_auto_configured{std::move(other.m_auto_configured)},
m_spi_port{std::move(other.m_spi_port)},
m_calibration_time{std::move(other.m_calibration_time)},
m_spi{std::move(other.m_spi)},
m_auto_interrupt{std::move(other.m_auto_interrupt)},
m_scaled_sample_rate{std::move(other.m_scaled_sample_rate)},
m_connected{std::move(other.m_connected)},
m_acquire_task{std::move(other.m_acquire_task)},
m_simDevice{std::move(other.m_simDevice)},
m_simConnected{std::move(other.m_simConnected)},
m_simGyroAngleX{std::move(other.m_simGyroAngleX)},
m_simGyroAngleY{std::move(other.m_simGyroAngleZ)},
m_simGyroAngleZ{std::move(other.m_simGyroAngleZ)},
m_simGyroRateX{std::move(other.m_simGyroRateX)},
m_simGyroRateY{std::move(other.m_simGyroRateY)},
m_simGyroRateZ{std::move(other.m_simGyroRateZ)},
m_simAccelX{std::move(other.m_simAccelX)},
m_simAccelY{std::move(other.m_simAccelY)},
m_simAccelZ{std::move(other.m_simAccelZ)},
m_mutex{std::move(other.m_mutex)} {}
ADIS16470_IMU& ADIS16470_IMU::operator=(ADIS16470_IMU&& other) {
if (this == &other) {
return *this;
}
std::swap(this->m_yaw_axis, other.m_yaw_axis);
std::swap(this->m_pitch_axis, other.m_pitch_axis);
std::swap(this->m_roll_axis, other.m_roll_axis);
std::swap(this->m_reset_in, other.m_reset_in);
std::swap(this->m_status_led, other.m_status_led);
std::swap(this->m_integ_angle_x, other.m_integ_angle_x);
std::swap(this->m_integ_angle_y, other.m_integ_angle_y);
std::swap(this->m_integ_angle_z, other.m_integ_angle_z);
std::swap(this->m_gyro_rate_x, other.m_gyro_rate_x);
std::swap(this->m_gyro_rate_y, other.m_gyro_rate_y);
std::swap(this->m_gyro_rate_z, other.m_gyro_rate_z);
std::swap(this->m_accel_x, other.m_accel_x);
std::swap(this->m_accel_y, other.m_accel_y);
std::swap(this->m_accel_z, other.m_accel_z);
std::swap(this->m_dt, other.m_dt);
std::swap(this->m_alpha, other.m_alpha);
std::swap(this->m_compAngleX, other.m_compAngleX);
std::swap(this->m_compAngleY, other.m_compAngleY);
std::swap(this->m_accelAngleX, other.m_accelAngleX);
std::swap(this->m_accelAngleY, other.m_accelAngleY);
this->m_thread_active = other.m_thread_active.load();
this->m_first_run = other.m_first_run.load();
this->m_thread_idle = other.m_thread_idle.load();
std::swap(this->m_auto_configured, other.m_auto_configured);
std::swap(this->m_spi_port, other.m_spi_port);
std::swap(this->m_calibration_time, other.m_calibration_time);
std::swap(this->m_spi, other.m_spi);
std::swap(this->m_auto_interrupt, other.m_auto_interrupt);
std::swap(this->m_scaled_sample_rate, other.m_scaled_sample_rate);
std::swap(this->m_connected, other.m_connected);
std::swap(this->m_acquire_task, other.m_acquire_task);
std::swap(this->m_simDevice, other.m_simDevice);
std::swap(this->m_simConnected, other.m_simConnected);
std::swap(this->m_simGyroAngleX, other.m_simGyroAngleX);
std::swap(this->m_simGyroAngleY, other.m_simGyroAngleZ);
std::swap(this->m_simGyroAngleZ, other.m_simGyroAngleZ);
std::swap(this->m_simGyroRateX, other.m_simGyroRateX);
std::swap(this->m_simGyroRateY, other.m_simGyroRateY);
std::swap(this->m_simGyroRateZ, other.m_simGyroRateZ);
std::swap(this->m_simAccelX, other.m_simAccelX);
std::swap(this->m_simAccelY, other.m_simAccelY);
std::swap(this->m_simAccelZ, other.m_simAccelZ);
std::swap(this->m_mutex, other.m_mutex);
return *this;
}
bool ADIS16470_IMU::IsConnected() const {
if (m_simConnected) {
return m_simConnected.Get();
}
return m_connected;
}
/**
* @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 remaining 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->SetMode(frc::SPI::Mode::kMode3);
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;
}
/**
* @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 && !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?
m_spi->SetAutoTransmitData(m_autospi_allangle_packet, 2);
// Configure auto stall time
m_spi->ConfigureAutoStall(HAL_SPI_kOnboardCS0, 5, 1000, 1);
// Kick off DMA SPI (Note: Device configuration 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;
}
int ADIS16470_IMU::ConfigDecRate(uint16_t decimationRate) {
// Switches the active SPI port to standard SPI mode, writes a new value to
// the DECIMATE register in the IMU, adjusts the sample scale factor, and
// re-enables auto SPI.
if (!SwitchToStandardSPI()) {
REPORT_ERROR("Failed to configure/reconfigure standard SPI.");
return 2;
}
if (decimationRate > 1999) {
REPORT_ERROR("Attempted to write an invalid decimation value.");
decimationRate = 1999;
}
m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0;
WriteRegister(DEC_RATE, decimationRate);
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.");
}
}
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 (static_cast<uint16_t>(buf[0]) << 8) | buf[1];
}
/**
* @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 unsigned, 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);
}
void ADIS16470_IMU::Reset() {
std::scoped_lock sync(m_mutex);
m_integ_angle_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
}
void ADIS16470_IMU::Close() {
if (m_reset_in != nullptr) {
delete m_reset_in;
m_reset_in = nullptr;
}
if (m_status_led != nullptr) {
delete m_status_led;
m_status_led = nullptr;
}
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 acquisition 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 = 27; // 26 data points + timestamp
const int BUFFER_SIZE = 4000;
// This buffer can contain many datasets
uint32_t buffer[BUFFER_SIZE];
uint32_t previous_timestamp = 0;
double compAngleX = 0.0;
double compAngleY = 0.0;
while (true) {
// Wait for data
Wait(10_ms);
if (m_thread_active) {
m_thread_idle = false;
// Read number of bytes currently stored in the buffer
int data_count = m_spi->ReadAutoReceivedData(buffer, 0, 0_s);
// Check if frame is incomplete
int data_remainder = data_count % dataset_len;
// Remove incomplete data from read count
int data_to_read = data_count - data_remainder;
// 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);
}
// Read data from DMA buffer (only complete sets)
m_spi->ReadAutoReceivedData(buffer, data_to_read, 0_s);
// 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)
double elapsed_time =
m_scaled_sample_rate / (buffer[i] - previous_timestamp);
double delta_angle_x =
ToInt(&buffer[i + 3]) * delta_angle_sf / elapsed_time;
double delta_angle_y =
ToInt(&buffer[i + 7]) * delta_angle_sf / elapsed_time;
double delta_angle_z =
ToInt(&buffer[i + 11]) * delta_angle_sf / elapsed_time;
double gyro_rate_x = BuffToShort(&buffer[i + 15]) / 10.0;
double gyro_rate_y = BuffToShort(&buffer[i + 17]) / 10.0;
double gyro_rate_z = BuffToShort(&buffer[i + 19]) / 10.0;
double accel_x = BuffToShort(&buffer[i + 21]) / 800.0;
double accel_y = BuffToShort(&buffer[i + 23]) / 800.0;
double accel_z = BuffToShort(&buffer[i + 25]) / 800.0;
// Convert scaled sensor data to SI units
double gyro_rate_x_si = gyro_rate_x * kDegToRad;
double gyro_rate_y_si = gyro_rate_y * kDegToRad;
// double gyro_rate_z_si = gyro_rate_z * kDegToRad;
double accel_x_si = accel_x * kGrav;
double accel_y_si = accel_y * kGrav;
double accel_z_si = accel_z * kGrav;
// Store timestamp for next iteration
previous_timestamp = buffer[i];
m_alpha = kTau / (kTau + m_dt);
// Run inclinometer calculations
double accelAngleX =
atan2f(accel_x_si, std::hypotf(accel_y_si, accel_z_si));
double accelAngleY =
atan2f(accel_y_si, std::hypotf(accel_x_si, accel_z_si));
if (m_first_run) {
compAngleX = accelAngleX;
compAngleY = accelAngleY;
} else {
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_x = 0.0;
m_integ_angle_y = 0.0;
m_integ_angle_z = 0.0;
} else {
m_integ_angle_x += delta_angle_x;
m_integ_angle_y += delta_angle_y;
m_integ_angle_z += delta_angle_z;
}
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 * kRadToDeg;
m_compAngleY = compAngleY * kRadToDeg;
m_accelAngleX = accelAngleX * kRadToDeg;
m_accelAngleY = accelAngleY * kRadToDeg;
}
m_first_run = false;
}
} else {
m_thread_idle = true;
previous_timestamp = 0;
compAngleX = 0.0;
compAngleY = 0.0;
}
}
}
/* Complementary filter functions */
double ADIS16470_IMU::FormatFastConverge(double compAngle, double accAngle) {
if (compAngle > accAngle + std::numbers::pi) {
compAngle = compAngle - 2.0 * std::numbers::pi;
} else if (accAngle > compAngle + std::numbers::pi) {
compAngle = compAngle + 2.0 * std::numbers::pi;
}
return compAngle;
}
double ADIS16470_IMU::FormatAccelRange(double accelAngle, double accelZ) {
if (accelZ < 0.0) {
accelAngle = std::numbers::pi - accelAngle;
} else if (accelZ > 0.0 && accelAngle < 0.0) {
accelAngle = 2.0 * std::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;
return frc::InputModulus(compAngle, -std::numbers::pi, std::numbers::pi);
}
void ADIS16470_IMU::SetGyroAngle(IMUAxis axis, units::degree_t angle) {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case kX:
SetGyroAngleX(angle);
break;
case kY:
SetGyroAngleY(angle);
break;
case kZ:
SetGyroAngleZ(angle);
break;
default:
break;
}
}
void ADIS16470_IMU::SetGyroAngleX(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_x = angle.value();
}
void ADIS16470_IMU::SetGyroAngleY(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_y = angle.value();
}
void ADIS16470_IMU::SetGyroAngleZ(units::degree_t angle) {
std::scoped_lock sync(m_mutex);
m_integ_angle_z = angle.value();
}
units::degree_t ADIS16470_IMU::GetAngle(IMUAxis axis) const {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case kX:
if (m_simGyroAngleX) {
return units::degree_t{m_simGyroAngleX.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_x};
}
case kY:
if (m_simGyroAngleY) {
return units::degree_t{m_simGyroAngleY.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_y};
}
case kZ:
if (m_simGyroAngleZ) {
return units::degree_t{m_simGyroAngleZ.Get()};
}
{
std::scoped_lock sync(m_mutex);
return units::degree_t{m_integ_angle_z};
}
default:
break;
}
return 0_deg;
}
units::degrees_per_second_t ADIS16470_IMU::GetRate(IMUAxis axis) const {
switch (axis) {
case kYaw:
axis = m_yaw_axis;
break;
case kPitch:
axis = m_pitch_axis;
break;
case kRoll:
axis = m_roll_axis;
break;
default:
break;
}
switch (axis) {
case 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};
}
case 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};
}
case 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};
}
default:
break;
}
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;
}
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetPitchAxis() const {
return m_pitch_axis;
}
ADIS16470_IMU::IMUAxis ADIS16470_IMU::GetRollAxis() const {
return m_roll_axis;
}
int ADIS16470_IMU::GetPort() const {
return m_spi_port;
}
void ADIS16470_IMU::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("ADIS16470 IMU");
builder.AddDoubleProperty(
"Yaw Angle", [=, this] { return GetAngle(kYaw).value(); }, nullptr);
}

131
wpilibc/src/main/native/cpp/ADXL345_SPI.cpp
View File

@@ -1,131 +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/ADXL345_SPI.h"
#include <hal/FRCUsageReporting.h>
#include <networktables/DoubleTopic.h>
#include <networktables/NTSendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
using namespace frc;
ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
: m_spi(port), m_simDevice("Accel:ADXL345_SPI", port) {
if (m_simDevice) {
m_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
{"2G", "4G", "8G", "16G"},
{2.0, 4.0, 8.0, 16.0}, 0);
m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
}
m_spi.SetClockRate(500000);
m_spi.SetMode(frc::SPI::Mode::kMode3);
m_spi.SetChipSelectActiveHigh();
uint8_t commands[2];
// Turn on the measurements
commands[0] = kPowerCtlRegister;
commands[1] = kPowerCtl_Measure;
m_spi.Transaction(commands, commands, 2);
SetRange(range);
HAL_Report(HALUsageReporting::kResourceType_ADXL345,
HALUsageReporting::kADXL345_SPI);
wpi::SendableRegistry::AddLW(this, "ADXL345_SPI", port);
}
SPI::Port ADXL345_SPI::GetSpiPort() const {
return m_spi.GetPort();
}
void ADXL345_SPI::SetRange(Range range) {
uint8_t commands[2];
// Specify the data format to read
commands[0] = kDataFormatRegister;
commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
m_spi.Transaction(commands, commands, 2);
if (m_simRange) {
m_simRange.Set(range);
}
}
double ADXL345_SPI::GetX() {
return GetAcceleration(kAxis_X);
}
double ADXL345_SPI::GetY() {
return GetAcceleration(kAxis_Y);
}
double ADXL345_SPI::GetZ() {
return GetAcceleration(kAxis_Z);
}
double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
if (axis == kAxis_X && m_simX) {
return m_simX.Get();
}
if (axis == kAxis_Y && m_simY) {
return m_simY.Get();
}
if (axis == kAxis_Z && m_simZ) {
return m_simZ.Get();
}
uint8_t buffer[3];
uint8_t command[3] = {0, 0, 0};
command[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister) +
static_cast<uint8_t>(axis);
m_spi.Transaction(command, buffer, 3);
// Sensor is little endian... swap bytes
int16_t rawAccel = buffer[2] << 8 | buffer[1];
return rawAccel * kGsPerLSB;
}
ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() {
AllAxes data;
if (m_simX && m_simY && m_simZ) {
data.XAxis = m_simX.Get();
data.YAxis = m_simY.Get();
data.ZAxis = m_simZ.Get();
return data;
}
uint8_t dataBuffer[7] = {0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];
// Select the data address.
dataBuffer[0] = (kAddress_Read | kAddress_MultiByte | kDataRegister);
m_spi.Transaction(dataBuffer, dataBuffer, 7);
for (int i = 0; i < 3; i++) {
// Sensor is little endian... swap bytes
rawData[i] = dataBuffer[i * 2 + 2] << 8 | dataBuffer[i * 2 + 1];
}
data.XAxis = rawData[0] * kGsPerLSB;
data.YAxis = rawData[1] * kGsPerLSB;
data.ZAxis = rawData[2] * kGsPerLSB;
return data;
}
void ADXL345_SPI::InitSendable(nt::NTSendableBuilder& builder) {
builder.SetSmartDashboardType("3AxisAccelerometer");
builder.SetUpdateTable(
[this, x = nt::DoubleTopic{builder.GetTopic("X")}.Publish(),
y = nt::DoubleTopic{builder.GetTopic("Y")}.Publish(),
z = nt::DoubleTopic{builder.GetTopic("Z")}.Publish()]() mutable {
auto data = GetAccelerations();
x.Set(data.XAxis);
y.Set(data.YAxis);
z.Set(data.ZAxis);
});
}

194
wpilibc/src/main/native/cpp/ADXL362.cpp
View File

@@ -1,194 +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/ADXL362.h"
#include <hal/FRCUsageReporting.h>
#include <networktables/DoubleTopic.h>
#include <networktables/NTSendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
using namespace frc;
static constexpr int kRegWrite = 0x0A;
static constexpr int kRegRead = 0x0B;
static constexpr int kPartIdRegister = 0x02;
static constexpr int kDataRegister = 0x0E;
static constexpr int kFilterCtlRegister = 0x2C;
static constexpr int kPowerCtlRegister = 0x2D;
// static constexpr int kFilterCtl_Range2G = 0x00;
// static constexpr int kFilterCtl_Range4G = 0x40;
// static constexpr int kFilterCtl_Range8G = 0x80;
static constexpr int kFilterCtl_ODR_100Hz = 0x03;
static constexpr int kPowerCtl_UltraLowNoise = 0x20;
// static constexpr int kPowerCtl_AutoSleep = 0x04;
static constexpr int kPowerCtl_Measure = 0x02;
ADXL362::ADXL362(Range range) : ADXL362(SPI::Port::kOnboardCS1, range) {}
ADXL362::ADXL362(SPI::Port port, Range range)
: m_spi(port), m_simDevice("Accel:ADXL362", port) {
if (m_simDevice) {
m_simRange = m_simDevice.CreateEnumDouble("range", hal::SimDevice::kOutput,
{"2G", "4G", "8G", "16G"},
{2.0, 4.0, 8.0, 16.0}, 0);
m_simX = m_simDevice.CreateDouble("x", hal::SimDevice::kInput, 0.0);
m_simY = m_simDevice.CreateDouble("y", hal::SimDevice::kInput, 0.0);
m_simZ = m_simDevice.CreateDouble("z", hal::SimDevice::kInput, 0.0);
}
m_spi.SetClockRate(3000000);
m_spi.SetMode(frc::SPI::Mode::kMode3);
m_spi.SetChipSelectActiveLow();
uint8_t commands[3];
if (!m_simDevice) {
// Validate the part ID
commands[0] = kRegRead;
commands[1] = kPartIdRegister;
commands[2] = 0;
m_spi.Transaction(commands, commands, 3);
if (commands[2] != 0xF2) {
FRC_ReportError(err::Error, "could not find ADXL362");
m_gsPerLSB = 0.0;
return;
}
}
SetRange(range);
// Turn on the measurements
commands[0] = kRegWrite;
commands[1] = kPowerCtlRegister;
commands[2] = kPowerCtl_Measure | kPowerCtl_UltraLowNoise;
m_spi.Write(commands, 3);
HAL_Report(HALUsageReporting::kResourceType_ADXL362, port + 1);
wpi::SendableRegistry::AddLW(this, "ADXL362", port);
}
SPI::Port ADXL362::GetSpiPort() const {
return m_spi.GetPort();
}
void ADXL362::SetRange(Range range) {
if (m_gsPerLSB == 0.0) {
return;
}
uint8_t commands[3];
switch (range) {
case kRange_2G:
m_gsPerLSB = 0.001;
break;
case kRange_4G:
m_gsPerLSB = 0.002;
break;
case kRange_8G:
m_gsPerLSB = 0.004;
break;
}
// Specify the data format to read
commands[0] = kRegWrite;
commands[1] = kFilterCtlRegister;
commands[2] =
kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
m_spi.Write(commands, 3);
if (m_simRange) {
m_simRange.Set(range);
}
}
double ADXL362::GetX() {
return GetAcceleration(kAxis_X);
}
double ADXL362::GetY() {
return GetAcceleration(kAxis_Y);
}
double ADXL362::GetZ() {
return GetAcceleration(kAxis_Z);
}
double ADXL362::GetAcceleration(ADXL362::Axes axis) {
if (m_gsPerLSB == 0.0) {
return 0.0;
}
if (axis == kAxis_X && m_simX) {
return m_simX.Get();
}
if (axis == kAxis_Y && m_simY) {
return m_simY.Get();
}
if (axis == kAxis_Z && m_simZ) {
return m_simZ.Get();
}
uint8_t buffer[4];
uint8_t command[4] = {0, 0, 0, 0};
command[0] = kRegRead;
command[1] = kDataRegister + static_cast<uint8_t>(axis);
m_spi.Transaction(command, buffer, 4);
// Sensor is little endian... swap bytes
int16_t rawAccel = buffer[3] << 8 | buffer[2];
return rawAccel * m_gsPerLSB;
}
ADXL362::AllAxes ADXL362::GetAccelerations() {
AllAxes data;
if (m_gsPerLSB == 0.0) {
data.XAxis = data.YAxis = data.ZAxis = 0.0;
return data;
}
if (m_simX && m_simY && m_simZ) {
data.XAxis = m_simX.Get();
data.YAxis = m_simY.Get();
data.ZAxis = m_simZ.Get();
return data;
}
uint8_t dataBuffer[8] = {0, 0, 0, 0, 0, 0, 0, 0};
int16_t rawData[3];
// Select the data address.
dataBuffer[0] = kRegRead;
dataBuffer[1] = kDataRegister;
m_spi.Transaction(dataBuffer, dataBuffer, 8);
for (int i = 0; i < 3; i++) {
// Sensor is little endian... swap bytes
rawData[i] = dataBuffer[i * 2 + 3] << 8 | dataBuffer[i * 2 + 2];
}
data.XAxis = rawData[0] * m_gsPerLSB;
data.YAxis = rawData[1] * m_gsPerLSB;
data.ZAxis = rawData[2] * m_gsPerLSB;
return data;
}
void ADXL362::InitSendable(nt::NTSendableBuilder& builder) {
builder.SetSmartDashboardType("3AxisAccelerometer");
builder.SetUpdateTable(
[this, x = nt::DoubleTopic{builder.GetTopic("X")}.Publish(),
y = nt::DoubleTopic{builder.GetTopic("Y")}.Publish(),
z = nt::DoubleTopic{builder.GetTopic("Z")}.Publish()]() mutable {
auto data = GetAccelerations();
x.Set(data.XAxis);
y.Set(data.YAxis);
z.Set(data.ZAxis);
});
}

154
wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp
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.
#include "frc/ADXRS450_Gyro.h"
#include <hal/FRCUsageReporting.h>
#include <wpi/sendable/SendableBuilder.h>
#include <wpi/sendable/SendableRegistry.h>
#include "frc/Errors.h"
#include "frc/Timer.h"
using namespace frc;
static constexpr auto kSamplePeriod = 0.5_ms;
static constexpr auto kCalibrationSampleTime = 5_s;
static constexpr double kDegreePerSecondPerLSB = 0.0125;
// static constexpr int kRateRegister = 0x00;
// static constexpr int kTemRegister = 0x02;
// static constexpr int kLoCSTRegister = 0x04;
// static constexpr int kHiCSTRegister = 0x06;
// static constexpr int kQuadRegister = 0x08;
// static constexpr int kFaultRegister = 0x0A;
static constexpr int kPIDRegister = 0x0C;
// static constexpr int kSNHighRegister = 0x0E;
// static constexpr int kSNLowRegister = 0x10;
ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
: m_spi(port), m_port(port), m_simDevice("Gyro:ADXRS450", port) {
if (m_simDevice) {
m_simConnected =
m_simDevice.CreateBoolean("connected", hal::SimDevice::kInput, true);
m_simAngle =
m_simDevice.CreateDouble("angle_x", hal::SimDevice::kInput, 0.0);
m_simRate = m_simDevice.CreateDouble("rate_x", hal::SimDevice::kInput, 0.0);
}
m_spi.SetClockRate(3000000);
m_spi.SetMode(frc::SPI::Mode::kMode0);
m_spi.SetChipSelectActiveLow();
if (!m_simDevice) {
// Validate the part ID
if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
FRC_ReportError(err::Error, "could not find ADXRS450 gyro");
return;
}
m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu,
0x04000000u, 10u, 16u, true, true);
Calibrate();
}
HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port + 1);
wpi::SendableRegistry::AddLW(this, "ADXRS450_Gyro", port);
m_connected = true;
}
bool ADXRS450_Gyro::IsConnected() const {
if (m_simConnected) {
return m_simConnected.Get();
}
return m_connected;
}
static bool CalcParity(uint32_t v) {
bool parity = false;
while (v != 0) {
parity = !parity;
v = v & (v - 1);
}
return parity;
}
static inline int BytesToIntBE(uint8_t* buf) {
int result = static_cast<int>(buf[0]) << 24;
result |= static_cast<int>(buf[1]) << 16;
result |= static_cast<int>(buf[2]) << 8;
result |= static_cast<int>(buf[3]);
return result;
}
uint16_t ADXRS450_Gyro::ReadRegister(int reg) {
uint32_t cmd = 0x80000000 | static_cast<int>(reg) << 17;
if (!CalcParity(cmd)) {
cmd |= 1u;
}
// big endian
uint8_t buf[4] = {static_cast<uint8_t>((cmd >> 24) & 0xff),
static_cast<uint8_t>((cmd >> 16) & 0xff),
static_cast<uint8_t>((cmd >> 8) & 0xff),
static_cast<uint8_t>(cmd & 0xff)};
m_spi.Write(buf, 4);
m_spi.Read(false, buf, 4);
if ((buf[0] & 0xe0) == 0) {
return 0; // error, return 0
}
return static_cast<uint16_t>((BytesToIntBE(buf) >> 5) & 0xffff);
}
double ADXRS450_Gyro::GetAngle() const {
if (m_simAngle) {
return m_simAngle.Get();
}
return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
}
double ADXRS450_Gyro::GetRate() const {
if (m_simRate) {
return m_simRate.Get();
}
return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
kDegreePerSecondPerLSB;
}
void ADXRS450_Gyro::Reset() {
if (m_simAngle) {
m_simAngle.Reset();
}
m_spi.ResetAccumulator();
}
void ADXRS450_Gyro::Calibrate() {
Wait(100_ms);
m_spi.SetAccumulatorIntegratedCenter(0);
m_spi.ResetAccumulator();
Wait(kCalibrationSampleTime);
m_spi.SetAccumulatorIntegratedCenter(m_spi.GetAccumulatorIntegratedAverage());
m_spi.ResetAccumulator();
}
Rotation2d ADXRS450_Gyro::GetRotation2d() const {
return units::degree_t{-GetAngle()};
}
int ADXRS450_Gyro::GetPort() const {
return m_port;
}
void ADXRS450_Gyro::InitSendable(wpi::SendableBuilder& builder) {
builder.SetSmartDashboardType("Gyro");
builder.AddDoubleProperty("Value", [=, this] { return GetAngle(); }, nullptr);
}

430
wpilibc/src/main/native/cpp/SPI.cpp
View File

@@ -1,430 +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/SPI.h"
#include <cstring>
#include <memory>
#include <utility>
#include <hal/FRCUsageReporting.h>
#include <hal/SPI.h>
#include <wpi/SmallVector.h>
#include <wpi/mutex.h>
#include "frc/DigitalSource.h"
#include "frc/Errors.h"
#include "frc/Notifier.h"
using namespace frc;
static constexpr int kAccumulateDepth = 2048;
class SPI::Accumulator {
public:
Accumulator(HAL_SPIPort port, int xferSize, int validMask, int validValue,
int dataShift, int dataSize, bool isSigned, bool bigEndian)
: m_notifier([=, this] {
std::scoped_lock lock(m_mutex);
Update();
}),
m_buf(new uint32_t[(xferSize + 1) * kAccumulateDepth]),
m_validMask(validMask),
m_validValue(validValue),
m_dataMax(1 << dataSize),
m_dataMsbMask(1 << (dataSize - 1)),
m_dataShift(dataShift),
m_xferSize(xferSize + 1), // +1 for timestamp
m_isSigned(isSigned),
m_bigEndian(bigEndian),
m_port(port) {}
~Accumulator() { delete[] m_buf; }
void Update();
Notifier m_notifier;
uint32_t* m_buf;
wpi::mutex m_mutex;
int64_t m_value = 0;
uint32_t m_count = 0;
int32_t m_lastValue = 0;
uint32_t m_lastTimestamp = 0;
double m_integratedValue = 0;
int32_t m_center = 0;
int32_t m_deadband = 0;
double m_integratedCenter = 0;
int32_t m_validMask;
int32_t m_validValue;
int32_t m_dataMax; // one more than max data value
int32_t m_dataMsbMask; // data field MSB mask (for signed)
uint8_t m_dataShift; // data field shift right amount, in bits
int32_t m_xferSize; // SPI transfer size, in bytes
bool m_isSigned; // is data field signed?
bool m_bigEndian; // is response big endian?
HAL_SPIPort m_port;
};
void SPI::Accumulator::Update() {
bool done;
do {
done = true;
int32_t status = 0;
// get amount of data available
int32_t numToRead =
HAL_ReadSPIAutoReceivedData(m_port, m_buf, 0, 0, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
// only get whole responses; +1 is for timestamp
numToRead -= numToRead % m_xferSize;
if (numToRead > m_xferSize * kAccumulateDepth) {
numToRead = m_xferSize * kAccumulateDepth;
done = false;
}
if (numToRead == 0) {
return; // no samples
}
// read buffered data
HAL_ReadSPIAutoReceivedData(m_port, m_buf, numToRead, 0, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
// loop over all responses
for (int32_t off = 0; off < numToRead; off += m_xferSize) {
// get timestamp from first word
uint32_t timestamp = m_buf[off];
// convert from bytes
uint32_t resp = 0;
if (m_bigEndian) {
for (int32_t i = 1; i < m_xferSize; ++i) {
resp <<= 8;
resp |= m_buf[off + i] & 0xff;
}
} else {
for (int32_t i = m_xferSize - 1; i >= 1; --i) {
resp <<= 8;
resp |= m_buf[off + i] & 0xff;
}
}
// process response
if ((resp & m_validMask) == static_cast<uint32_t>(m_validValue)) {
// valid sensor data; extract data field
int32_t data = static_cast<int32_t>(resp >> m_dataShift);
data &= m_dataMax - 1;
// 2s complement conversion if signed MSB is set
if (m_isSigned && (data & m_dataMsbMask) != 0) {
data -= m_dataMax;
}
// center offset
int32_t dataNoCenter = data;
data -= m_center;
// only accumulate if outside deadband
if (data < -m_deadband || data > m_deadband) {
m_value += data;
if (m_count != 0) {
// timestamps use the 1us FPGA clock; also handle rollover
if (timestamp >= m_lastTimestamp) {
m_integratedValue +=
dataNoCenter *
static_cast<int32_t>(timestamp - m_lastTimestamp) * 1e-6 -
m_integratedCenter;
} else {
m_integratedValue +=
dataNoCenter *
static_cast<int32_t>((1ULL << 32) - m_lastTimestamp +
timestamp) *
1e-6 -
m_integratedCenter;
}
}
}
++m_count;
m_lastValue = data;
} else {
// no data from the sensor; just clear the last value
m_lastValue = 0;
}
m_lastTimestamp = timestamp;
}
} while (!done);
}
SPI::SPI(Port port) : m_port(static_cast<HAL_SPIPort>(port)) {
int32_t status = 0;
HAL_InitializeSPI(m_port, &status);
HAL_SetSPIMode(m_port, m_mode);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
HAL_Report(HALUsageReporting::kResourceType_SPI,
static_cast<uint8_t>(port) + 1);
}
SPI::~SPI() = default;
SPI::Port SPI::GetPort() const {
return static_cast<Port>(static_cast<int>(m_port));
}
void SPI::SetClockRate(int hz) {
HAL_SetSPISpeed(m_port, hz);
}
void SPI::SetMode(Mode mode) {
m_mode = static_cast<HAL_SPIMode>(mode & 0x3);
HAL_SetSPIMode(m_port, m_mode);
}
void SPI::SetChipSelectActiveHigh() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::SetChipSelectActiveLow() {
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
int SPI::Write(uint8_t* data, int size) {
int retVal = 0;
retVal = HAL_WriteSPI(m_port, data, size);
return retVal;
}
int SPI::Read(bool initiate, uint8_t* dataReceived, int size) {
int retVal = 0;
if (initiate) {
wpi::SmallVector<uint8_t, 32> dataToSend;
dataToSend.resize(size);
retVal = HAL_TransactionSPI(m_port, dataToSend.data(), dataReceived, size);
} else {
retVal = HAL_ReadSPI(m_port, dataReceived, size);
}
return retVal;
}
int SPI::Transaction(uint8_t* dataToSend, uint8_t* dataReceived, int size) {
int retVal = 0;
retVal = HAL_TransactionSPI(m_port, dataToSend, dataReceived, size);
return retVal;
}
void SPI::InitAuto(int bufferSize) {
int32_t status = 0;
HAL_InitSPIAuto(m_port, bufferSize, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::FreeAuto() {
int32_t status = 0;
HAL_FreeSPIAuto(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::SetAutoTransmitData(std::span<const uint8_t> dataToSend,
int zeroSize) {
int32_t status = 0;
HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(),
zeroSize, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::StartAutoRate(units::second_t period) {
int32_t status = 0;
HAL_StartSPIAutoRate(m_port, period.value(), &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) {
int32_t status = 0;
HAL_StartSPIAutoTrigger(m_port, source.GetPortHandleForRouting(),
static_cast<HAL_AnalogTriggerType>(
source.GetAnalogTriggerTypeForRouting()),
rising, falling, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::StopAuto() {
int32_t status = 0;
HAL_StopSPIAuto(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::ForceAutoRead() {
int32_t status = 0;
HAL_ForceSPIAutoRead(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
int SPI::ReadAutoReceivedData(uint32_t* buffer, int numToRead,
units::second_t timeout) {
int32_t status = 0;
int32_t val = HAL_ReadSPIAutoReceivedData(m_port, buffer, numToRead,
timeout.value(), &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
return val;
}
int SPI::GetAutoDroppedCount() {
int32_t status = 0;
int32_t val = HAL_GetSPIAutoDroppedCount(m_port, &status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
return val;
}
void SPI::ConfigureAutoStall(HAL_SPIPort port, int csToSclkTicks,
int stallTicks, int pow2BytesPerRead) {
int32_t status = 0;
HAL_ConfigureSPIAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead,
&status);
FRC_CheckErrorStatus(status, "Port {}", static_cast<int>(m_port));
}
void SPI::InitAccumulator(units::second_t period, int cmd, int xferSize,
int validMask, int validValue, int dataShift,
int dataSize, bool isSigned, bool bigEndian) {
InitAuto(xferSize * kAccumulateDepth);
uint8_t cmdBytes[4] = {0, 0, 0, 0};
if (bigEndian) {
for (int32_t i = xferSize - 1; i >= 0; --i) {
cmdBytes[i] = cmd & 0xff;
cmd >>= 8;
}
} else {
cmdBytes[0] = cmd & 0xff;
cmd >>= 8;
cmdBytes[1] = cmd & 0xff;
cmd >>= 8;
cmdBytes[2] = cmd & 0xff;
cmd >>= 8;
cmdBytes[3] = cmd & 0xff;
}
SetAutoTransmitData(cmdBytes, xferSize - 4);
StartAutoRate(period);
m_accum =
std::make_unique<Accumulator>(m_port, xferSize, validMask, validValue,
dataShift, dataSize, isSigned, bigEndian);
m_accum->m_notifier.StartPeriodic(period * kAccumulateDepth / 2);
}
void SPI::FreeAccumulator() {
m_accum.reset(nullptr);
FreeAuto();
}
void SPI::ResetAccumulator() {
if (!m_accum) {
return;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_value = 0;
m_accum->m_count = 0;
m_accum->m_lastValue = 0;
m_accum->m_lastTimestamp = 0;
m_accum->m_integratedValue = 0;
}
void SPI::SetAccumulatorCenter(int center) {
if (!m_accum) {
return;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_center = center;
}
void SPI::SetAccumulatorDeadband(int deadband) {
if (!m_accum) {
return;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_deadband = deadband;
}
int SPI::GetAccumulatorLastValue() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_lastValue;
}
int64_t SPI::GetAccumulatorValue() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_value;
}
int64_t SPI::GetAccumulatorCount() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_count;
}
double SPI::GetAccumulatorAverage() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
if (m_accum->m_count == 0) {
return 0.0;
}
return static_cast<double>(m_accum->m_value) / m_accum->m_count;
}
void SPI::GetAccumulatorOutput(int64_t& value, int64_t& count) const {
if (!m_accum) {
value = 0;
count = 0;
return;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
value = m_accum->m_value;
count = m_accum->m_count;
}
void SPI::SetAccumulatorIntegratedCenter(double center) {
if (!m_accum) {
return;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->m_integratedCenter = center;
}
double SPI::GetAccumulatorIntegratedValue() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
return m_accum->m_integratedValue;
}
double SPI::GetAccumulatorIntegratedAverage() const {
if (!m_accum) {
return 0;
}
std::scoped_lock lock(m_accum->m_mutex);
m_accum->Update();
if (m_accum->m_count <= 1) {
return 0.0;
}
// count-1 due to not integrating the first value received
return m_accum->m_integratedValue / (m_accum->m_count - 1);
}

59
wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp
View File

@@ -1,59 +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/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());
}

59
wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp
View File

@@ -1,59 +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/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());
}

8
wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp
View File

@@ -5,7 +5,6 @@
#include "frc/simulation/ADXL345Sim.h"
#include "frc/ADXL345_I2C.h"
#include "frc/ADXL345_SPI.h"
#include "frc/simulation/SimDeviceSim.h"
using namespace frc::sim;
@@ -18,13 +17,6 @@ ADXL345Sim::ADXL345Sim(const frc::ADXL345_I2C& accel) {
m_simZ = deviceSim.GetDouble("z");
}
ADXL345Sim::ADXL345Sim(const frc::ADXL345_SPI& accel) {
frc::sim::SimDeviceSim deviceSim{"Accel:ADXL345_SPI", accel.GetSpiPort()};
m_simX = deviceSim.GetDouble("x");
m_simY = deviceSim.GetDouble("y");
m_simZ = deviceSim.GetDouble("z");
}
void ADXL345Sim::SetX(double accel) {
m_simX.Set(accel);
}

29
wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp
View File

@@ -1,29 +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/simulation/ADXL362Sim.h"
#include "frc/ADXL362.h"
#include "frc/simulation/SimDeviceSim.h"
using namespace frc::sim;
ADXL362Sim::ADXL362Sim(const frc::ADXL362& accel) {
frc::sim::SimDeviceSim deviceSim{"Accel:ADXL362", accel.GetSpiPort()};
m_simX = deviceSim.GetDouble("x");
m_simY = deviceSim.GetDouble("y");
m_simZ = deviceSim.GetDouble("z");
}
void ADXL362Sim::SetX(double accel) {
m_simX.Set(accel);
}
void ADXL362Sim::SetY(double accel) {
m_simY.Set(accel);
}
void ADXL362Sim::SetZ(double accel) {
m_simZ.Set(accel);
}

24
wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp
View File

@@ -1,24 +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/simulation/ADXRS450_GyroSim.h"
#include "frc/ADXRS450_Gyro.h"
#include "frc/simulation/SimDeviceSim.h"
using namespace frc::sim;
ADXRS450_GyroSim::ADXRS450_GyroSim(const frc::ADXRS450_Gyro& gyro) {
frc::sim::SimDeviceSim deviceSim{"Gyro:ADXRS450", gyro.GetPort()};
m_simAngle = deviceSim.GetDouble("angle_x");
m_simRate = deviceSim.GetDouble("rate_x");
}
void ADXRS450_GyroSim::SetAngle(units::degree_t angle) {
m_simAngle.Set(angle.value());
}
void ADXRS450_GyroSim::SetRate(units::degrees_per_second_t rate) {
m_simRate.Set(rate.value());
}

105
wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp
View File

@@ -1,105 +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/simulation/SPIAccelerometerSim.h"
#include <memory>
#include <hal/simulation/SPIAccelerometerData.h>
using namespace frc;
using namespace frc::sim;
SPIAccelerometerSim::SPIAccelerometerSim(int index) {
m_index = index;
}
std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterActiveCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelSPIAccelerometerActiveCallback);
store->SetUid(HALSIM_RegisterSPIAccelerometerActiveCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
bool SPIAccelerometerSim::GetActive() const {
return HALSIM_GetSPIAccelerometerActive(m_index);
}
void SPIAccelerometerSim::SetActive(bool active) {
HALSIM_SetSPIAccelerometerActive(m_index, active);
}
std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterRangeCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelSPIAccelerometerRangeCallback);
store->SetUid(HALSIM_RegisterSPIAccelerometerRangeCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
int SPIAccelerometerSim::GetRange() const {
return HALSIM_GetSPIAccelerometerRange(m_index);
}
void SPIAccelerometerSim::SetRange(int range) {
HALSIM_SetSPIAccelerometerRange(m_index, range);
}
std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterXCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelSPIAccelerometerXCallback);
store->SetUid(HALSIM_RegisterSPIAccelerometerXCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
double SPIAccelerometerSim::GetX() const {
return HALSIM_GetSPIAccelerometerX(m_index);
}
void SPIAccelerometerSim::SetX(double x) {
HALSIM_SetSPIAccelerometerX(m_index, x);
}
std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterYCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelSPIAccelerometerYCallback);
store->SetUid(HALSIM_RegisterSPIAccelerometerYCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
double SPIAccelerometerSim::GetY() const {
return HALSIM_GetSPIAccelerometerY(m_index);
}
void SPIAccelerometerSim::SetY(double y) {
HALSIM_SetSPIAccelerometerY(m_index, y);
}
std::unique_ptr<CallbackStore> SPIAccelerometerSim::RegisterZCallback(
NotifyCallback callback, bool initialNotify) {
auto store = std::make_unique<CallbackStore>(
m_index, -1, callback, &HALSIM_CancelSPIAccelerometerZCallback);
store->SetUid(HALSIM_RegisterSPIAccelerometerZCallback(
m_index, &CallbackStoreThunk, store.get(), initialNotify));
return store;
}
double SPIAccelerometerSim::GetZ() const {
return HALSIM_GetSPIAccelerometerZ(m_index);
}
void SPIAccelerometerSim::SetZ(double z) {
HALSIM_SetSPIAccelerometerZ(m_index, z);
}
void SPIAccelerometerSim::ResetData() {
HALSIM_ResetSPIAccelerometerData(m_index);
}

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