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

326
hal/src/main/java/edu/wpi/first/hal/SPIJNI.java
View File

@@ -1,326 +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.
package edu.wpi.first.hal;
import java.nio.ByteBuffer;
/**
* SPI HAL JNI functions.
*
* @see "SPI.h"
*/
public class SPIJNI extends JNIWrapper {
/** Invalid port number. */
public static final int INVALID_PORT = -1;
/** Onboard SPI bus port CS0. */
public static final int ONBOARD_CS0_PORT = 0;
/** Onboard SPI bus port CS1. */
public static final int ONBOARD_CS1_PORT = 1;
/** Onboard SPI bus port CS2. */
public static final int ONBOARD_CS2_PORT = 2;
/** Onboard SPI bus port CS3. */
public static final int ONBOARD_CS3_PORT = 3;
/** MXP (roboRIO MXP) SPI bus port. */
public static final int MXP_PORT = 4;
/** Clock idle low, data sampled on rising edge. */
public static final int SPI_MODE0 = 0;
/** Clock idle low, data sampled on falling edge. */
public static final int SPI_MODE1 = 1;
/** Clock idle high, data sampled on falling edge. */
public static final int SPI_MODE2 = 2;
/** Clock idle high, data sampled on rising edge. */
public static final int SPI_MODE3 = 3;
/**
* Initializes the SPI port. Opens the port if necessary and saves the handle.
*
* <p>If opening the MXP port, also sets up the channel functions appropriately.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS3, 4 for MXP
* @see "HAL_InitializeSPI"
*/
public static native void spiInitialize(int port);
/**
* Performs an SPI send/receive transaction.
*
* <p>This is a lower-level interface to the spi hardware giving you more control over each
* transaction.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param dataToSend Buffer of data to send as part of the transaction.
* @param dataReceived Buffer to read data into.
* @param size Number of bytes to transfer. [0..7]
* @return Number of bytes transferred, -1 for error
* @see "HAL_TransactionSPI"
*/
public static native int spiTransaction(
int port, ByteBuffer dataToSend, ByteBuffer dataReceived, byte size);
/**
* Performs an SPI send/receive transaction.
*
* <p>This is a lower-level interface to the spi hardware giving you more control over each
* transaction.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param dataToSend Buffer of data to send as part of the transaction.
* @param dataReceived Buffer to read data into.
* @param size Number of bytes to transfer. [0..7]
* @return Number of bytes transferred, -1 for error
* @see "HAL_TransactionSPI"
*/
public static native int spiTransactionB(
int port, byte[] dataToSend, byte[] dataReceived, byte size);
/**
* Executes a write transaction with the device.
*
* <p>Writes to a device and wait until the transaction is complete.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param dataToSend The data to write to the register on the device.
* @param sendSize The number of bytes to be written
* @return The number of bytes written. -1 for an error
* @see "HAL_WriteSPI"
*/
public static native int spiWrite(int port, ByteBuffer dataToSend, byte sendSize);
/**
* Executes a write transaction with the device.
*
* <p>Writes to a device and wait until the transaction is complete.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param dataToSend The data to write to the register on the device.
* @param sendSize The number of bytes to be written
* @return The number of bytes written. -1 for an error
* @see "HAL_WriteSPI"
*/
public static native int spiWriteB(int port, byte[] dataToSend, byte sendSize);
/**
* Executes a read from the device.
*
* <p>This method does not write any data out to the device.
*
* <p>Most spi devices will require a register address to be written before they begin returning
* data.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param initiate initiates a transaction when true. Just reads when false.
* @param dataReceived A pointer to the array of bytes to store the data read from the device.
* @param size The number of bytes to read in the transaction. [1..7]
* @return Number of bytes read. -1 for error.
* @see "HAL_ReadSPI"
*/
public static native int spiRead(int port, boolean initiate, ByteBuffer dataReceived, byte size);
/**
* Executes a read from the device.
*
* <p>This method does not write any data out to the device.
*
* <p>Most spi devices will require a register address to be written before they begin returning
* data.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param initiate initiates a transaction when true. Just reads when false.
* @param dataReceived A pointer to the array of bytes to store the data read from the device.
* @param size The number of bytes to read in the transaction. [1..7]
* @return Number of bytes read. -1 for error.
* @see "HAL_ReadSPI"
*/
public static native int spiReadB(int port, boolean initiate, byte[] dataReceived, byte size);
/**
* Closes the SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @see "HAL_CloseSPI"
*/
public static native void spiClose(int port);
/**
* Sets the clock speed for the SPI bus.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param speed The speed in Hz (500KHz-10MHz)
* @see "HAL_SetSPISpeed"
*/
public static native void spiSetSpeed(int port, int speed);
/**
* Sets the SPI Mode.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @param mode The SPI mode to use
* @see "HAL_SetSPIMode"
*/
public static native void spiSetMode(int port, int mode);
/**
* Gets the SPI Mode.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @return The SPI mode currently set
* @see "HAL_GetSPIMode"
*/
public static native int spiGetMode(int port);
/**
* Sets the CS Active high for a SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @see "HAL_SetSPIChipSelectActiveHigh"
*/
public static native void spiSetChipSelectActiveHigh(int port);
/**
* Sets the CS Active low for a SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @see "HAL_SetSPIChipSelectActiveLow"
*/
public static native void spiSetChipSelectActiveLow(int port);
/**
* Initializes the SPI automatic accumulator.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param bufferSize The accumulator buffer size.
* @see "HAL_InitSPIAuto"
*/
public static native void spiInitAuto(int port, int bufferSize);
/**
* Frees an SPI automatic accumulator.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @see "HAL_FreeSPIAuto"
*/
public static native void spiFreeAuto(int port);
/**
* Sets the period for automatic SPI accumulation.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param period The accumulation period (seconds).
* @see "HAL_StartSPIAutoRate"
*/
public static native void spiStartAutoRate(int port, double period);
/**
* Starts the auto SPI accumulator on a specific trigger.
*
* <p>Note that triggering on both rising and falling edges is a valid configuration.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param digitalSourceHandle The trigger source to use (Either HAL_AnalogTriggerHandle or
* HAL_DigitalHandle).
* @param analogTriggerType The analog trigger type, if the source is an analog trigger.
* @param triggerRising Trigger on the rising edge if true.
* @param triggerFalling Trigger on the falling edge if true.
* @see "HAL_StartSPIAutoTrigger"
*/
public static native void spiStartAutoTrigger(
int port,
int digitalSourceHandle,
int analogTriggerType,
boolean triggerRising,
boolean triggerFalling);
/**
* Stops an automatic SPI accumulation.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @see "HAL_StopSPIAuto"
*/
public static native void spiStopAuto(int port);
/**
* Sets the data to be transmitted to the device to initiate a read.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param dataToSend Pointer to the data to send (Gets copied for continue use, so no need to keep
* alive).
* @param zeroSize The number of zeros to send after the data.
* @see "HAL_SetSPIAutoTransmitData"
*/
public static native void spiSetAutoTransmitData(int port, byte[] dataToSend, int zeroSize);
/**
* Immediately forces an SPI read to happen.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @see "HAL_ForceSPIAutoRead"
*/
public static native void spiForceAutoRead(int port);
/**
* Reads data received by the SPI accumulator. 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 dataSize + zeroSize set in
* spiSetAutoTransmitData.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param buffer The buffer to store the data into.
* @param numToRead The number of words to read.
* @param timeout The read timeout (in seconds).
* @return The number of words actually read.
* @see "HAL_ReadSPIAutoReceivedData"
*/
public static native int spiReadAutoReceivedData(
int port, ByteBuffer buffer, int numToRead, double timeout);
/**
* Reads data received by the SPI accumulator. 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 dataSize + zeroSize set in
* spiSetAutoTransmitData.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @param buffer The buffer to store the data into.
* @param numToRead The number of words to read.
* @param timeout The read timeout (in seconds).
* @return The number of words actually read.
* @see "HAL_ReadSPIAutoReceivedData"
*/
public static native int spiReadAutoReceivedData(
int port, int[] buffer, int numToRead, double timeout);
/**
* Gets the count of how many SPI accumulations have been missed.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP.
* @return The number of missed accumulations.
* @see "HAL_GetSPIAutoDroppedCount"
*/
public static native int spiGetAutoDroppedCount(int port);
/**
* 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
* @see "HAL_ConfigureSPIAutoStall"
*/
public static native void spiConfigureAutoStall(
int port, int csToSclkTicks, int stallTicks, int pow2BytesPerRead);
/** Utility class. */
private SPIJNI() {}
}

60
hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java
View File

@@ -1,60 +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.
package edu.wpi.first.hal.simulation;
import edu.wpi.first.hal.JNIWrapper;
/** JNI for SPI accelerometer data. */
public class SPIAccelerometerDataJNI extends JNIWrapper {
public static native int registerActiveCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelActiveCallback(int index, int uid);
public static native boolean getActive(int index);
public static native void setActive(int index, boolean active);
public static native int registerRangeCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelRangeCallback(int index, int uid);
public static native int getRange(int index);
public static native void setRange(int index, int range);
public static native int registerXCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelXCallback(int index, int uid);
public static native double getX(int index);
public static native void setX(int index, double x);
public static native int registerYCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelYCallback(int index, int uid);
public static native double getY(int index);
public static native void setY(int index, double y);
public static native int registerZCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelZCallback(int index, int uid);
public static native double getZ(int index);
public static native void setZ(int index, double z);
public static native void resetData(int index);
/** Utility class. */
private SPIAccelerometerDataJNI() {}
}

37
hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java
View File

@@ -1,37 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
package edu.wpi.first.hal.simulation;
import edu.wpi.first.hal.JNIWrapper;
/** JNI for SPI data. */
public class SPIDataJNI extends JNIWrapper {
public static native int registerInitializedCallback(
int index, NotifyCallback callback, boolean initialNotify);
public static native void cancelInitializedCallback(int index, int uid);
public static native boolean getInitialized(int index);
public static native void setInitialized(int index, boolean initialized);
public static native int registerReadCallback(int index, BufferCallback callback);
public static native void cancelReadCallback(int index, int uid);
public static native int registerWriteCallback(int index, ConstBufferCallback callback);
public static native void cancelWriteCallback(int index, int uid);
public static native int registerReadAutoReceiveBufferCallback(
int index, SpiReadAutoReceiveBufferCallback callback);
public static native void cancelReadAutoReceiveBufferCallback(int index, int uid);
public static native void resetData(int index);
/** Utility class. */
private SPIDataJNI() {}
}

9
hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java
View File

@@ -1,9 +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.
package edu.wpi.first.hal.simulation;
public interface SpiReadAutoReceiveBufferCallback {
int callback(String name, int[] buffer, int numToRead);
}

14
hal/src/main/native/cpp/handles/HandlesInternal.cpp
View File

