Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-20 00:51:42 +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);
}