@@ -61,20 +61,6 @@ HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module) {
handle += channel;
return handle;
}
HAL_PortHandle createPortHandleForSPI(uint8_t channel) {
// set last 8 bits, then shift to first 8 bits
HAL_PortHandle handle = static_cast<HAL_PortHandle>(HAL_HandleEnum::Port);
handle = handle << 16;
// set second set up bits to 1
int32_t temp = 1;
temp = (temp << 8) & 0xff00;
handle += temp;
// shift to last set of bits
handle = handle << 8;
// add channel to last 8 bits
handle += channel;
return handle;
}
HAL_Handle createHandle(int16_t index, HAL_HandleEnum handleType,
int16_t version) {
if (index < 0) {

466
hal/src/main/native/cpp/jni/SPIJNI.cpp
View File

@@ -1,466 +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 <jni.h>
#include <cassert>
#include <wpi/jni_util.h>
#include "HALUtil.h"
#include "edu_wpi_first_hal_SPIJNI.h"
#include "hal/SPI.h"
using namespace hal;
using namespace wpi::java;
static_assert(HAL_SPIPort::HAL_SPI_kInvalid ==
edu_wpi_first_hal_SPIJNI_INVALID_PORT);
static_assert(HAL_SPIPort::HAL_SPI_kOnboardCS0 ==
edu_wpi_first_hal_SPIJNI_ONBOARD_CS0_PORT);
static_assert(HAL_SPIPort::HAL_SPI_kOnboardCS1 ==
edu_wpi_first_hal_SPIJNI_ONBOARD_CS1_PORT);
static_assert(HAL_SPIPort::HAL_SPI_kOnboardCS2 ==
edu_wpi_first_hal_SPIJNI_ONBOARD_CS2_PORT);
static_assert(HAL_SPIPort::HAL_SPI_kOnboardCS3 ==
edu_wpi_first_hal_SPIJNI_ONBOARD_CS3_PORT);
static_assert(HAL_SPIPort::HAL_SPI_kMXP == edu_wpi_first_hal_SPIJNI_MXP_PORT);
static_assert(HAL_SPIMode::HAL_SPI_kMode0 ==
edu_wpi_first_hal_SPIJNI_SPI_MODE0);
static_assert(HAL_SPIMode::HAL_SPI_kMode1 ==
edu_wpi_first_hal_SPIJNI_SPI_MODE1);
static_assert(HAL_SPIMode::HAL_SPI_kMode2 ==
edu_wpi_first_hal_SPIJNI_SPI_MODE2);
static_assert(HAL_SPIMode::HAL_SPI_kMode3 ==
edu_wpi_first_hal_SPIJNI_SPI_MODE3);
extern "C" {
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiInitialize
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiInitialize
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
HAL_InitializeSPI(static_cast<HAL_SPIPort>(port), &status);
CheckStatusForceThrow(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiTransaction
* Signature: (ILjava/lang/Object;Ljava/lang/Object;B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiTransaction
(JNIEnv* env, jclass, jint port, jobject dataToSend, jobject dataReceived,
jbyte size)
{
uint8_t* dataToSendPtr = nullptr;
if (dataToSend != nullptr) {
dataToSendPtr =
reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
}
uint8_t* dataReceivedPtr =
reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
jint retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port),
dataToSendPtr, dataReceivedPtr, size);
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiTransactionB
* Signature: (I[B[BB)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiTransactionB
(JNIEnv* env, jclass, jint port, jbyteArray dataToSend,
jbyteArray dataReceived, jbyte size)
{
if (size < 0) {
ThrowIllegalArgumentException(env, "SPIJNI.spiTransactionB() size < 0");
return 0;
}
wpi::SmallVector<uint8_t, 128> recvBuf;
recvBuf.resize(size);
jint retVal =
HAL_TransactionSPI(static_cast<HAL_SPIPort>(port),
reinterpret_cast<const uint8_t*>(
JSpan<const jbyte>(env, dataToSend).data()),
recvBuf.data(), size);
env->SetByteArrayRegion(dataReceived, 0, size,
reinterpret_cast<const jbyte*>(recvBuf.data()));
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiWrite
* Signature: (ILjava/lang/Object;B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiWrite
(JNIEnv* env, jclass, jint port, jobject dataToSend, jbyte size)
{
uint8_t* dataToSendPtr = nullptr;
if (dataToSend != nullptr) {
dataToSendPtr =
reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataToSend));
}
jint retVal =
HAL_WriteSPI(static_cast<HAL_SPIPort>(port), dataToSendPtr, size);
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiWriteB
* Signature: (I[BB)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiWriteB
(JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jbyte size)
{
jint retVal = HAL_WriteSPI(static_cast<HAL_SPIPort>(port),
reinterpret_cast<const uint8_t*>(
JSpan<const jbyte>(env, dataToSend).data()),
size);
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiRead
* Signature: (IZLjava/lang/Object;B)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiRead
(JNIEnv* env, jclass, jint port, jboolean initiate, jobject dataReceived,
jbyte size)
{
if (size < 0) {
ThrowIllegalArgumentException(env, "SPIJNI.spiRead() size < 0");
return 0;
}
uint8_t* dataReceivedPtr =
reinterpret_cast<uint8_t*>(env->GetDirectBufferAddress(dataReceived));
jint retVal;
if (initiate) {
wpi::SmallVector<uint8_t, 128> sendBuf;
sendBuf.resize(size);
retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port), sendBuf.data(),
dataReceivedPtr, size);
} else {
retVal = HAL_ReadSPI(static_cast<HAL_SPIPort>(port),
reinterpret_cast<uint8_t*>(dataReceivedPtr), size);
}
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiReadB
* Signature: (IZ[BB)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiReadB
(JNIEnv* env, jclass, jint port, jboolean initiate, jbyteArray dataReceived,
jbyte size)
{
if (size < 0) {
ThrowIllegalArgumentException(env, "SPIJNI.spiReadB() size < 0");
return 0;
}
jint retVal;
wpi::SmallVector<uint8_t, 128> recvBuf;
recvBuf.resize(size);
if (initiate) {
wpi::SmallVector<uint8_t, 128> sendBuf;
sendBuf.resize(size);
retVal = HAL_TransactionSPI(static_cast<HAL_SPIPort>(port), sendBuf.data(),
recvBuf.data(), size);
} else {
retVal = HAL_ReadSPI(static_cast<HAL_SPIPort>(port), recvBuf.data(), size);
}
env->SetByteArrayRegion(dataReceived, 0, size,
reinterpret_cast<const jbyte*>(recvBuf.data()));
return retVal;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiClose
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiClose
(JNIEnv*, jclass, jint port)
{
HAL_CloseSPI(static_cast<HAL_SPIPort>(port));
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiSetSpeed
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiSetSpeed
(JNIEnv*, jclass, jint port, jint speed)
{
HAL_SetSPISpeed(static_cast<HAL_SPIPort>(port), speed);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiSetMode
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiSetMode
(JNIEnv*, jclass, jint port, jint mode)
{
HAL_SetSPIMode(static_cast<HAL_SPIPort>(port),
static_cast<HAL_SPIMode>(mode));
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiGetMode
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiGetMode
(JNIEnv*, jclass, jint port)
{
return static_cast<jint>(HAL_GetSPIMode(static_cast<HAL_SPIPort>(port)));
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiSetChipSelectActiveHigh
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiSetChipSelectActiveHigh
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
HAL_SetSPIChipSelectActiveHigh(static_cast<HAL_SPIPort>(port), &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiSetChipSelectActiveLow
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiSetChipSelectActiveLow
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
HAL_SetSPIChipSelectActiveLow(static_cast<HAL_SPIPort>(port), &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiInitAuto
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiInitAuto
(JNIEnv* env, jclass, jint port, jint bufferSize)
{
int32_t status = 0;
HAL_InitSPIAuto(static_cast<HAL_SPIPort>(port), bufferSize, &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiFreeAuto
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiFreeAuto
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
if (port != HAL_kInvalidHandle) {
HAL_FreeSPIAuto(static_cast<HAL_SPIPort>(port), &status);
}
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiStartAutoRate
* Signature: (ID)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiStartAutoRate
(JNIEnv* env, jclass, jint port, jdouble period)
{
int32_t status = 0;
HAL_StartSPIAutoRate(static_cast<HAL_SPIPort>(port), period, &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiStartAutoTrigger
* Signature: (IIIZZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiStartAutoTrigger
(JNIEnv* env, jclass, jint port, jint digitalSourceHandle,
jint analogTriggerType, jboolean triggerRising, jboolean triggerFalling)
{
int32_t status = 0;
HAL_StartSPIAutoTrigger(static_cast<HAL_SPIPort>(port), digitalSourceHandle,
static_cast<HAL_AnalogTriggerType>(analogTriggerType),
triggerRising, triggerFalling, &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiStopAuto
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiStopAuto
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
HAL_StopSPIAuto(static_cast<HAL_SPIPort>(port), &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiSetAutoTransmitData
* Signature: (I[BI)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiSetAutoTransmitData
(JNIEnv* env, jclass, jint port, jbyteArray dataToSend, jint zeroSize)
{
JSpan<const jbyte> jarr(env, dataToSend);
int32_t status = 0;
HAL_SetSPIAutoTransmitData(static_cast<HAL_SPIPort>(port),
reinterpret_cast<const uint8_t*>(jarr.data()),
jarr.size(), zeroSize, &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiForceAutoRead
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiForceAutoRead
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
HAL_ForceSPIAutoRead(static_cast<HAL_SPIPort>(port), &status);
CheckStatus(env, status);
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiReadAutoReceivedData
* Signature: (ILjava/lang/Object;ID)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__ILjava_nio_ByteBuffer_2ID
(JNIEnv* env, jclass, jint port, jobject buffer, jint numToRead,
jdouble timeout)
{
uint32_t* recvBuf =
reinterpret_cast<uint32_t*>(env->GetDirectBufferAddress(buffer));
int32_t status = 0;
jint retval = HAL_ReadSPIAutoReceivedData(
static_cast<HAL_SPIPort>(port), recvBuf, numToRead, timeout, &status);
CheckStatus(env, status);
return retval;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiReadAutoReceivedData
* Signature: (I[IID)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiReadAutoReceivedData__I_3IID
(JNIEnv* env, jclass, jint port, jintArray buffer, jint numToRead,
jdouble timeout)
{
if (numToRead < 0) {
ThrowIllegalArgumentException(
env, "SPIJNI.spiReadAutoReceivedData() numToRead < 0");
return 0;
}
wpi::SmallVector<uint32_t, 128> recvBuf;
recvBuf.resize(numToRead);
int32_t status = 0;
jint retval =
HAL_ReadSPIAutoReceivedData(static_cast<HAL_SPIPort>(port),
recvBuf.data(), numToRead, timeout, &status);
if (!CheckStatus(env, status)) {
return retval;
}
if (numToRead > 0) {
env->SetIntArrayRegion(buffer, 0, numToRead,
reinterpret_cast<const jint*>(recvBuf.data()));
}
return retval;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiGetAutoDroppedCount
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiGetAutoDroppedCount
(JNIEnv* env, jclass, jint port)
{
int32_t status = 0;
auto retval =
HAL_GetSPIAutoDroppedCount(static_cast<HAL_SPIPort>(port), &status);
CheckStatus(env, status);
return retval;
}
/*
* Class: edu_wpi_first_hal_SPIJNI
* Method: spiConfigureAutoStall
* Signature: (IIII)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_SPIJNI_spiConfigureAutoStall
(JNIEnv* env, jclass, jint port, jint csToSclkTicks, jint stallTicks,
jint pow2BytesPerRead)
{
int32_t status = 0;
HAL_ConfigureSPIAutoStall(static_cast<HAL_SPIPort>(port), csToSclkTicks,
stallTicks, pow2BytesPerRead, &status);
CheckStatus(env, status);
}
} // extern "C"

277
hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp
View File

@@ -1,277 +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 <jni.h>
#include "CallbackStore.h"
#include "edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI.h"
#include "hal/simulation/SPIAccelerometerData.h"
using namespace hal;
extern "C" {
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: registerActiveCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerActiveCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIAccelerometerActiveCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: cancelActiveCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelActiveCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIAccelerometerActiveCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: getActive
* Signature: (I)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getActive
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIAccelerometerActive(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: setActive
* Signature: (IZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setActive
(JNIEnv*, jclass, jint index, jboolean value)
{
HALSIM_SetSPIAccelerometerActive(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: registerRangeCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerRangeCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIAccelerometerRangeCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: cancelRangeCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelRangeCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIAccelerometerRangeCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: getRange
* Signature: (I)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getRange
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIAccelerometerRange(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: setRange
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setRange
(JNIEnv*, jclass, jint index, jint value)
{
HALSIM_SetSPIAccelerometerRange(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: registerXCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerXCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIAccelerometerXCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: cancelXCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelXCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIAccelerometerXCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: getX
* Signature: (I)D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getX
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIAccelerometerX(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: setX
* Signature: (ID)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setX
(JNIEnv*, jclass, jint index, jdouble value)
{
HALSIM_SetSPIAccelerometerX(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: registerYCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerYCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIAccelerometerYCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: cancelYCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelYCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIAccelerometerYCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: getY
* Signature: (I)D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getY
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIAccelerometerY(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: setY
* Signature: (ID)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setY
(JNIEnv*, jclass, jint index, jdouble value)
{
HALSIM_SetSPIAccelerometerY(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: registerZCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_registerZCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIAccelerometerZCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: cancelZCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_cancelZCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIAccelerometerZCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: getZ
* Signature: (I)D
*/
JNIEXPORT jdouble JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_getZ
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIAccelerometerZ(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: setZ
* Signature: (ID)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_setZ
(JNIEnv*, jclass, jint index, jdouble value)
{
HALSIM_SetSPIAccelerometerZ(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI
* Method: resetData
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIAccelerometerDataJNI_resetData
(JNIEnv*, jclass, jint index)
{
HALSIM_ResetSPIAccelerometerData(index);
}
} // extern "C"

157
hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp
View File

@@ -1,157 +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 <jni.h>
#include "BufferCallbackStore.h"
#include "CallbackStore.h"
#include "ConstBufferCallbackStore.h"
#include "SpiReadAutoReceiveBufferCallbackStore.h"
#include "edu_wpi_first_hal_simulation_SPIDataJNI.h"
#include "hal/simulation/SPIData.h"
using namespace hal;
extern "C" {
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: registerInitializedCallback
* Signature: (ILjava/lang/Object;Z)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerInitializedCallback
(JNIEnv* env, jclass, jint index, jobject callback, jboolean initialNotify)
{
return sim::AllocateCallback(env, index, callback, initialNotify,
&HALSIM_RegisterSPIInitializedCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: cancelInitializedCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelInitializedCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
return sim::FreeCallback(env, handle, index,
&HALSIM_CancelSPIInitializedCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: getInitialized
* Signature: (I)Z
*/
JNIEXPORT jboolean JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_getInitialized
(JNIEnv*, jclass, jint index)
{
return HALSIM_GetSPIInitialized(index);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: setInitialized
* Signature: (IZ)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_setInitialized
(JNIEnv*, jclass, jint index, jboolean value)
{
HALSIM_SetSPIInitialized(index, value);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: registerReadCallback
* Signature: (ILjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerReadCallback
(JNIEnv* env, jclass, jint index, jobject callback)
{
return sim::AllocateBufferCallback(env, index, callback,
&HALSIM_RegisterSPIReadCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: cancelReadCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelReadCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
sim::FreeBufferCallback(env, handle, index, &HALSIM_CancelSPIReadCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: registerWriteCallback
* Signature: (ILjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerWriteCallback
(JNIEnv* env, jclass, jint index, jobject callback)
{
return sim::AllocateConstBufferCallback(env, index, callback,
&HALSIM_RegisterSPIWriteCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: cancelWriteCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelWriteCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
sim::FreeConstBufferCallback(env, handle, index,
&HALSIM_CancelSPIWriteCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: registerReadAutoReceiveBufferCallback
* Signature: (ILjava/lang/Object;)I
*/
JNIEXPORT jint JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_registerReadAutoReceiveBufferCallback
(JNIEnv* env, jclass, jint index, jobject callback)
{
return sim::AllocateSpiBufferCallback(
env, index, callback, &HALSIM_RegisterSPIReadAutoReceivedDataCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: cancelReadAutoReceiveBufferCallback
* Signature: (II)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_cancelReadAutoReceiveBufferCallback
(JNIEnv* env, jclass, jint index, jint handle)
{
sim::FreeSpiBufferCallback(env, handle, index,
&HALSIM_CancelSPIReadAutoReceivedDataCallback);
}
/*
* Class: edu_wpi_first_hal_simulation_SPIDataJNI
* Method: resetData
* Signature: (I)V
*/
JNIEXPORT void JNICALL
Java_edu_wpi_first_hal_simulation_SPIDataJNI_resetData
(JNIEnv*, jclass, jint index)
{
HALSIM_ResetSPIData(index);
}
} // extern "C"

21
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp
View File

@@ -10,7 +10,6 @@
#include "CallbackStore.h"
#include "ConstBufferCallbackStore.h"
#include "SimDeviceDataJNI.h"
#include "SpiReadAutoReceiveBufferCallbackStore.h"
#include "edu_wpi_first_hal_simulation_SimulatorJNI.h"
#include "hal/HAL.h"
#include "hal/handles/HandlesInternal.h"
@@ -22,11 +21,9 @@ static JavaVM* jvm = nullptr;
static JClass notifyCallbackCls;
static JClass bufferCallbackCls;
static JClass constBufferCallbackCls;
static JClass spiReadAutoReceiveBufferCallbackCls;
static jmethodID notifyCallbackCallback;
static jmethodID bufferCallbackCallback;
static jmethodID constBufferCallbackCallback;
static jmethodID spiReadAutoReceiveBufferCallbackCallback;
namespace hal::sim {
jint SimOnLoad(JavaVM* vm, void* reserved) {
@@ -73,23 +70,9 @@ jint SimOnLoad(JavaVM* vm, void* reserved) {
return JNI_ERR;
}
spiReadAutoReceiveBufferCallbackCls = JClass(
env, "edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback");
if (!spiReadAutoReceiveBufferCallbackCls) {
return JNI_ERR;
}
spiReadAutoReceiveBufferCallbackCallback =
env->GetMethodID(spiReadAutoReceiveBufferCallbackCls, "callback",
"(Ljava/lang/String;[II)I");
if (!spiReadAutoReceiveBufferCallbackCallback) {
return JNI_ERR;
}
InitializeStore();
InitializeBufferStore();
InitializeConstBufferStore();
InitializeSpiBufferStore();
if (!InitializeSimDeviceDataJNI(env)) {
return JNI_ERR;
}
@@ -106,7 +89,6 @@ void SimOnUnload(JavaVM* vm, void* reserved) {
notifyCallbackCls.free(env);
bufferCallbackCls.free(env);
constBufferCallbackCls.free(env);
spiReadAutoReceiveBufferCallbackCls.free(env);
FreeSimDeviceDataJNI(env);
jvm = nullptr;
}
@@ -127,9 +109,6 @@ jmethodID GetConstBufferCallback() {
return constBufferCallbackCallback;
}
jmethodID GetSpiReadAutoReceiveBufferCallback() {
return spiReadAutoReceiveBufferCallbackCallback;
}
} // namespace hal::sim
extern "C" {

1
hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h
View File

@@ -15,5 +15,4 @@ JavaVM* GetJVM();
jmethodID GetNotifyCallback();
jmethodID GetBufferCallback();
jmethodID GetConstBufferCallback();
jmethodID GetSpiReadAutoReceiveBufferCallback();
} // namespace hal::sim

134
hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp
View File

@@ -1,134 +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 "SpiReadAutoReceiveBufferCallbackStore.h"
#include <jni.h>
#include <cstdio>
#include <memory>
#include <wpi/jni_util.h>
#include "SimulatorJNI.h"
#include "hal/Types.h"
#include "hal/handles/UnlimitedHandleResource.h"
using namespace hal;
using namespace hal::sim;
using namespace wpi::java;
static hal::UnlimitedHandleResource<
SIM_JniHandle, SpiReadAutoReceiveBufferCallbackStore,
hal::HAL_HandleEnum::SimulationJni>* callbackHandles;
namespace hal::sim {
void InitializeSpiBufferStore() {
static hal::UnlimitedHandleResource<SIM_JniHandle,
SpiReadAutoReceiveBufferCallbackStore,
hal::HAL_HandleEnum::SimulationJni>
cb;
callbackHandles = &cb;
}
} // namespace hal::sim
void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) {
m_call = JGlobal<jobject>(env, obj);
}
int32_t SpiReadAutoReceiveBufferCallbackStore::performCallback(
const char* name, uint32_t* buffer, int32_t numToRead) {
JNIEnv* env;
JavaVM* vm = sim::GetJVM();
bool didAttachThread = false;
int tryGetEnv = vm->GetEnv(reinterpret_cast<void**>(&env), JNI_VERSION_1_6);
if (tryGetEnv == JNI_EDETACHED) {
// Thread not attached
didAttachThread = true;
if (vm->AttachCurrentThread(reinterpret_cast<void**>(&env), nullptr) != 0) {
// Failed to attach, log and return
std::puts("Failed to attach");
std::fflush(stdout);
return -1;
}
} else if (tryGetEnv == JNI_EVERSION) {
std::puts("Invalid JVM Version requested");
std::fflush(stdout);
}
auto toCallbackArr = MakeJIntArray(
env, std::span<const uint32_t>{buffer, static_cast<size_t>(numToRead)});
jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(),
MakeJString(env, name), toCallbackArr,
static_cast<jint>(numToRead));
jint* fromCallbackArr = reinterpret_cast<jint*>(
env->GetPrimitiveArrayCritical(toCallbackArr, nullptr));
for (int i = 0; i < ret; i++) {
buffer[i] = fromCallbackArr[i];
}
env->ReleasePrimitiveArrayCritical(toCallbackArr, fromCallbackArr, JNI_ABORT);
if (env->ExceptionCheck()) {
env->ExceptionDescribe();
}
if (didAttachThread) {
vm->DetachCurrentThread();
}
return ret;
}
void SpiReadAutoReceiveBufferCallbackStore::free(JNIEnv* env) {
m_call.free(env);
}
SIM_JniHandle sim::AllocateSpiBufferCallback(
JNIEnv* env, jint index, jobject callback,
RegisterSpiBufferCallbackFunc createCallback) {
auto callbackStore =
std::make_shared<SpiReadAutoReceiveBufferCallbackStore>();
auto handle = callbackHandles->Allocate(callbackStore);
if (handle == HAL_kInvalidHandle) {
return -1;
}
uintptr_t handleAsPtr = static_cast<uintptr_t>(handle);
void* handleAsVoidPtr = reinterpret_cast<void*>(handleAsPtr);
callbackStore->create(env, callback);
auto callbackFunc = [](const char* name, void* param, uint32_t* buffer,
int32_t numToRead, int32_t* outputCount) {
uintptr_t handleTmp = reinterpret_cast<uintptr_t>(param);
SIM_JniHandle handle = static_cast<SIM_JniHandle>(handleTmp);
auto data = callbackHandles->Get(handle);
if (!data) {
return;
}
*outputCount = data->performCallback(name, buffer, numToRead);
};
auto id = createCallback(index, callbackFunc, handleAsVoidPtr);
callbackStore->setCallbackId(id);
return handle;
}
void sim::FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
FreeSpiBufferCallbackFunc freeCallback) {
auto callback = callbackHandles->Free(handle);
if (callback == nullptr) {
return;
}
freeCallback(index, callback->getCallbackId());
callback->free(env);
}

44
hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h
View File

@@ -1,44 +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 <jni.h>
#include <wpi/jni_util.h>
#include "SimulatorJNI.h"
#include "hal/Types.h"
#include "hal/Value.h"
#include "hal/handles/UnlimitedHandleResource.h"
#include "hal/simulation/NotifyListener.h"
#include "hal/simulation/SPIData.h"
namespace hal::sim {
class SpiReadAutoReceiveBufferCallbackStore {
public:
void create(JNIEnv* env, jobject obj);
int32_t performCallback(const char* name, uint32_t* buffer,
int32_t numToRead);
void free(JNIEnv* env);
void setCallbackId(int32_t id) { callbackId = id; }
int32_t getCallbackId() { return callbackId; }
private:
wpi::java::JGlobal<jobject> m_call;
int32_t callbackId;
};
void InitializeSpiBufferStore();
using RegisterSpiBufferCallbackFunc = int32_t (*)(
int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
using FreeSpiBufferCallbackFunc = void (*)(int32_t index, int32_t uid);
SIM_JniHandle AllocateSpiBufferCallback(
JNIEnv* env, jint index, jobject callback,
RegisterSpiBufferCallbackFunc createCallback);
void FreeSpiBufferCallback(JNIEnv* env, SIM_JniHandle handle, jint index,
FreeSpiBufferCallbackFunc freeCallback);
} // namespace hal::sim

1
hal/src/main/native/include/hal/HAL.h
View File

@@ -30,7 +30,6 @@
#include "hal/PWM.h"
#include "hal/Ports.h"
#include "hal/Power.h"
#include "hal/SPI.h"
#include "hal/SerialPort.h"
#include "hal/SimDevice.h"
#include "hal/Threads.h"

281
hal/src/main/native/include/hal/SPI.h
View File

@@ -1,281 +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/AnalogTrigger.h"
#include "hal/SPITypes.h"
#include "hal/Types.h"
/**
* @defgroup hal_spi SPI Functions
* @ingroup hal_capi
* @{
*/
#ifdef __cplusplus
extern "C" {
#endif
/**
* Initializes the SPI port. Opens the port if necessary and saves the handle.
*
* If opening the MXP port, also sets up the channel functions appropriately.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS3, 4
* for MXP
* @param[out] status the error code, or 0 for success
*/
void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status);
/**
* Performs an SPI send/receive transaction.
*
* This is a lower-level interface to the spi hardware giving you more control
* over each transaction.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP
* @param dataToSend Buffer of data to send as part of the transaction.
* @param dataReceived Buffer to read data into.
* @param size Number of bytes to transfer. [0..7]
* @return Number of bytes transferred, -1 for error
*/
int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
uint8_t* dataReceived, int32_t size);
/**
* Executes a write transaction with the device.
*
* Writes to a device and wait until the transaction is complete.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP
* @param dataToSend The data to write to the register on the device.
* @param sendSize The number of bytes to be written
* @return The number of bytes written. -1 for an error
*/
int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t sendSize);
/**
* Executes a read from the device.
*
* This method does not write any data out to the device.
*
* Most spi devices will require a register address to be written before they
* begin returning data.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP
* @param buffer A pointer to the array of bytes to store the data read from the
* device.
* @param count The number of bytes to read in the transaction. [1..7]
* @return Number of bytes read. -1 for error.
*/
int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count);
/**
* Closes the SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
*/
void HAL_CloseSPI(HAL_SPIPort port);
/**
* Sets the clock speed for the SPI bus.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP
* @param speed The speed in Hz (500KHz-10MHz)
*/
void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed);
/**
* Sets the SPI Mode.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP
* @param mode The SPI mode to use
*/
void HAL_SetSPIMode(HAL_SPIPort port, HAL_SPIMode mode);
/**
* Gets the SPI Mode.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP
* @return The SPI mode currently set
*/
HAL_SPIMode HAL_GetSPIMode(HAL_SPIPort port);
/**
* Sets the CS Active high for a SPI port.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP
* @param[out] status the error code, or 0 for success
*/
void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status);
/**
* Sets the CS Active low for a SPI port.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP
* @param[out] status the error code, or 0 for success
*/
void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status);
/**
* Gets the stored handle for a SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for MXP
* @return The stored handle for the SPI port. 0 represents no stored
* handle.
*/
int32_t HAL_GetSPIHandle(HAL_SPIPort port);
/**
* Sets the stored handle for a SPI port.
*
* @param port The number of the port to use. 0-3 for Onboard CS0-CS2, 4 for
* MXP.
* @param handle The value of the handle for the port.
*/
void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle);
/**
* Initializes the SPI automatic accumulator.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2,
* 4 for MXP.
* @param[in] bufferSize The accumulator buffer size.
* @param[out] status the error code, or 0 for success
*/
void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status);
/**
* Frees an SPI automatic accumulator.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP.
* @param[out] status the error code, or 0 for success
*/
void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status);
/**
* Sets the period for automatic SPI accumulation.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP.
* @param[in] period The accumulation period (seconds).
* @param[out] status the error code, or 0 for success
*/
void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status);
/**
* Starts the auto SPI accumulator on a specific trigger.
*
* Note that triggering on both rising and falling edges is a valid
* configuration.
*
* @param[in] port The number of the port to use. 0-3 for Onboard
* CS0-CS2, 4 for MXP.
* @param[in] digitalSourceHandle The trigger source to use (Either
* HAL_AnalogTriggerHandle or HAL_DigitalHandle).
* @param[in] analogTriggerType The analog trigger type, if the source is an
* analog trigger.
* @param[in] triggerRising Trigger on the rising edge if true.
* @param[in] triggerFalling Trigger on the falling edge if true.
* @param[out] status the error code, or 0 for success
*/
void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool triggerRising, HAL_Bool triggerFalling,
int32_t* status);
/**
* Stops an automatic SPI accumulation.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP.
* @param[out] status the error code, or 0 for success
*/
void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status);
/**
* Sets the data to be transmitted to the device to initiate a read.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2,
* 4 for MXP.
* @param[in] dataToSend Pointer to the data to send (Gets copied for continue
* use, so no need to keep alive).
* @param[in] dataSize The length of the data to send.
* @param[in] zeroSize The number of zeros to send after the data.
* @param[out] status the error code, or 0 for success
*/
void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t dataSize, int32_t zeroSize,
int32_t* status);
/**
* Immediately forces an SPI read to happen.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP.
* @param[out] status the error code, or 0 for success
*/
void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status);
/**
* Reads data received by the SPI accumulator. 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 dataSize + zeroSize set in
* HAL_SetSPIAutoTransmitData.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2,
* 4 for MXP.
* @param[out] buffer The buffer to store the data into.
* @param[in] numToRead The number of words to read.
* @param[in] timeout The read timeout (in seconds).
* @param[out] status the error code, or 0 for success
* @return The number of words actually read.
*/
int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
int32_t numToRead, double timeout,
int32_t* status);
/**
* Gets the count of how many SPI accumulations have been missed.
*
* @param[in] port The number of the port to use. 0-3 for Onboard CS0-CS2, 4
* for MXP.
* @param[out] status the error code, or 0 for success
* @return The number of missed accumulations.
*/
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status);
/**
* Configure the Auto SPI Stall time between reads.
*
* @param[in] port The number of the port to use. 0-3 for Onboard
* CS0-CS2, 4 for MXP.
* @param[in] csToSclkTicks the number of ticks to wait before asserting the
* cs pin
* @param[in] stallTicks the number of ticks to stall for
* @param[in] pow2BytesPerRead the number of bytes to read before stalling
* @param[out] status the error code, or 0 for success
*/
void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
int32_t stallTicks, int32_t pow2BytesPerRead,
int32_t* status);
#ifdef __cplusplus
} // extern "C"
#endif
/** @} */

44
hal/src/main/native/include/hal/SPITypes.h
View File

@@ -1,44 +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/Types.h"
/**
* @defgroup hal_spi SPI Functions
* @ingroup hal_capi
* @{
*/
/** SPI port. */
HAL_ENUM(HAL_SPIPort) {
/** Invalid port number. */
HAL_SPI_kInvalid = -1,
/** Onboard SPI bus port CS0. */
HAL_SPI_kOnboardCS0,
/** Onboard SPI bus port CS1. */
HAL_SPI_kOnboardCS1,
/** Onboard SPI bus port CS2. */
HAL_SPI_kOnboardCS2,
/** Onboard SPI bus port CS3. */
HAL_SPI_kOnboardCS3,
/** MXP (roboRIO MXP) SPI bus port. */
HAL_SPI_kMXP
};
/** SPI mode. */
HAL_ENUM(HAL_SPIMode) {
/** Clock idle low, data sampled on rising edge. */
HAL_SPI_kMode0 = 0,
/** Clock idle low, data sampled on falling edge. */
HAL_SPI_kMode1 = 1,
/** Clock idle high, data sampled on falling edge. */
HAL_SPI_kMode2 = 2,
/** Clock idle high, data sampled on rising edge. */
HAL_SPI_kMode3 = 3,
};
/** @} */

22
hal/src/main/native/include/hal/handles/HandlesInternal.h
View File

@@ -181,20 +181,6 @@ inline int16_t getPortHandleModule(HAL_PortHandle handle) {
return static_cast<uint8_t>((handle >> 8) & 0xff);
}
// using a 16 bit value so we can store 0-255 and still report error
/**
* Gets the SPI channel of a port handle.
*
* @param handle the port handle
* @return the port SPI channel
*/
inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) {
if (!isHandleType(handle, HAL_HandleEnum::Port)) {
return InvalidHandleIndex;
}
return static_cast<uint8_t>((handle >> 16) & 0xff);
}
/**
* Create a port handle.
*
@@ -204,14 +190,6 @@ inline int16_t getPortHandleSPIEnable(HAL_PortHandle handle) {
*/
HAL_PortHandle createPortHandle(uint8_t channel, uint8_t module);
/**
* Create a port handle for SPI.
*
* @param channel the SPI channel
* @return port handle for the channel
*/
HAL_PortHandle createPortHandleForSPI(uint8_t channel);
/**
* Create a handle for a specific index, type and version.
*

60
hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h
View File

@@ -1,60 +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/Types.h"
#include "hal/simulation/NotifyListener.h"
#ifdef __cplusplus
extern "C" {
#endif
void HALSIM_ResetSPIAccelerometerData(int32_t index);
int32_t HALSIM_RegisterSPIAccelerometerActiveCallback(
int32_t index, HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIAccelerometerActiveCallback(int32_t index, int32_t uid);
HAL_Bool HALSIM_GetSPIAccelerometerActive(int32_t index);
void HALSIM_SetSPIAccelerometerActive(int32_t index, HAL_Bool active);
int32_t HALSIM_RegisterSPIAccelerometerRangeCallback(
int32_t index, HAL_NotifyCallback callback, void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIAccelerometerRangeCallback(int32_t index, int32_t uid);
int32_t HALSIM_GetSPIAccelerometerRange(int32_t index);
void HALSIM_SetSPIAccelerometerRange(int32_t index, int32_t range);
int32_t HALSIM_RegisterSPIAccelerometerXCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIAccelerometerXCallback(int32_t index, int32_t uid);
double HALSIM_GetSPIAccelerometerX(int32_t index);
void HALSIM_SetSPIAccelerometerX(int32_t index, double x);
int32_t HALSIM_RegisterSPIAccelerometerYCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIAccelerometerYCallback(int32_t index, int32_t uid);
double HALSIM_GetSPIAccelerometerY(int32_t index);
void HALSIM_SetSPIAccelerometerY(int32_t index, double y);
int32_t HALSIM_RegisterSPIAccelerometerZCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIAccelerometerZCallback(int32_t index, int32_t uid);
double HALSIM_GetSPIAccelerometerZ(int32_t index);
void HALSIM_SetSPIAccelerometerZ(int32_t index, double z);
void HALSIM_RegisterSPIAccelerometerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
#ifdef __cplusplus
} // extern "C"
#endif

46
hal/src/main/native/include/hal/simulation/SPIData.h
View File

@@ -1,46 +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/Types.h"
#include "hal/simulation/NotifyListener.h"
typedef void (*HAL_SpiReadAutoReceiveBufferCallback)(const char* name,
void* param,
uint32_t* buffer,
int32_t numToRead,
int32_t* outputCount);
#ifdef __cplusplus
extern "C" {
#endif
void HALSIM_ResetSPIData(int32_t index);
int32_t HALSIM_RegisterSPIInitializedCallback(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify);
void HALSIM_CancelSPIInitializedCallback(int32_t index, int32_t uid);
HAL_Bool HALSIM_GetSPIInitialized(int32_t index);
void HALSIM_SetSPIInitialized(int32_t index, HAL_Bool initialized);
int32_t HALSIM_RegisterSPIReadCallback(int32_t index,
HAL_BufferCallback callback,
void* param);
void HALSIM_CancelSPIReadCallback(int32_t index, int32_t uid);
int32_t HALSIM_RegisterSPIWriteCallback(int32_t index,
HAL_ConstBufferCallback callback,
void* param);
void HALSIM_CancelSPIWriteCallback(int32_t index, int32_t uid);
int32_t HALSIM_RegisterSPIReadAutoReceivedDataCallback(
int32_t index, HAL_SpiReadAutoReceiveBufferCallback callback, void* param);
void HALSIM_CancelSPIReadAutoReceivedDataCallback(int32_t index, int32_t uid);
#ifdef __cplusplus
} // extern "C"
#endif

3
hal/src/main/native/sim/HAL.cpp
View File

@@ -85,8 +85,6 @@ void InitializeHAL() {
InitializePWMData();
InitializeRoboRioData();
InitializeSimDeviceData();
InitializeSPIAccelerometerData();
InitializeSPIData();
InitializeAccelerometer();
InitializeAddressableLED();
InitializeAnalogAccumulator();
@@ -116,7 +114,6 @@ void InitializeHAL() {
InitializePWM();
InitializeSerialPort();
InitializeSimDevice();
InitializeSPI();
InitializeThreads();
}
} // namespace hal::init

3
hal/src/main/native/sim/HALInitializer.h
View File

@@ -36,8 +36,6 @@ extern void InitializePowerDistributionData();
extern void InitializePWMData();
extern void InitializeRoboRioData();
extern void InitializeSimDeviceData();
extern void InitializeSPIAccelerometerData();
extern void InitializeSPIData();
extern void InitializeAccelerometer();
extern void InitializeAddressableLED();
extern void InitializeAnalogAccumulator();
@@ -67,7 +65,6 @@ extern void InitializeREVPH();
extern void InitializePWM();
extern void InitializeSerialPort();
extern void InitializeSimDevice();
extern void InitializeSPI();
extern void InitializeThreads();
} // namespace hal::init

74
hal/src/main/native/sim/SPI.cpp
View File

@@ -1,74 +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 "hal/SPI.h"
#include "HALInitializer.h"
#include "mockdata/SPIDataInternal.h"
using namespace hal;
namespace hal::init {
void InitializeSPI() {}
} // namespace hal::init
extern "C" {
void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
hal::init::CheckInit();
SimSPIData[port].initialized = true;
}
int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
uint8_t* dataReceived, int32_t size) {
return SimSPIData[port].Transaction(dataToSend, dataReceived, size);
}
int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t sendSize) {
return SimSPIData[port].Write(dataToSend, sendSize);
}
int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
return SimSPIData[port].Read(buffer, count);
}
void HAL_CloseSPI(HAL_SPIPort port) {
SimSPIData[port].initialized = false;
}
void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {}
void HAL_SetSPIMode(HAL_SPIPort port, HAL_SPIMode mode) {}
HAL_SPIMode HAL_GetSPIMode(HAL_SPIPort port) {
return HAL_SPI_kMode0;
}
void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {}
void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {}
int32_t HAL_GetSPIHandle(HAL_SPIPort port) {
return 0;
}
void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {}
void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {}
void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {}
void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {}
void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool triggerRising, HAL_Bool triggerFalling,
int32_t* status) {}
void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {}
void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t dataSize, int32_t zeroSize,
int32_t* status) {}
void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {}
int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
int32_t numToRead, double timeout,
int32_t* status) {
return SimSPIData[port].ReadAutoReceivedData(buffer, numToRead, timeout,
status);
}
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
return 0;
}
void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
int32_t stallTicks, int32_t pow2BytesPerRead,
int32_t* status) {}
} // extern "C"

10
hal/src/main/native/sim/mockdata/Reset.cpp
View File

@@ -19,8 +19,6 @@
#include <hal/simulation/PowerDistributionData.h>
#include <hal/simulation/REVPHData.h>
#include <hal/simulation/RoboRioData.h>
#include <hal/simulation/SPIAccelerometerData.h>
#include <hal/simulation/SPIData.h>
#include <hal/simulation/SimDeviceData.h>
#include "../PortsInternal.h"
@@ -88,12 +86,4 @@ extern "C" void HALSIM_ResetAllSimData(void) {
HALSIM_ResetRoboRioData();
HALSIM_ResetSimDeviceData();
for (int32_t i = 0; i < hal::kSPIAccelerometers; i++) {
HALSIM_ResetSPIAccelerometerData(i);
}
for (int32_t i = 0; i < hal::kSPIPorts; i++) {
HALSIM_ResetSPIData(i);
}
}

55
hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp
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.
#include "../PortsInternal.h"
#include "SPIAccelerometerDataInternal.h"
using namespace hal;
namespace hal::init {
void InitializeSPIAccelerometerData() {
static SPIAccelerometerData ssad[kSPIAccelerometers];
::hal::SimSPIAccelerometerData = ssad;
}
} // namespace hal::init
SPIAccelerometerData* hal::SimSPIAccelerometerData;
void SPIAccelerometerData::ResetData() {
active.Reset(false);
range.Reset(0);
x.Reset(0.0);
y.Reset(0.0);
z.Reset(0.0);
}
extern "C" {
void HALSIM_ResetSPIAccelerometerData(int32_t index) {
SimSPIAccelerometerData[index].ResetData();
}
#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, \
SimSPIAccelerometerData, LOWERNAME)
DEFINE_CAPI(HAL_Bool, Active, active)
DEFINE_CAPI(int32_t, Range, range)
DEFINE_CAPI(double, X, x)
DEFINE_CAPI(double, Y, y)
DEFINE_CAPI(double, Z, z)
#define REGISTER(NAME) \
SimSPIAccelerometerData[index].NAME.RegisterCallback(callback, param, \
initialNotify)
void HALSIM_RegisterSPIAccelerometerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {
REGISTER(active);
REGISTER(range);
REGISTER(x);
REGISTER(y);
REGISTER(z);
}
} // extern "C"

28
hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h
View File

@@ -1,28 +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/simulation/SPIAccelerometerData.h"
#include "hal/simulation/SimDataValue.h"
namespace hal {
class SPIAccelerometerData {
HAL_SIMDATAVALUE_DEFINE_NAME(Active)
HAL_SIMDATAVALUE_DEFINE_NAME(Range)
HAL_SIMDATAVALUE_DEFINE_NAME(X)
HAL_SIMDATAVALUE_DEFINE_NAME(Y)
HAL_SIMDATAVALUE_DEFINE_NAME(Z)
public:
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetActiveName> active{false};
SimDataValue<int32_t, HAL_MakeInt, GetRangeName> range{0};
SimDataValue<double, HAL_MakeDouble, GetXName> x{0.0};
SimDataValue<double, HAL_MakeDouble, GetYName> y{0.0};
SimDataValue<double, HAL_MakeDouble, GetZName> z{0.0};
virtual void ResetData();
};
extern SPIAccelerometerData* SimSPIAccelerometerData;
} // namespace hal

70
hal/src/main/native/sim/mockdata/SPIData.cpp
View File

@@ -1,70 +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 "../PortsInternal.h"
#include "SPIDataInternal.h"
using namespace hal;
namespace hal::init {
void InitializeSPIData() {
static SPIData ssd[kSPIPorts];
::hal::SimSPIData = ssd;
}
} // namespace hal::init
SPIData* hal::SimSPIData;
void SPIData::ResetData() {
initialized.Reset(false);
read.Reset();
write.Reset();
autoReceivedData.Reset();
}
int32_t SPIData::Read(uint8_t* buffer, int32_t count) {
read(buffer, count);
return count;
}
int32_t SPIData::Write(const uint8_t* dataToSend, int32_t sendSize) {
write(dataToSend, sendSize);
return sendSize;
}
int32_t SPIData::Transaction(const uint8_t* dataToSend, uint8_t* dataReceived,
int32_t size) {
write(dataToSend, size);
read(dataReceived, size);
return size;
}
int32_t SPIData::ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead,
double timeout, int32_t* status) {
int32_t outputCount = 0;
autoReceivedData(buffer, numToRead, &outputCount);
return outputCount;
}
extern "C" {
void HALSIM_ResetSPIData(int32_t index) {
SimSPIData[index].ResetData();
}
#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
HAL_SIMDATAVALUE_DEFINE_CAPI(TYPE, HALSIM, SPI##CAPINAME, SimSPIData, \
LOWERNAME)
DEFINE_CAPI(HAL_Bool, Initialized, initialized)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME, LOWERNAME) \
HAL_SIMCALLBACKREGISTRY_DEFINE_CAPI(TYPE, HALSIM, SPI##CAPINAME, SimSPIData, \
LOWERNAME)
DEFINE_CAPI(HAL_BufferCallback, Read, read)
DEFINE_CAPI(HAL_ConstBufferCallback, Write, write)
DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData,
autoReceivedData)
} // extern "C"

37
hal/src/main/native/sim/mockdata/SPIDataInternal.h
View File

@@ -1,37 +0,0 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include "hal/simulation/SPIData.h"
#include "hal/simulation/SimCallbackRegistry.h"
#include "hal/simulation/SimDataValue.h"
namespace hal {
class SPIData {
HAL_SIMDATAVALUE_DEFINE_NAME(Initialized)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Read)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(Write)
HAL_SIMCALLBACKREGISTRY_DEFINE_NAME(AutoReceive)
public:
int32_t Read(uint8_t* buffer, int32_t count);
int32_t Write(const uint8_t* dataToSend, int32_t sendSize);
int32_t Transaction(const uint8_t* dataToSend, uint8_t* dataReceived,
int32_t size);
int32_t ReadAutoReceivedData(uint32_t* buffer, int32_t numToRead,
double timeout, int32_t* status);
SimDataValue<HAL_Bool, HAL_MakeBoolean, GetInitializedName> initialized{
false};
SimCallbackRegistry<HAL_BufferCallback, GetReadName> read;
SimCallbackRegistry<HAL_ConstBufferCallback, GetWriteName> write;
SimCallbackRegistry<HAL_SpiReadAutoReceiveBufferCallback, GetAutoReceiveName>
autoReceivedData;
void ResetData();
};
extern SPIData* SimSPIData;
} // namespace hal

1
hal/src/main/native/systemcore/HAL.cpp
View File

@@ -75,7 +75,6 @@ void InitializeHAL() {
InitializePWM();
InitializeSerialPort();
InitializeSmartIo();
InitializeSPI();
InitializeThreads();
}
} // namespace init

1
hal/src/main/native/systemcore/HALInitializer.h
View File

@@ -49,6 +49,5 @@ extern void InitializePower();
extern void InitializePWM();
extern void InitializeSerialPort();
extern void InitializeSmartIo();
extern void InitializeSPI();
extern void InitializeThreads();
} // namespace hal::init

141
hal/src/main/native/systemcore/SPI.cpp
View File

@@ -1,141 +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 "hal/SPI.h"
#include <fcntl.h>
#include <linux/spi/spidev.h>
#include <sys/ioctl.h>
#include <unistd.h>
#include <array>
#include <atomic>
#include <cstdio>
#include <cstring>
#include <memory>
#include <fmt/format.h>
#include <wpi/mutex.h>
#include <wpi/print.h>
#include "HALInitializer.h"
#include "HALInternal.h"
#include "hal/DIO.h"
#include "hal/HAL.h"
#include "hal/handles/HandlesInternal.h"
using namespace hal;
namespace hal::init {
void InitializeSPI() {}
} // namespace hal::init
extern "C" {
void HAL_InitializeSPI(HAL_SPIPort port, int32_t* status) {
hal::init::CheckInit();
*status = HAL_HANDLE_ERROR;
return;
}
int32_t HAL_TransactionSPI(HAL_SPIPort port, const uint8_t* dataToSend,
uint8_t* dataReceived, int32_t size) {
return -1;
}
int32_t HAL_WriteSPI(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t sendSize) {
return -1;
}
int32_t HAL_ReadSPI(HAL_SPIPort port, uint8_t* buffer, int32_t count) {
return -1;
}
void HAL_CloseSPI(HAL_SPIPort port) {}
void HAL_SetSPISpeed(HAL_SPIPort port, int32_t speed) {}
void HAL_SetSPIMode(HAL_SPIPort port, HAL_SPIMode mode) {}
HAL_SPIMode HAL_GetSPIMode(HAL_SPIPort port) {
return HAL_SPI_kMode0;
}
void HAL_SetSPIChipSelectActiveHigh(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_SetSPIChipSelectActiveLow(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t HAL_GetSPIHandle(HAL_SPIPort port) {
return 0;
}
void HAL_SetSPIHandle(HAL_SPIPort port, int32_t handle) {}
void HAL_InitSPIAuto(HAL_SPIPort port, int32_t bufferSize, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_FreeSPIAuto(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_StartSPIAutoRate(HAL_SPIPort port, double period, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_StartSPIAutoTrigger(HAL_SPIPort port, HAL_Handle digitalSourceHandle,
HAL_AnalogTriggerType analogTriggerType,
HAL_Bool triggerRising, HAL_Bool triggerFalling,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_StopSPIAuto(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_SetSPIAutoTransmitData(HAL_SPIPort port, const uint8_t* dataToSend,
int32_t dataSize, int32_t zeroSize,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
void HAL_ForceSPIAutoRead(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
int32_t HAL_ReadSPIAutoReceivedData(HAL_SPIPort port, uint32_t* buffer,
int32_t numToRead, double timeout,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return 0;
}
int32_t HAL_GetSPIAutoDroppedCount(HAL_SPIPort port, int32_t* status) {
*status = HAL_HANDLE_ERROR;
return 0;
}
void HAL_ConfigureSPIAutoStall(HAL_SPIPort port, int32_t csToSclkTicks,
int32_t stallTicks, int32_t pow2BytesPerRead,
int32_t* status) {
*status = HAL_HANDLE_ERROR;
return;
}
} // extern "C"

25
hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp
View File

@@ -1,25 +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 "hal/simulation/SPIAccelerometerData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetSPIAccelerometerData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPIAccelerometer##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Active, false)
DEFINE_CAPI(int32_t, Range, 0)
DEFINE_CAPI(double, X, 0)
DEFINE_CAPI(double, Y, 0)
DEFINE_CAPI(double, Z, 0)
void HALSIM_RegisterSPIAccelerometerAllCallbacks(int32_t index,
HAL_NotifyCallback callback,
void* param,
HAL_Bool initialNotify) {}
} // extern "C"

25
hal/src/main/native/systemcore/mockdata/SPIData.cpp
View File

@@ -1,25 +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 "hal/simulation/SPIData.h"
#include "hal/simulation/SimDataValue.h"
extern "C" {
void HALSIM_ResetSPIData(int32_t index) {}
#define DEFINE_CAPI(TYPE, CAPINAME, RETURN) \
HAL_SIMDATAVALUE_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME, RETURN)
DEFINE_CAPI(HAL_Bool, Initialized, false)
#undef DEFINE_CAPI
#define DEFINE_CAPI(TYPE, CAPINAME) \
HAL_SIMCALLBACKREGISTRY_STUB_CAPI(TYPE, HALSIM, SPI##CAPINAME)
DEFINE_CAPI(HAL_BufferCallback, Read)
DEFINE_CAPI(HAL_ConstBufferCallback, Write)
DEFINE_CAPI(HAL_SpiReadAutoReceiveBufferCallback, ReadAutoReceivedData)
} // extern "C"

40
hal/src/test/native/cpp/mockdata/SPIDataTest.cpp
View File

@@ -1,40 +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 <string>
#include <gtest/gtest.h>
#include "hal/SPI.h"
#include "hal/simulation/SPIData.h"
namespace hal {
std::string gTestSpiCallbackName;
HAL_Value gTestSpiCallbackValue;
void TestSpiInitializationCallback(const char* name, void* param,
const struct HAL_Value* value) {
gTestSpiCallbackName = name;
gTestSpiCallbackValue = *value;
}
TEST(SpiSimTest, SpiInitialization) {
const int INDEX_TO_TEST = 2;
int32_t status = 0;
HAL_SPIPort port;
int callbackParam = 0;
int callbackId = HALSIM_RegisterSPIInitializedCallback(
INDEX_TO_TEST, &TestSpiInitializationCallback, &callbackParam, false);
ASSERT_TRUE(0 != callbackId);
port = HAL_SPI_kOnboardCS2;
gTestSpiCallbackName = "Unset";
HAL_InitializeSPI(port, &status);
EXPECT_STREQ("Initialized", gTestSpiCallbackName.c_str());
}
} // namespace hal

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

63
wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp
View File

@@ -1,63 +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/ADXL345Sim.h" // NOLINT(build/include_order)
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include "frc/ADXL345_I2C.h"
#include "frc/ADXL345_SPI.h"
namespace frc::sim {
TEST(ADXL345SimTest, SetSpiAttributes) {
HAL_Initialize(500, 0);
ADXL345_SPI accel(SPI::kMXP, ADXL345_SPI::kRange_2G);
ADXL345Sim sim(accel);
EXPECT_EQ(0, accel.GetX());
EXPECT_EQ(0, accel.GetY());
EXPECT_EQ(0, accel.GetZ());
sim.SetX(1.91);
sim.SetY(-3.405);
sim.SetZ(2.29);
EXPECT_EQ(1.91, accel.GetX());
EXPECT_EQ(-3.405, accel.GetY());
EXPECT_EQ(2.29, accel.GetZ());
ADXL345_SPI::AllAxes allAccel = accel.GetAccelerations();
EXPECT_EQ(1.91, allAccel.XAxis);
EXPECT_EQ(-3.405, allAccel.YAxis);
EXPECT_EQ(2.29, allAccel.ZAxis);
}
TEST(ADXL345SimTest, SetI2CAttribute) {
HAL_Initialize(500, 0);
ADXL345_I2C accel(I2C::kMXP);
ADXL345Sim sim(accel);
EXPECT_EQ(0, accel.GetX());
EXPECT_EQ(0, accel.GetY());
EXPECT_EQ(0, accel.GetZ());
sim.SetX(1.91);
sim.SetY(-3.405);
sim.SetZ(2.29);
EXPECT_EQ(1.91, accel.GetX());
EXPECT_EQ(-3.405, accel.GetY());
EXPECT_EQ(2.29, accel.GetZ());
ADXL345_I2C::AllAxes allAccel = accel.GetAccelerations();
EXPECT_EQ(1.91, allAccel.XAxis);
EXPECT_EQ(-3.405, allAccel.YAxis);
EXPECT_EQ(2.29, allAccel.ZAxis);
}
} // namespace frc::sim

38
wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp
View File

@@ -1,38 +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" // NOLINT(build/include_order)
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include "frc/ADXL362.h"
namespace frc::sim {
TEST(ADXL362SimTest, SetAttributes) {
HAL_Initialize(500, 0);
ADXL362 accel(SPI::kMXP, ADXL362::kRange_2G);
ADXL362Sim sim(accel);
EXPECT_EQ(0, accel.GetX());
EXPECT_EQ(0, accel.GetY());
EXPECT_EQ(0, accel.GetZ());
sim.SetX(1.91);
sim.SetY(-3.405);
sim.SetZ(2.29);
EXPECT_EQ(1.91, accel.GetX());
EXPECT_EQ(-3.405, accel.GetY());
EXPECT_EQ(2.29, accel.GetZ());
ADXL362::AllAxes allAccel = accel.GetAccelerations();
EXPECT_EQ(1.91, allAccel.XAxis);
EXPECT_EQ(-3.405, allAccel.YAxis);
EXPECT_EQ(2.29, allAccel.ZAxis);
}
} // namespace frc::sim

35
wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp
View File

@@ -1,35 +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" // NOLINT(build/include_order)
#include <gtest/gtest.h>
#include <hal/HAL.h>
#include "frc/ADXRS450_Gyro.h"
namespace frc::sim {
TEST(ADXRS450GyroSimTest, SetAttributes) {
HAL_Initialize(500, 0);
ADXRS450_Gyro gyro;
ADXRS450_GyroSim sim{gyro};
EXPECT_EQ(0, gyro.GetAngle());
EXPECT_EQ(0, gyro.GetRate());
constexpr units::degree_t TEST_ANGLE{123.456};
constexpr units::degrees_per_second_t TEST_RATE{229.3504};
sim.SetAngle(TEST_ANGLE);
sim.SetRate(TEST_RATE);
EXPECT_EQ(TEST_ANGLE.value(), gyro.GetAngle());
EXPECT_EQ(TEST_RATE.value(), gyro.GetRate());
gyro.Reset();
EXPECT_EQ(0, gyro.GetAngle());
}
} // namespace frc::sim

2
wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp
View File

@@ -21,7 +21,6 @@
#include "frc/simulation/PWMSim.h"
#include "frc/simulation/PowerDistributionSim.h"
#include "frc/simulation/RoboRioSim.h"
#include "frc/simulation/SPIAccelerometerSim.h"
using namespace frc::sim;
@@ -42,7 +41,6 @@ TEST(SimInitializationTest, AllInitialize) {
PWMSim pwmsim{0};
RoboRioSim rrsim;
(void)rrsim;
SPIAccelerometerSim sasim{0};
DutyCycleSim dcsim = DutyCycleSim::CreateForIndex(0);
(void)dcsim;
AddressableLEDSim adLED;

4
wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <frc/ADXRS450_Gyro.h>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/drive/MecanumDrive.h>
#include <frc/geometry/Pose2d.h>
@@ -165,7 +165,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
frc::Encoder m_rearRightEncoder;
// The gyro sensor
frc::ADXRS450_Gyro m_gyro;
frc::AnalogGyro m_gyro{0};
// Odometry class for tracking robot pose
frc::MecanumDriveOdometry m_odometry;

4
wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h
View File

@@ -6,7 +6,7 @@
#include <functional>
#include <frc/ADXRS450_Gyro.h>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/controller/ProfiledPIDController.h>
#include <frc/controller/SimpleMotorFeedforward.h>
@@ -68,7 +68,7 @@ class Drive : public frc2::SubsystemBase {
DriveConstants::kRightEncoderPorts[1],
DriveConstants::kRightEncoderReversed};
frc::ADXRS450_Gyro m_gyro;
frc::AnalogGyro m_gyro{0};
frc::ProfiledPIDController<units::radians> m_controller{
DriveConstants::kTurnP,

4
wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h
View File

@@ -4,7 +4,7 @@
#pragma once
#include <frc/ADXRS450_Gyro.h>
#include <frc/AnalogGyro.h>
#include <frc/Encoder.h>
#include <frc/drive/MecanumDrive.h>
#include <frc/geometry/Pose2d.h>
@@ -109,7 +109,7 @@ class DriveSubsystem : public frc2::SubsystemBase {
SwerveModule m_rearRight;
// The gyro sensor
frc::ADXRS450_Gyro m_gyro;
frc::AnalogGyro m_gyro{0};
// Odometry class for tracking robot pose
// 4 defines the number of modules

1078
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java
View File

File diff suppressed because it is too large Load Diff

1084
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java
View File

File diff suppressed because it is too large Load Diff

289
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java
View File

@@ -1,289 +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.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tInstances;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleTopic;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/** ADXL345 SPI Accelerometer. */
@SuppressWarnings("TypeName")
public class ADXL345_SPI implements NTSendable, AutoCloseable {
private static final int kPowerCtlRegister = 0x2D;
private static final int kDataFormatRegister = 0x31;
private static final int kDataRegister = 0x32;
private static final double kGsPerLSB = 0.00390625;
private static final int kAddress_Read = 0x80;
private static final int kAddress_MultiByte = 0x40;
// private static final int kPowerCtl_Link = 0x20;
// private static final int kPowerCtl_AutoSleep = 0x10;
private static final int kPowerCtl_Measure = 0x08;
// private static final int kPowerCtl_Sleep = 0x04;
// private static final int kDataFormat_SelfTest = 0x80;
// private static final int kDataFormat_SPI = 0x40;
// private static final int kDataFormat_IntInvert = 0x20;
private static final int kDataFormat_FullRes = 0x08;
// private static final int kDataFormat_Justify = 0x04;
/** Accelerometer range. */
public enum Range {
/** 2 Gs max. */
k2G,
/** 4 Gs max. */
k4G,
/** 8 Gs max. */
k8G,
/** 16 Gs max. */
k16G
}
/** Accelerometer axes. */
public enum Axes {
/** X axis. */
kX((byte) 0x00),
/** Y axis. */
kY((byte) 0x02),
/** Z axis. */
kZ((byte) 0x04);
/** The integer value representing this enumeration. */
public final byte value;
Axes(byte value) {
this.value = value;
}
}
/** Container type for accelerations from all axes. */
@SuppressWarnings("MemberName")
public static class AllAxes {
/** Acceleration along the X axis in g-forces. */
public double XAxis;
/** Acceleration along the Y axis in g-forces. */
public double YAxis;
/** Acceleration along the Z axis in g-forces. */
public double ZAxis;
/** Default constructor. */
public AllAxes() {}
}
private SPI m_spi;
private SimDevice m_simDevice;
private SimEnum m_simRange;
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param port The SPI port that the accelerometer is connected to
* @param range The range (+ or -) that the accelerometer will measure.
*/
@SuppressWarnings("this-escape")
public ADXL345_SPI(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Accel:ADXL345_SPI", port.value);
if (m_simDevice != null) {
m_simRange =
m_simDevice.createEnumDouble(
"range",
SimDevice.Direction.kOutput,
new String[] {"2G", "4G", "8G", "16G"},
new double[] {2.0, 4.0, 8.0, 16.0},
0);
m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
}
init(range);
SendableRegistry.addLW(this, "ADXL345_SPI", port.value);
}
/**
* Returns the SPI port.
*
* @return The SPI port.
*/
public int getPort() {
return m_spi.getPort();
}
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Set SPI bus parameters, bring device out of sleep and set format.
*
* @param range The range (+ or -) that the accelerometer will measure.
*/
private void init(Range range) {
m_spi.setClockRate(500000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveHigh();
// Turn on the measurements
byte[] commands = new byte[2];
commands[0] = kPowerCtlRegister;
commands[1] = kPowerCtl_Measure;
m_spi.write(commands, 2);
setRange(range);
HAL.report(tResourceType.kResourceType_ADXL345, tInstances.kADXL345_SPI);
}
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the accelerometer will
* measure.
*/
public void setRange(Range range) {
final byte value =
switch (range) {
case k2G -> 0;
case k4G -> 1;
case k8G -> 2;
case k16G -> 3;
};
// Specify the data format to read
byte[] commands = new byte[] {kDataFormatRegister, (byte) (kDataFormat_FullRes | value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
public double getX() {
return getAcceleration(Axes.kX);
}
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
public double getY() {
return getAcceleration(Axes.kY);
}
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
public double getZ() {
return getAcceleration(Axes.kZ);
}
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL345 in Gs.
*/
public double getAcceleration(ADXL345_SPI.Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
transferBuffer.put(
0, (byte) ((kAddress_Read | kAddress_MultiByte | kDataRegister) + axis.value));
m_spi.transaction(transferBuffer, transferBuffer, 3);
// Sensor is little endian
transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
return transferBuffer.getShort(1) * kGsPerLSB;
}
/**
* Get the acceleration of all axes in Gs.
*
* @return An object containing the acceleration measured on each axis of the ADXL345 in Gs.
*/
public ADXL345_SPI.AllAxes getAccelerations() {
ADXL345_SPI.AllAxes data = new ADXL345_SPI.AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(7);
// Select the data address.
dataBuffer.put(0, (byte) (kAddress_Read | kAddress_MultiByte | kDataRegister));
m_spi.transaction(dataBuffer, dataBuffer, 7);
// Sensor is little endian... swap bytes
dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
data.XAxis = dataBuffer.getShort(1) * kGsPerLSB;
data.YAxis = dataBuffer.getShort(3) * kGsPerLSB;
data.ZAxis = dataBuffer.getShort(5) * kGsPerLSB;
}
return data;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("3AxisAccelerometer");
DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
builder.addCloseable(pubX);
builder.addCloseable(pubY);
builder.addCloseable(pubZ);
builder.setUpdateTable(
() -> {
AllAxes data = getAccelerations();
pubX.set(data.XAxis);
pubY.set(data.YAxis);
pubZ.set(data.ZAxis);
});
}
}

324
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java
View File

@@ -1,324 +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.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.hal.SimEnum;
import edu.wpi.first.networktables.DoublePublisher;
import edu.wpi.first.networktables.DoubleTopic;
import edu.wpi.first.networktables.NTSendable;
import edu.wpi.first.networktables.NTSendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/**
* ADXL362 SPI Accelerometer.
*
* <p>This class allows access to an Analog Devices ADXL362 3-axis accelerometer.
*/
public class ADXL362 implements NTSendable, AutoCloseable {
private static final byte kRegWrite = 0x0A;
private static final byte kRegRead = 0x0B;
private static final byte kPartIdRegister = 0x02;
private static final byte kDataRegister = 0x0E;
private static final byte kFilterCtlRegister = 0x2C;
private static final byte kPowerCtlRegister = 0x2D;
private static final byte kFilterCtl_Range2G = 0x00;
private static final byte kFilterCtl_Range4G = 0x40;
private static final byte kFilterCtl_Range8G = (byte) 0x80;
private static final byte kFilterCtl_ODR_100Hz = 0x03;
private static final byte kPowerCtl_UltraLowNoise = 0x20;
// @SuppressWarnings("PMD.UnusedPrivateField")
// private static final byte kPowerCtl_AutoSleep = 0x04;
private static final byte kPowerCtl_Measure = 0x02;
/** Accelerometer range. */
public enum Range {
/** 2 Gs max. */
k2G,
/** 4 Gs max. */
k4G,
/** 8 Gs max. */
k8G
}
/** Accelerometer axes. */
public enum Axes {
/** X axis. */
kX((byte) 0x00),
/** Y axis. */
kY((byte) 0x02),
/** Z axis. */
kZ((byte) 0x04);
/** Axis value. */
public final byte value;
Axes(byte value) {
this.value = value;
}
}
/** Container type for accelerations from all axes. */
@SuppressWarnings("MemberName")
public static class AllAxes {
/** Acceleration along the X axis in g-forces. */
public double XAxis;
/** Acceleration along the Y axis in g-forces. */
public double YAxis;
/** Acceleration along the Z axis in g-forces. */
public double ZAxis;
/** Default constructor. */
public AllAxes() {}
}
private SPI m_spi;
private SimDevice m_simDevice;
private SimEnum m_simRange;
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
private double m_gsPerLSB;
/**
* Constructor. Uses the onboard CS1.
*
* @param range The range (+ or -) that the accelerometer will measure.
*/
public ADXL362(Range range) {
this(SPI.Port.kOnboardCS1, range);
}
/**
* Constructor.
*
* @param port The SPI port that the accelerometer is connected to
* @param range The range (+ or -) that the accelerometer will measure.
*/
@SuppressWarnings("this-escape")
public ADXL362(SPI.Port port, Range range) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Accel:ADXL362", port.value);
if (m_simDevice != null) {
m_simRange =
m_simDevice.createEnumDouble(
"range",
SimDevice.Direction.kOutput,
new String[] {"2G", "4G", "8G", "16G"},
new double[] {2.0, 4.0, 8.0, 16.0},
0);
m_simX = m_simDevice.createDouble("x", SimDevice.Direction.kInput, 0.0);
m_simY = m_simDevice.createDouble("y", SimDevice.Direction.kInput, 0.0);
m_simZ = m_simDevice.createDouble("z", SimDevice.Direction.kInput, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMode(SPI.Mode.kMode3);
m_spi.setChipSelectActiveLow();
ByteBuffer transferBuffer = ByteBuffer.allocate(3);
if (m_simDevice == null) {
// Validate the part ID
transferBuffer.put(0, kRegRead);
transferBuffer.put(1, kPartIdRegister);
m_spi.transaction(transferBuffer, transferBuffer, 3);
if (transferBuffer.get(2) != (byte) 0xF2) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXL362 on SPI port " + port.value, false);
return;
}
}
setRange(range);
// Turn on the measurements
transferBuffer.put(0, kRegWrite);
transferBuffer.put(1, kPowerCtlRegister);
transferBuffer.put(2, (byte) (kPowerCtl_Measure | kPowerCtl_UltraLowNoise));
m_spi.write(transferBuffer, 3);
HAL.report(tResourceType.kResourceType_ADXL362, port.value + 1);
SendableRegistry.addLW(this, "ADXL362", port.value);
}
/**
* Returns the SPI port.
*
* @return The SPI port.
*/
public int getPort() {
return m_spi.getPort();
}
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Set the measuring range of the accelerometer.
*
* @param range The maximum acceleration, positive or negative, that the accelerometer will
* measure.
*/
public final void setRange(Range range) {
if (m_spi == null) {
return;
}
final byte value =
switch (range) {
case k2G -> {
m_gsPerLSB = 0.001;
yield kFilterCtl_Range2G;
}
case k4G -> {
m_gsPerLSB = 0.002;
yield kFilterCtl_Range4G;
}
case k8G -> {
m_gsPerLSB = 0.004;
yield kFilterCtl_Range8G;
}
};
// Specify the data format to read
byte[] commands =
new byte[] {kRegWrite, kFilterCtlRegister, (byte) (kFilterCtl_ODR_100Hz | value)};
m_spi.write(commands, commands.length);
if (m_simRange != null) {
m_simRange.set(value);
}
}
/**
* Returns the acceleration along the X axis in g-forces.
*
* @return The acceleration along the X axis in g-forces.
*/
public double getX() {
return getAcceleration(Axes.kX);
}
/**
* Returns the acceleration along the Y axis in g-forces.
*
* @return The acceleration along the Y axis in g-forces.
*/
public double getY() {
return getAcceleration(Axes.kY);
}
/**
* Returns the acceleration along the Z axis in g-forces.
*
* @return The acceleration along the Z axis in g-forces.
*/
public double getZ() {
return getAcceleration(Axes.kZ);
}
/**
* Get the acceleration of one axis in Gs.
*
* @param axis The axis to read from.
* @return Acceleration of the ADXL362 in Gs.
*/
public double getAcceleration(ADXL362.Axes axis) {
if (axis == Axes.kX && m_simX != null) {
return m_simX.get();
}
if (axis == Axes.kY && m_simY != null) {
return m_simY.get();
}
if (axis == Axes.kZ && m_simZ != null) {
return m_simZ.get();
}
if (m_spi == null) {
return 0.0;
}
ByteBuffer transferBuffer = ByteBuffer.allocate(4);
transferBuffer.put(0, kRegRead);
transferBuffer.put(1, (byte) (kDataRegister + axis.value));
m_spi.transaction(transferBuffer, transferBuffer, 4);
// Sensor is little endian
transferBuffer.order(ByteOrder.LITTLE_ENDIAN);
return transferBuffer.getShort(2) * m_gsPerLSB;
}
/**
* Get the acceleration of all axes in Gs.
*
* @return An object containing the acceleration measured on each axis of the ADXL362 in Gs.
*/
public ADXL362.AllAxes getAccelerations() {
ADXL362.AllAxes data = new ADXL362.AllAxes();
if (m_simX != null && m_simY != null && m_simZ != null) {
data.XAxis = m_simX.get();
data.YAxis = m_simY.get();
data.ZAxis = m_simZ.get();
return data;
}
if (m_spi != null) {
ByteBuffer dataBuffer = ByteBuffer.allocate(8);
// Select the data address.
dataBuffer.put(0, kRegRead);
dataBuffer.put(1, kDataRegister);
m_spi.transaction(dataBuffer, dataBuffer, 8);
// Sensor is little endian... swap bytes
dataBuffer.order(ByteOrder.LITTLE_ENDIAN);
data.XAxis = dataBuffer.getShort(2) * m_gsPerLSB;
data.YAxis = dataBuffer.getShort(4) * m_gsPerLSB;
data.ZAxis = dataBuffer.getShort(6) * m_gsPerLSB;
}
return data;
}
@Override
public void initSendable(NTSendableBuilder builder) {
builder.setSmartDashboardType("3AxisAccelerometer");
DoublePublisher pubX = new DoubleTopic(builder.getTopic("X")).publish();
DoublePublisher pubY = new DoubleTopic(builder.getTopic("Y")).publish();
DoublePublisher pubZ = new DoubleTopic(builder.getTopic("Z")).publish();
builder.addCloseable(pubX);
builder.addCloseable(pubY);
builder.addCloseable(pubZ);
builder.setUpdateTable(
() -> {
AllAxes data = getAccelerations();
pubX.set(data.XAxis);
pubY.set(data.YAxis);
pubZ.set(data.ZAxis);
});
}
}

267
wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java
View File

@@ -1,267 +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.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SimBoolean;
import edu.wpi.first.hal.SimDevice;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.math.geometry.Rotation2d;
import edu.wpi.first.util.sendable.Sendable;
import edu.wpi.first.util.sendable.SendableBuilder;
import edu.wpi.first.util.sendable.SendableRegistry;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
/**
* 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.
*
* <p>This class is for the digital ADXRS450 gyro sensor that connects via SPI. Only one instance of
* an ADXRS Gyro is supported.
*/
@SuppressWarnings("TypeName")
public class ADXRS450_Gyro implements Sendable, AutoCloseable {
private static final double kSamplePeriod = 0.0005;
private static final double kCalibrationSampleTime = 5.0;
private static final double kDegreePerSecondPerLSB = 0.0125;
// private static final int kRateRegister = 0x00;
// private static final int kTemRegister = 0x02;
// private static final int kLoCSTRegister = 0x04;
// private static final int kHiCSTRegister = 0x06;
// private static final int kQuadRegister = 0x08;
// private static final int kFaultRegister = 0x0A;
private static final int kPIDRegister = 0x0C;
// private static final int kSNHighRegister = 0x0E;
// private static final int kSNLowRegister = 0x10;
private SPI m_spi;
private SimDevice m_simDevice;
private SimBoolean m_simConnected;
private SimDouble m_simAngle;
private SimDouble m_simRate;
/** Constructor. Uses the onboard CS0. */
public ADXRS450_Gyro() {
this(SPI.Port.kOnboardCS0);
}
/**
* Constructor.
*
* @param port The SPI port that the gyro is connected to
*/
@SuppressWarnings("this-escape")
public ADXRS450_Gyro(SPI.Port port) {
m_spi = new SPI(port);
// simulation
m_simDevice = SimDevice.create("Gyro:ADXRS450", port.value);
if (m_simDevice != null) {
m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true);
m_simAngle = m_simDevice.createDouble("angle_x", SimDevice.Direction.kInput, 0.0);
m_simRate = m_simDevice.createDouble("rate_x", SimDevice.Direction.kInput, 0.0);
}
m_spi.setClockRate(3000000);
m_spi.setMode(SPI.Mode.kMode0);
m_spi.setChipSelectActiveLow();
if (m_simDevice == null) {
// Validate the part ID
if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
m_spi.close();
m_spi = null;
DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.value, false);
return;
}
m_spi.initAccumulator(
kSamplePeriod, 0x20000000, 4, 0x0c00000e, 0x04000000, 10, 16, true, true);
calibrate();
}
HAL.report(tResourceType.kResourceType_ADXRS450, port.value + 1);
SendableRegistry.addLW(this, "ADXRS450_Gyro", port.value);
}
/**
* Determine if the gyro is connected.
*
* @return true if it is connected, false otherwise.
*/
public boolean isConnected() {
if (m_simConnected != null) {
return m_simConnected.get();
}
return m_spi != null;
}
/**
* 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.
*
* <p>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.
*/
public final void calibrate() {
if (m_spi == null) {
return;
}
Timer.delay(0.1);
m_spi.setAccumulatorIntegratedCenter(0);
m_spi.resetAccumulator();
Timer.delay(kCalibrationSampleTime);
m_spi.setAccumulatorIntegratedCenter(m_spi.getAccumulatorIntegratedAverage());
m_spi.resetAccumulator();
}
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
public int getPort() {
return m_spi.getPort();
}
private boolean calcParity(int value) {
boolean parity = false;
while (value != 0) {
parity = !parity;
value = value & (value - 1);
}
return parity;
}
private int readRegister(int reg) {
int cmdhi = 0x8000 | (reg << 1);
boolean parity = calcParity(cmdhi);
ByteBuffer buf = ByteBuffer.allocate(4);
buf.order(ByteOrder.BIG_ENDIAN);
buf.put(0, (byte) (cmdhi >> 8));
buf.put(1, (byte) (cmdhi & 0xff));
buf.put(2, (byte) 0);
buf.put(3, (byte) (parity ? 0 : 1));
m_spi.write(buf, 4);
m_spi.read(false, buf, 4);
if ((buf.get(0) & 0xe0) == 0) {
return 0; // error, return 0
}
return (buf.getInt(0) >> 5) & 0xffff;
}
/**
* Reset the gyro.
*
* <p>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.
*/
public void reset() {
if (m_simAngle != null) {
m_simAngle.reset();
}
if (m_spi != null) {
m_spi.resetAccumulator();
}
}
/** Delete (free) the spi port used for the gyro and stop accumulating. */
@Override
public void close() {
SendableRegistry.remove(this);
if (m_spi != null) {
m_spi.close();
m_spi = null;
}
if (m_simDevice != null) {
m_simDevice.close();
m_simDevice = null;
}
}
/**
* Return the heading of the robot in degrees.
*
* <p>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.
*
* <p>The angle is expected to increase as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot in degrees.
*/
public double getAngle() {
if (m_simAngle != null) {
return m_simAngle.get();
}
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
}
/**
* Return the rate of rotation of the gyro.
*
* <p>The rate is based on the most recent reading of the gyro analog value
*
* <p>The rate is expected to be positive as the gyro turns clockwise when looked at from the top.
* It needs to follow the NED axis convention.
*
* @return the current rate in degrees per second
*/
public double getRate() {
if (m_simRate != null) {
return m_simRate.get();
}
if (m_spi == null) {
return 0.0;
}
return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
}
/**
* Return the heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*
* <p>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.
*
* <p>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.
*
* <p>This heading is based on integration of the returned rate from the gyro.
*
* @return the current heading of the robot as a {@link edu.wpi.first.math.geometry.Rotation2d}.
*/
public Rotation2d getRotation2d() {
return Rotation2d.fromDegrees(-getAngle());
}
@Override
public void initSendable(SendableBuilder builder) {
builder.setSmartDashboardType("Gyro");
builder.addDoubleProperty("Value", this::getAngle, null);
}
}

791
wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java
View File

@@ -1,791 +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.
package edu.wpi.first.wpilibj;
import edu.wpi.first.hal.AccumulatorResult;
import edu.wpi.first.hal.FRCNetComm.tResourceType;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.hal.SPIJNI;
import java.nio.ByteBuffer;
import java.nio.ByteOrder;
import java.nio.IntBuffer;
/** Represents an SPI bus port. */
public class SPI implements AutoCloseable {
/** SPI port. */
public enum Port {
/** Onboard SPI bus port CS0. */
kOnboardCS0(SPIJNI.ONBOARD_CS0_PORT),
/** Onboard SPI bus port CS1. */
kOnboardCS1(SPIJNI.ONBOARD_CS1_PORT),
/** Onboard SPI bus port CS2. */
kOnboardCS2(SPIJNI.ONBOARD_CS2_PORT),
/** Onboard SPI bus port CS3. */
kOnboardCS3(SPIJNI.ONBOARD_CS3_PORT),
/** MXP (roboRIO MXP) SPI bus port. */
kMXP(SPIJNI.MXP_PORT);
/** SPI port value. */
public final int value;
Port(int value) {
this.value = value;
}
}
/** SPI mode. */
public enum Mode {
/** Clock idle low, data sampled on rising edge. */
kMode0(SPIJNI.SPI_MODE0),
/** Clock idle low, data sampled on falling edge. */
kMode1(SPIJNI.SPI_MODE1),
/** Clock idle high, data sampled on falling edge. */
kMode2(SPIJNI.SPI_MODE2),
/** Clock idle high, data sampled on rising edge. */
kMode3(SPIJNI.SPI_MODE3);
/** SPI mode value. */
public final int value;
Mode(int value) {
this.value = value;
}
}
private final int m_port;
/**
* Constructor.
*
* @param port the physical SPI port
*/
public SPI(Port port) {
m_port = port.value;
SPIJNI.spiInitialize(m_port);
SPIJNI.spiSetMode(m_port, 0);
HAL.report(tResourceType.kResourceType_SPI, port.value + 1);
}
/**
* Returns the SPI port value.
*
* @return SPI port value.
*/
public int getPort() {
return m_port;
}
@Override
public void close() {
if (m_accum != null) {
m_accum.close();
m_accum = null;
}
SPIJNI.spiClose(m_port);
}
/**
* Configure the rate of the generated clock signal. The default value is 500,000 Hz. The maximum
* value is 4,000,000 Hz.
*
* @param hz The clock rate in Hertz.
*/
public final void setClockRate(int hz) {
SPIJNI.spiSetSpeed(m_port, 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.
*/
public final void setMode(Mode mode) {
SPIJNI.spiSetMode(m_port, mode.value & 0x3);
}
/** Configure the chip select line to be active high. */
public final void setChipSelectActiveHigh() {
SPIJNI.spiSetChipSelectActiveHigh(m_port);
}
/** Configure the chip select line to be active low. */
public final void setChipSelectActiveLow() {
SPIJNI.spiSetChipSelectActiveLow(m_port);
}
/**
* Write data to the peripheral device. Blocks until there is space in the output FIFO.
*
* <p>If not running in output only mode, also saves the data received on the CIPO input during
* the transfer into the receive FIFO.
*
* @param dataToSend The buffer containing the data to send.
* @param size The number of bytes to send.
* @return Number of bytes written or -1 on error.
*/
public int write(byte[] dataToSend, int size) {
if (dataToSend.length < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiWriteB(m_port, dataToSend, (byte) size);
}
/**
* Write data to the peripheral device. Blocks until there is space in the output FIFO.
*
* <p>If not running in output only mode, also saves the data received on the CIPO input during
* the transfer into the receive FIFO.
*
* @param dataToSend The buffer containing the data to send.
* @param size The number of bytes to send.
* @return Number of bytes written or -1 on error.
*/
public int write(ByteBuffer dataToSend, int size) {
if (dataToSend.hasArray()) {
return write(dataToSend.array(), size);
}
if (!dataToSend.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (dataToSend.capacity() < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiWrite(m_port, dataToSend, (byte) size);
}
/**
* Read a word from the receive FIFO.
*
* <p>Waits for the current transfer to complete if the receive FIFO is empty.
*
* <p>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 in which to store bytes read.
* @param size Number of bytes to read.
* @return Number of bytes read or -1 on error.
*/
public int read(boolean initiate, byte[] dataReceived, int size) {
if (dataReceived.length < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiReadB(m_port, initiate, dataReceived, (byte) size);
}
/**
* Read a word from the receive FIFO.
*
* <p>Waits for the current transfer to complete if the receive FIFO is empty.
*
* <p>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 The buffer to be filled with the received data.
* @param size The length of the transaction, in bytes
* @return Number of bytes read or -1 on error.
*/
public int read(boolean initiate, ByteBuffer dataReceived, int size) {
if (dataReceived.hasArray()) {
return read(initiate, dataReceived.array(), size);
}
if (!dataReceived.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (dataReceived.capacity() < size) {
throw new IllegalArgumentException("buffer is too small, must be at least " + size);
}
return SPIJNI.spiRead(m_port, initiate, dataReceived, (byte) 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
* @return TODO
*/
public int transaction(byte[] dataToSend, byte[] dataReceived, int size) {
if (dataToSend.length < size) {
throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
}
if (dataReceived.length < size) {
throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
}
return SPIJNI.spiTransactionB(m_port, dataToSend, dataReceived, (byte) 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
* @return TODO
*/
public int transaction(ByteBuffer dataToSend, ByteBuffer dataReceived, int size) {
if (dataToSend.hasArray() && dataReceived.hasArray()) {
return transaction(dataToSend.array(), dataReceived.array(), size);
}
if (!dataToSend.isDirect()) {
throw new IllegalArgumentException("dataToSend must be a direct buffer");
}
if (dataToSend.capacity() < size) {
throw new IllegalArgumentException("dataToSend is too small, must be at least " + size);
}
if (!dataReceived.isDirect()) {
throw new IllegalArgumentException("dataReceived must be a direct buffer");
}
if (dataReceived.capacity() < size) {
throw new IllegalArgumentException("dataReceived is too small, must be at least " + size);
}
return SPIJNI.spiTransaction(m_port, dataToSend, dataReceived, (byte) size);
}
/**
* Initialize automatic SPI transfer engine.
*
* <p>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
*/
public void initAuto(int bufferSize) {
SPIJNI.spiInitAuto(m_port, bufferSize);
}
/** Frees the automatic SPI transfer engine. */
public void freeAuto() {
SPIJNI.spiFreeAuto(m_port);
}
/**
* Set the data to be transmitted by the engine.
*
* <p>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
*/
public void setAutoTransmitData(byte[] dataToSend, int zeroSize) {
SPIJNI.spiSetAutoTransmitData(m_port, dataToSend, zeroSize);
}
/**
* Start running the automatic SPI transfer engine at a periodic rate.
*
* <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} must be called before
* calling this function.
*
* @param period period between transfers, in seconds (us resolution)
*/
public void startAutoRate(double period) {
SPIJNI.spiStartAutoRate(m_port, period);
}
/**
* Start running the automatic SPI transfer engine when a trigger occurs.
*
* <p>{@link #initAuto(int)} and {@link #setAutoTransmitData(byte[], int)} 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
*/
public void startAutoTrigger(DigitalSource source, boolean rising, boolean falling) {
SPIJNI.spiStartAutoTrigger(
m_port,
source.getPortHandleForRouting(),
source.getAnalogTriggerTypeForRouting(),
rising,
falling);
}
/** Stop running the automatic SPI transfer engine. */
public void stopAuto() {
SPIJNI.spiStopAuto(m_port);
}
/** Force the engine to make a single transfer. */
public void forceAutoRead() {
SPIJNI.spiForceAutoRead(m_port);
}
/**
* Read data that has been transferred by the automatic SPI transfer engine.
*
* <p>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.
*
* <p>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().
*
* <p>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 in seconds (ms resolution)
* @return Number of words remaining to be read
*/
public int readAutoReceivedData(ByteBuffer buffer, int numToRead, double timeout) {
if (!buffer.isDirect()) {
throw new IllegalArgumentException("must be a direct buffer");
}
if (buffer.capacity() < numToRead * 4) {
throw new IllegalArgumentException(
"buffer is too small, must be at least " + (numToRead * 4));
}
return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, timeout);
}
/**
* Read data that has been transferred by the automatic SPI transfer engine.
*
* <p>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.
*
* <p>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().
*
* <p>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 array where read words are stored
* @param numToRead number of words to read
* @param timeout timeout in seconds (ms resolution)
* @return Number of words remaining to be read
*/
public int readAutoReceivedData(int[] buffer, int numToRead, double timeout) {
if (buffer.length < numToRead) {
throw new IllegalArgumentException("buffer is too small, must be at least " + numToRead);
}
return SPIJNI.spiReadAutoReceivedData(m_port, buffer, numToRead, 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
*/
public int getAutoDroppedCount() {
return SPIJNI.spiGetAutoDroppedCount(m_port);
}
/**
* Configure the Auto SPI Stall time between reads.
*
* @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
*/
public void configureAutoStall(int csToSclkTicks, int stallTicks, int pow2BytesPerRead) {
SPIJNI.spiConfigureAutoStall(m_port, csToSclkTicks, stallTicks, pow2BytesPerRead);
}
private static final int kAccumulateDepth = 2048;
private static class Accumulator implements AutoCloseable {
Accumulator(
int port,
int xferSize,
int validMask,
int validValue,
int dataShift,
int dataSize,
boolean isSigned,
boolean bigEndian) {
m_notifier = new Notifier(this::update);
m_buf =
ByteBuffer.allocateDirect((xferSize + 1) * kAccumulateDepth * 4)
.order(ByteOrder.nativeOrder());
m_intBuf = m_buf.asIntBuffer();
m_xferSize = xferSize + 1; // +1 for timestamp
m_validMask = validMask;
m_validValue = validValue;
m_dataShift = dataShift;
m_dataMax = 1 << dataSize;
m_dataMsbMask = 1 << (dataSize - 1);
m_isSigned = isSigned;
m_bigEndian = bigEndian;
m_port = port;
}
@Override
public void close() {
m_notifier.close();
}
final Notifier m_notifier;
final ByteBuffer m_buf;
final IntBuffer m_intBuf;
final Object m_mutex = new Object();
long m_value;
int m_count;
int m_lastValue;
long m_lastTimestamp;
double m_integratedValue;
int m_center;
int m_deadband;
double m_integratedCenter;
final int m_validMask;
final int m_validValue;
final int m_dataMax; // one more than max data value
final int m_dataMsbMask; // data field MSB mask (for signed)
final int m_dataShift; // data field shift right amount, in bits
final int m_xferSize; // SPI transfer size, in bytes
final boolean m_isSigned; // is data field signed?
final boolean m_bigEndian; // is response big endian?
final int m_port;
void update() {
synchronized (m_mutex) {
boolean done = false;
while (!done) {
done = true;
// get amount of data available
int numToRead = SPIJNI.spiReadAutoReceivedData(m_port, m_buf, 0, 0);
// only get whole responses
numToRead -= numToRead % m_xferSize;
if (numToRead > m_xferSize * kAccumulateDepth) {
numToRead = m_xferSize * kAccumulateDepth;
done = false;
}
if (numToRead == 0) {
return; // no samples
}
// read buffered data
SPIJNI.spiReadAutoReceivedData(m_port, m_buf, numToRead, 0);
// loop over all responses
for (int off = 0; off < numToRead; off += m_xferSize) {
// get timestamp from first word
long timestamp = m_intBuf.get(off) & 0xffffffffL;
// convert from bytes
int resp = 0;
if (m_bigEndian) {
for (int i = 1; i < m_xferSize; ++i) {
resp <<= 8;
resp |= m_intBuf.get(off + i) & 0xff;
}
} else {
for (int i = m_xferSize - 1; i >= 1; --i) {
resp <<= 8;
resp |= m_intBuf.get(off + i) & 0xff;
}
}
// process response
if ((resp & m_validMask) == m_validValue) {
// valid sensor data; extract data field
int data = 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
int 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 * (timestamp - m_lastTimestamp) * 1e-6 - m_integratedCenter;
} else {
m_integratedValue +=
dataNoCenter * ((1L << 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;
}
}
}
}
}
private Accumulator m_accum;
/**
* 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 validMask 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?
*/
public void initAccumulator(
double period,
int cmd,
int xferSize,
int validMask,
int validValue,
int dataShift,
int dataSize,
boolean isSigned,
boolean bigEndian) {
initAuto(xferSize * 2048);
byte[] cmdBytes = new byte[] {0, 0, 0, 0};
if (bigEndian) {
for (int i = xferSize - 1; i >= 0; --i) {
cmdBytes[i] = (byte) (cmd & 0xff);
cmd >>= 8;
}
} else {
cmdBytes[0] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[1] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[2] = (byte) (cmd & 0xff);
cmd >>= 8;
cmdBytes[3] = (byte) (cmd & 0xff);
}
setAutoTransmitData(cmdBytes, xferSize - 4);
startAutoRate(period);
m_accum =
new Accumulator(
m_port, xferSize, validMask, validValue, dataShift, dataSize, isSigned, bigEndian);
m_accum.m_notifier.startPeriodic(period * 1024);
}
/** Frees the accumulator. */
public void freeAccumulator() {
if (m_accum != null) {
m_accum.close();
m_accum = null;
}
freeAuto();
}
/** Resets the accumulator to zero. */
public void resetAccumulator() {
if (m_accum == null) {
return;
}
synchronized (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;
}
}
/**
* Set the center value of the accumulator.
*
* <p>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.
*
* @param center The accumulator's center value.
*/
public void setAccumulatorCenter(int center) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_center = center;
}
}
/**
* Set the accumulator's deadband.
*
* @param deadband The accumulator's deadband.
*/
public void setAccumulatorDeadband(int deadband) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_deadband = deadband;
}
}
/**
* Read the last value read by the accumulator engine.
*
* @return The last value read by the accumulator engine.
*/
public int getAccumulatorLastValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_lastValue;
}
}
/**
* Read the accumulated value.
*
* @return The 64-bit value accumulated since the last Reset().
*/
public long getAccumulatorValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_value;
}
}
/**
* Read the number of accumulated values.
*
* <p>Read the count of the accumulated values since the accumulator was last Reset().
*
* @return The number of times samples from the channel were accumulated.
*/
public int getAccumulatorCount() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_count;
}
}
/**
* Read the average of the accumulated value.
*
* @return The accumulated average value (value / count).
*/
public double getAccumulatorAverage() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
if (m_accum.m_count == 0) {
return 0.0;
}
return ((double) m_accum.m_value) / m_accum.m_count;
}
}
/**
* Read the accumulated value and the number of accumulated values atomically.
*
* <p>This function reads the value and count atomically. This can be used for averaging.
*
* @param result AccumulatorResult object to store the results in.
*/
public void getAccumulatorOutput(AccumulatorResult result) {
if (result == null) {
throw new IllegalArgumentException("Null parameter `result'");
}
if (m_accum == null) {
result.value = 0;
result.count = 0;
return;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
result.value = m_accum.m_value;
result.count = m_accum.m_count;
}
}
/**
* Set the center value of the accumulator integrator.
*
* <p>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.
*
* @param center The accumulator integrator's center value.
*/
public void setAccumulatorIntegratedCenter(double center) {
if (m_accum == null) {
return;
}
synchronized (m_accum.m_mutex) {
m_accum.m_integratedCenter = center;
}
}
/**
* Read the integrated value. This is the sum of (each value * time between values).
*
* @return The integrated value accumulated since the last Reset().
*/
public double getAccumulatorIntegratedValue() {
if (m_accum == null) {
return 0;
}
synchronized (m_accum.m_mutex) {
m_accum.update();
return m_accum.m_integratedValue;
}
}
/**
* 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().
*/
public double getAccumulatorIntegratedAverage() {
if (m_accum == null) {
return 0;
}
synchronized (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);
}
}
}

3
wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java
View File

@@ -351,8 +351,6 @@ public enum BuiltInWidgets implements WidgetType {
*
* <ul>
* <li>{@link edu.wpi.first.wpilibj.ADXL345_I2C}
* <li>{@link edu.wpi.first.wpilibj.ADXL345_SPI}
* <li>{@link edu.wpi.first.wpilibj.ADXL362}
* </ul>
*
* <br>
@@ -374,7 +372,6 @@ public enum BuiltInWidgets implements WidgetType {
* Supported types:
*
* <ul>
* <li>{@link edu.wpi.first.wpilibj.ADXRS450_Gyro}
* <li>{@link edu.wpi.first.wpilibj.AnalogGyro}
* <li>Any custom subclass of {@code GyroBase} (such as a MXP gyro)
* </ul>

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java
View File

@@ -1,121 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
/** Class to control a simulated ADIS16448 gyroscope. */
@SuppressWarnings("TypeName")
public class ADIS16448_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
/**
* Constructs from an ADIS16448_IMU object.
*
* @param gyro ADIS16448_IMU to simulate
*/
public ADIS16448_IMUSim(ADIS16448_IMU gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16448" + "[" + gyro.getPort() + "]");
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleX(double angleDegrees) {
m_simGyroAngleX.set(angleDegrees);
}
/**
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleY(double angleDegrees) {
m_simGyroAngleY.set(angleDegrees);
}
/**
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleZ(double angleDegrees) {
m_simGyroAngleZ.set(angleDegrees);
}
/**
* Sets the X axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateX(double angularRateDegreesPerSecond) {
m_simGyroRateX.set(angularRateDegreesPerSecond);
}
/**
* Sets the Y axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateY(double angularRateDegreesPerSecond) {
m_simGyroRateY.set(angularRateDegreesPerSecond);
}
/**
* Sets the Z axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateZ(double angularRateDegreesPerSecond) {
m_simGyroRateZ.set(angularRateDegreesPerSecond);
}
/**
* Sets the X axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}

121
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java
View File

@@ -1,121 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADIS16470_IMU;
/** Class to control a simulated ADIS16470 gyroscope. */
@SuppressWarnings("TypeName")
public class ADIS16470_IMUSim {
private final SimDouble m_simGyroAngleX;
private final SimDouble m_simGyroAngleY;
private final SimDouble m_simGyroAngleZ;
private final SimDouble m_simGyroRateX;
private final SimDouble m_simGyroRateY;
private final SimDouble m_simGyroRateZ;
private final SimDouble m_simAccelX;
private final SimDouble m_simAccelY;
private final SimDouble m_simAccelZ;
/**
* Constructs from an ADIS16470_IMU object.
*
* @param gyro ADIS16470_IMU to simulate
*/
public ADIS16470_IMUSim(ADIS16470_IMU gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADIS16470" + "[" + gyro.getPort() + "]");
m_simGyroAngleX = wrappedSimDevice.getDouble("gyro_angle_x");
m_simGyroAngleY = wrappedSimDevice.getDouble("gyro_angle_y");
m_simGyroAngleZ = wrappedSimDevice.getDouble("gyro_angle_z");
m_simGyroRateX = wrappedSimDevice.getDouble("gyro_rate_x");
m_simGyroRateY = wrappedSimDevice.getDouble("gyro_rate_y");
m_simGyroRateZ = wrappedSimDevice.getDouble("gyro_rate_z");
m_simAccelX = wrappedSimDevice.getDouble("accel_x");
m_simAccelY = wrappedSimDevice.getDouble("accel_y");
m_simAccelZ = wrappedSimDevice.getDouble("accel_z");
}
/**
* Sets the X axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleX(double angleDegrees) {
m_simGyroAngleX.set(angleDegrees);
}
/**
* Sets the Y axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleY(double angleDegrees) {
m_simGyroAngleY.set(angleDegrees);
}
/**
* Sets the Z axis angle in degrees (CCW positive).
*
* @param angleDegrees The angle.
*/
public void setGyroAngleZ(double angleDegrees) {
m_simGyroAngleZ.set(angleDegrees);
}
/**
* Sets the X axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateX(double angularRateDegreesPerSecond) {
m_simGyroRateX.set(angularRateDegreesPerSecond);
}
/**
* Sets the Y axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateY(double angularRateDegreesPerSecond) {
m_simGyroRateY.set(angularRateDegreesPerSecond);
}
/**
* Sets the Z axis angle in degrees per second (CCW positive).
*
* @param angularRateDegreesPerSecond The angular rate.
*/
public void setGyroRateZ(double angularRateDegreesPerSecond) {
m_simGyroRateZ.set(angularRateDegreesPerSecond);
}
/**
* Sets the X axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelX(double accelMetersPerSecondSquared) {
m_simAccelX.set(accelMetersPerSecondSquared);
}
/**
* Sets the Y axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelY(double accelMetersPerSecondSquared) {
m_simAccelY.set(accelMetersPerSecondSquared);
}
/**
* Sets the Z axis acceleration in meters per second squared.
*
* @param accelMetersPerSecondSquared The acceleration.
*/
public void setAccelZ(double accelMetersPerSecondSquared) {
m_simAccelZ.set(accelMetersPerSecondSquared);
}
}

11
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java
View File

@@ -6,7 +6,6 @@ package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_SPI;
import java.util.Objects;
/** Class to control a simulated ADXL345. */
@@ -15,16 +14,6 @@ public class ADXL345Sim {
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param device The device to simulate
*/
public ADXL345Sim(ADXL345_SPI device) {
SimDeviceSim simDevice = new SimDeviceSim("Accel:ADXL345_SPI" + "[" + device.getPort() + "]");
initSim(simDevice);
}
/**
* Constructor.
*

64
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java
View File

@@ -1,64 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXL362;
import java.util.Objects;
/** Class to control a simulated ADXL362. */
public class ADXL362Sim {
private SimDouble m_simX;
private SimDouble m_simY;
private SimDouble m_simZ;
/**
* Constructor.
*
* @param device The device to simulate
*/
public ADXL362Sim(ADXL362 device) {
SimDeviceSim wrappedSimDevice =
new SimDeviceSim("Accel:ADXL362" + "[" + device.getPort() + "]");
initSim(wrappedSimDevice);
}
private void initSim(SimDeviceSim wrappedSimDevice) {
m_simX = wrappedSimDevice.getDouble("x");
m_simY = wrappedSimDevice.getDouble("y");
m_simZ = wrappedSimDevice.getDouble("z");
Objects.requireNonNull(m_simX);
Objects.requireNonNull(m_simY);
Objects.requireNonNull(m_simZ);
}
/**
* Sets the X acceleration.
*
* @param accel The X acceleration.
*/
public void setX(double accel) {
m_simX.set(accel);
}
/**
* Sets the Y acceleration.
*
* @param accel The Y acceleration.
*/
public void setY(double accel) {
m_simY.set(accel);
}
/**
* Sets the Z acceleration.
*
* @param accel The Z acceleration.
*/
public void setZ(double accel) {
m_simZ.set(accel);
}
}

44
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java
View File

@@ -1,44 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.SimDouble;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
/** Class to control a simulated ADXRS450 gyroscope. */
@SuppressWarnings("TypeName")
public class ADXRS450_GyroSim {
private final SimDouble m_simAngle;
private final SimDouble m_simRate;
/**
* Constructs from an ADXRS450_Gyro object.
*
* @param gyro ADXRS450_Gyro to simulate
*/
public ADXRS450_GyroSim(ADXRS450_Gyro gyro) {
SimDeviceSim wrappedSimDevice = new SimDeviceSim("Gyro:ADXRS450" + "[" + gyro.getPort() + "]");
m_simAngle = wrappedSimDevice.getDouble("angle_x");
m_simRate = wrappedSimDevice.getDouble("rate_x");
}
/**
* Sets the angle in degrees (clockwise positive).
*
* @param angleDegrees The angle.
*/
public void setAngle(double angleDegrees) {
m_simAngle.set(angleDegrees);
}
/**
* Sets the angular rate in degrees per second (clockwise positive).
*
* @param rateDegreesPerSecond The angular rate.
*/
public void setRate(double rateDegreesPerSecond) {
m_simRate.set(rateDegreesPerSecond);
}
}

177
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java
View File

@@ -1,177 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.simulation.NotifyCallback;
import edu.wpi.first.hal.simulation.SPIAccelerometerDataJNI;
/** A class to control a simulated accelerometer over SPI. */
public class SPIAccelerometerSim {
private final int m_index;
/**
* Construct a new simulation object.
*
* @param index the HAL index of the accelerometer
*/
public SPIAccelerometerSim(int index) {
m_index = 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 {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerActiveCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerActiveCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelActiveCallback);
}
/**
* Check whether the accelerometer is active.
*
* @return true if active
*/
public boolean getActive() {
return SPIAccelerometerDataJNI.getActive(m_index);
}
/**
* Define whether this accelerometer is active.
*
* @param active the new state
*/
public void setActive(boolean active) {
SPIAccelerometerDataJNI.setActive(m_index, 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 {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerRangeCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerRangeCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelRangeCallback);
}
/**
* Check the range of this accelerometer.
*
* @return the accelerometer range
*/
public int getRange() {
return SPIAccelerometerDataJNI.getRange(m_index);
}
/**
* Change the range of this accelerometer.
*
* @param range the new accelerometer range
*/
public void setRange(int range) {
SPIAccelerometerDataJNI.setRange(m_index, 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 {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerXCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerXCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelXCallback);
}
/**
* Measure the X axis value.
*
* @return the X axis measurement
*/
public double getX() {
return SPIAccelerometerDataJNI.getX(m_index);
}
/**
* Change the X axis value of the accelerometer.
*
* @param x the new reading of the X axis
*/
public void setX(double x) {
SPIAccelerometerDataJNI.setX(m_index, 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 {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerYCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerYCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelYCallback);
}
/**
* Measure the Y axis value.
*
* @return the Y axis measurement
*/
public double getY() {
return SPIAccelerometerDataJNI.getY(m_index);
}
/**
* Change the Y axis value of the accelerometer.
*
* @param y the new reading of the Y axis
*/
public void setY(double y) {
SPIAccelerometerDataJNI.setY(m_index, 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 {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerZCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIAccelerometerDataJNI.registerZCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIAccelerometerDataJNI::cancelZCallback);
}
/**
* Measure the Z axis value.
*
* @return the Z axis measurement
*/
public double getZ() {
return SPIAccelerometerDataJNI.getZ(m_index);
}
/**
* Change the Z axis value of the accelerometer.
*
* @param z the new reading of the Z axis
*/
public void setZ(double z) {
SPIAccelerometerDataJNI.setZ(m_index, z);
}
/** Reset all simulation data of this object. */
public void resetData() {
SPIAccelerometerDataJNI.resetData(m_index);
}
}

90
wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java
View File

@@ -1,90 +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.
package edu.wpi.first.wpilibj.simulation;
import edu.wpi.first.hal.simulation.BufferCallback;
import edu.wpi.first.hal.simulation.ConstBufferCallback;
import edu.wpi.first.hal.simulation.NotifyCallback;
import edu.wpi.first.hal.simulation.SPIDataJNI;
import edu.wpi.first.hal.simulation.SpiReadAutoReceiveBufferCallback;
/** A class for controlling a simulated SPI device. */
public class SPISim {
private final int m_index;
/** Create a new simulated SPI device. */
public SPISim() {
m_index = 0;
}
/**
* Register a callback to be run when this device is initialized.
*
* @param callback the callback
* @param initialNotify whether to run the callback with the initial state
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerInitializedCallback(NotifyCallback callback, boolean initialNotify) {
int uid = SPIDataJNI.registerInitializedCallback(m_index, callback, initialNotify);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelInitializedCallback);
}
/**
* Check whether this device has been initialized.
*
* @return true if initialized
*/
public boolean getInitialized() {
return SPIDataJNI.getInitialized(m_index);
}
/**
* Define whether this device has been initialized.
*
* @param initialized whether this object is initialized
*/
public void setInitialized(boolean initialized) {
SPIDataJNI.setInitialized(m_index, initialized);
}
/**
* Register a callback to be run whenever a `read` operation is executed.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerReadCallback(BufferCallback callback) {
int uid = SPIDataJNI.registerReadCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadCallback);
}
/**
* Register a callback to be run whenever a `write` operation is executed.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerWriteCallback(ConstBufferCallback callback) {
int uid = SPIDataJNI.registerWriteCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelWriteCallback);
}
/**
* Register a callback to be run whenever an auto receive buffer is received.
*
* @param callback the callback
* @return the {@link CallbackStore} object associated with this callback.
*/
public CallbackStore registerReadAutoReceiveBufferCallback(
SpiReadAutoReceiveBufferCallback callback) {
int uid = SPIDataJNI.registerReadAutoReceiveBufferCallback(m_index, callback);
return new CallbackStore(m_index, uid, SPIDataJNI::cancelReadAutoReceiveBufferCallback);
}
/** Reset all simulation data. */
public void resetData() {
SPIDataJNI.resetData(m_index);
}
}

132
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java
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.
package edu.wpi.first.wpilibj.simulation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import static org.junit.jupiter.api.Assertions.fail;
import static org.junit.jupiter.params.provider.Arguments.arguments;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.ADIS16448_IMU;
import edu.wpi.first.wpilibj.SPI;
import java.util.stream.Stream;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.Arguments;
import org.junit.jupiter.params.provider.MethodSource;
class ADIS16448SimTest {
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testYawAxis(ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
assertEquals(yawAxis, imu.getYawAxis());
}
}
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testGyroAngles(
ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
sim.setGyroAngleX(123.45);
sim.setGyroAngleY(-67.89);
sim.setGyroAngleZ(4.13);
assertEquals(123.45, imu.getGyroAngleX());
assertEquals(-67.89, imu.getGyroAngleY());
assertEquals(4.13, imu.getGyroAngleZ());
}
}
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testGyroRates(ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
sim.setGyroRateX(15.92);
sim.setGyroRateY(-65.35);
sim.setGyroRateZ(2.71);
assertEquals(15.92, imu.getGyroRateX());
assertEquals(-65.35, imu.getGyroRateY());
assertEquals(2.71, imu.getGyroRateZ());
}
}
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testAccelValues(
ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
sim.setAccelX(6.85);
sim.setAccelY(-1.82);
sim.setAccelZ(8.69);
assertEquals(6.85, imu.getAccelX());
assertEquals(-1.82, imu.getAccelY());
assertEquals(8.69, imu.getAccelZ());
}
}
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testAngleBasedOnYawAxis(
ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
sim.setGyroAngleX(123.45);
sim.setGyroAngleY(-67.89);
sim.setGyroAngleZ(4.13);
switch (yawAxis) {
case kX -> assertEquals(imu.getAngle(), imu.getGyroAngleX());
case kY -> assertEquals(imu.getAngle(), imu.getGyroAngleY());
case kZ -> assertEquals(imu.getAngle(), imu.getGyroAngleZ());
default -> fail("invalid YawAxis!");
}
}
}
@ParameterizedTest
@MethodSource("provideEnumCombinations")
void testRateBasedOnYawAxis(
ADIS16448_IMU.IMUAxis yawAxis, ADIS16448_IMU.CalibrationTime calibrationTime) {
HAL.initialize(500, 0);
try (ADIS16448_IMU imu = new ADIS16448_IMU(yawAxis, SPI.Port.kMXP, calibrationTime)) {
ADIS16448_IMUSim sim = new ADIS16448_IMUSim(imu);
sim.setGyroRateX(1.92);
sim.setGyroRateY(-6.35);
sim.setGyroRateZ(20.71);
switch (yawAxis) {
case kX -> assertEquals(imu.getRate(), imu.getGyroRateX());
case kY -> assertEquals(imu.getRate(), imu.getGyroRateY());
case kZ -> assertEquals(imu.getRate(), imu.getGyroRateZ());
default -> fail("invalid YawAxis!");
}
}
}
static Stream<Arguments> provideEnumCombinations() {
return Stream.of(ADIS16448_IMU.IMUAxis.values())
.flatMap(
imuAxis ->
Stream.of(ADIS16448_IMU.CalibrationTime.values())
.map(calibrationTime -> arguments(imuAxis, calibrationTime)));
}
}

25
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java
View File

@@ -8,9 +8,7 @@ import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.ADXL345_I2C;
import edu.wpi.first.wpilibj.ADXL345_SPI;
import edu.wpi.first.wpilibj.I2C;
import edu.wpi.first.wpilibj.SPI;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.EnumSource;
@@ -37,27 +35,4 @@ class ADXL345SimTest {
assertEquals(2.29, allAccel.ZAxis);
}
}
@ParameterizedTest
@EnumSource(ADXL345_SPI.Range.class)
void testInitSPi(ADXL345_SPI.Range range) {
HAL.initialize(500, 0);
try (ADXL345_SPI accel = new ADXL345_SPI(SPI.Port.kMXP, range)) {
ADXL345Sim sim = new ADXL345Sim(accel);
sim.setX(1.91);
sim.setY(-3.405);
sim.setZ(2.29);
assertEquals(1.91, accel.getX());
assertEquals(-3.405, accel.getY());
assertEquals(2.29, accel.getZ());
ADXL345_SPI.AllAxes allAccel = accel.getAccelerations();
assertEquals(1.91, allAccel.XAxis);
assertEquals(-3.405, allAccel.YAxis);
assertEquals(2.29, allAccel.ZAxis);
}
}
}

41
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java
View File

@@ -1,41 +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.
package edu.wpi.first.wpilibj.simulation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.ADXL362;
import edu.wpi.first.wpilibj.SPI;
import org.junit.jupiter.params.ParameterizedTest;
import org.junit.jupiter.params.provider.EnumSource;
class ADXL362SimTest {
@ParameterizedTest
@EnumSource(ADXL362.Range.class)
void testAccel(ADXL362.Range range) {
HAL.initialize(500, 0);
try (ADXL362 accel = new ADXL362(SPI.Port.kMXP, range)) {
assertEquals(0, accel.getX());
assertEquals(0, accel.getY());
assertEquals(0, accel.getZ());
ADXL362Sim sim = new ADXL362Sim(accel);
sim.setX(1.91);
sim.setY(-3.405);
sim.setZ(2.29);
assertEquals(1.91, accel.getX());
assertEquals(-3.405, accel.getY());
assertEquals(2.29, accel.getZ());
ADXL362.AllAxes allAccel = accel.getAccelerations();
assertEquals(1.91, allAccel.XAxis);
assertEquals(-3.405, allAccel.YAxis);
assertEquals(2.29, allAccel.ZAxis);
}
}
}

35
wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java
View File

@@ -1,35 +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.
package edu.wpi.first.wpilibj.simulation;
import static org.junit.jupiter.api.Assertions.assertEquals;
import edu.wpi.first.hal.HAL;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import org.junit.jupiter.api.Test;
@SuppressWarnings({"TypeName"})
class ADXRS450_GyroSimTest {
@Test
void testCallbacks() {
HAL.initialize(500, 0);
try (ADXRS450_Gyro gyro = new ADXRS450_Gyro()) {
ADXRS450_GyroSim sim = new ADXRS450_GyroSim(gyro);
assertEquals(0, gyro.getAngle());
assertEquals(0, gyro.getRate());
sim.setAngle(123.456);
sim.setRate(229.3504);
assertEquals(123.456, gyro.getAngle());
assertEquals(229.3504, gyro.getRate());
gyro.reset();
assertEquals(0, gyro.getAngle());
}
}
}

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -9,7 +9,7 @@ import edu.wpi.first.math.kinematics.MecanumDriveOdometry;
import edu.wpi.first.math.kinematics.MecanumDriveWheelPositions;
import edu.wpi.first.math.kinematics.MecanumDriveWheelSpeeds;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.drive.MecanumDrive;
import edu.wpi.first.wpilibj.examples.mecanumcontrollercommand.Constants.DriveConstants;
@@ -54,7 +54,7 @@ public class DriveSubsystem extends SubsystemBase {
DriveConstants.kRearRightEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
private final AnalogGyro m_gyro = new AnalogGyro(0);
// Odometry class for tracking robot pose
MecanumDriveOdometry m_odometry =

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java
View File

@@ -10,7 +10,7 @@ import edu.wpi.first.math.controller.ProfiledPIDController;
import edu.wpi.first.math.controller.SimpleMotorFeedforward;
import edu.wpi.first.math.trajectory.TrapezoidProfile;
import edu.wpi.first.util.sendable.SendableRegistry;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.Encoder;
import edu.wpi.first.wpilibj.RobotController;
import edu.wpi.first.wpilibj.drive.DifferentialDrive;
@@ -49,7 +49,7 @@ public class Drive extends SubsystemBase {
DriveConstants.kRightEncoderPorts[1],
DriveConstants.kRightEncoderReversed);
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
private final AnalogGyro m_gyro = new AnalogGyro(0);
private final ProfiledPIDController m_controller =
new ProfiledPIDController(
DriveConstants.kTurnP,

4
wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java
View File

@@ -10,7 +10,7 @@ import edu.wpi.first.math.kinematics.SwerveDriveKinematics;
import edu.wpi.first.math.kinematics.SwerveDriveOdometry;
import edu.wpi.first.math.kinematics.SwerveModulePosition;
import edu.wpi.first.math.kinematics.SwerveModuleState;
import edu.wpi.first.wpilibj.ADXRS450_Gyro;
import edu.wpi.first.wpilibj.AnalogGyro;
import edu.wpi.first.wpilibj.examples.swervecontrollercommand.Constants.DriveConstants;
import edu.wpi.first.wpilibj2.command.SubsystemBase;
@@ -53,7 +53,7 @@ public class DriveSubsystem extends SubsystemBase {
DriveConstants.kRearRightTurningEncoderReversed);
// The gyro sensor
private final ADXRS450_Gyro m_gyro = new ADXRS450_Gyro();
private final AnalogGyro m_gyro = new AnalogGyro(0);
// Odometry class for tracking robot pose
SwerveDriveOdometry m_odometry =