diff --git a/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java b/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java deleted file mode 100644 index 242053ec382..00000000000 --- a/hal/src/main/java/edu/wpi/first/hal/SPIJNI.java +++ /dev/null @@ -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. - * - *

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. - * - *

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. - * - *

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. - * - *

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. - * - *

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. - * - *

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 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. - * - *

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 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. - * - *

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() {} -} diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java deleted file mode 100644 index b38fb553314..00000000000 --- a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIAccelerometerDataJNI.java +++ /dev/null @@ -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() {} -} diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java deleted file mode 100644 index fa8df895df2..00000000000 --- a/hal/src/main/java/edu/wpi/first/hal/simulation/SPIDataJNI.java +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -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() {} -} diff --git a/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java b/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java deleted file mode 100644 index 10dd0d2e213..00000000000 --- a/hal/src/main/java/edu/wpi/first/hal/simulation/SpiReadAutoReceiveBufferCallback.java +++ /dev/null @@ -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); -} diff --git a/hal/src/main/native/cpp/handles/HandlesInternal.cpp b/hal/src/main/native/cpp/handles/HandlesInternal.cpp index 80dca7107a4..6436667fb31 100644 --- a/hal/src/main/native/cpp/handles/HandlesInternal.cpp +++ b/hal/src/main/native/cpp/handles/HandlesInternal.cpp @@ -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_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) { diff --git a/hal/src/main/native/cpp/jni/SPIJNI.cpp b/hal/src/main/native/cpp/jni/SPIJNI.cpp deleted file mode 100644 index 0940de6da73..00000000000 --- a/hal/src/main/native/cpp/jni/SPIJNI.cpp +++ /dev/null @@ -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 - -#include - -#include - -#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(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(env->GetDirectBufferAddress(dataToSend)); - } - uint8_t* dataReceivedPtr = - reinterpret_cast(env->GetDirectBufferAddress(dataReceived)); - jint retVal = HAL_TransactionSPI(static_cast(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 recvBuf; - recvBuf.resize(size); - jint retVal = - HAL_TransactionSPI(static_cast(port), - reinterpret_cast( - JSpan(env, dataToSend).data()), - recvBuf.data(), size); - env->SetByteArrayRegion(dataReceived, 0, size, - reinterpret_cast(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(env->GetDirectBufferAddress(dataToSend)); - } - jint retVal = - HAL_WriteSPI(static_cast(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(port), - reinterpret_cast( - JSpan(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(env->GetDirectBufferAddress(dataReceived)); - jint retVal; - if (initiate) { - wpi::SmallVector sendBuf; - sendBuf.resize(size); - retVal = HAL_TransactionSPI(static_cast(port), sendBuf.data(), - dataReceivedPtr, size); - } else { - retVal = HAL_ReadSPI(static_cast(port), - reinterpret_cast(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 recvBuf; - recvBuf.resize(size); - if (initiate) { - wpi::SmallVector sendBuf; - sendBuf.resize(size); - retVal = HAL_TransactionSPI(static_cast(port), sendBuf.data(), - recvBuf.data(), size); - } else { - retVal = HAL_ReadSPI(static_cast(port), recvBuf.data(), size); - } - env->SetByteArrayRegion(dataReceived, 0, size, - reinterpret_cast(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(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(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(port), - static_cast(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(HAL_GetSPIMode(static_cast(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(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(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(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(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(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(port), digitalSourceHandle, - static_cast(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(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 jarr(env, dataToSend); - int32_t status = 0; - HAL_SetSPIAutoTransmitData(static_cast(port), - reinterpret_cast(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(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(env->GetDirectBufferAddress(buffer)); - int32_t status = 0; - jint retval = HAL_ReadSPIAutoReceivedData( - static_cast(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 recvBuf; - recvBuf.resize(numToRead); - int32_t status = 0; - jint retval = - HAL_ReadSPIAutoReceivedData(static_cast(port), - recvBuf.data(), numToRead, timeout, &status); - if (!CheckStatus(env, status)) { - return retval; - } - if (numToRead > 0) { - env->SetIntArrayRegion(buffer, 0, numToRead, - reinterpret_cast(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(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(port), csToSclkTicks, - stallTicks, pow2BytesPerRead, &status); - CheckStatus(env, status); -} - -} // extern "C" diff --git a/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp deleted file mode 100644 index 51bb32061d6..00000000000 --- a/hal/src/main/native/cpp/jni/simulation/SPIAccelerometerDataJNI.cpp +++ /dev/null @@ -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 - -#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" diff --git a/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp deleted file mode 100644 index 977f25b65ee..00000000000 --- a/hal/src/main/native/cpp/jni/simulation/SPIDataJNI.cpp +++ /dev/null @@ -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 - -#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" diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp index 859c7f5b73d..84dded1d340 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp +++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.cpp @@ -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" { diff --git a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h index 3e71acdb381..6432079cacf 100644 --- a/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h +++ b/hal/src/main/native/cpp/jni/simulation/SimulatorJNI.h @@ -15,5 +15,4 @@ JavaVM* GetJVM(); jmethodID GetNotifyCallback(); jmethodID GetBufferCallback(); jmethodID GetConstBufferCallback(); -jmethodID GetSpiReadAutoReceiveBufferCallback(); } // namespace hal::sim diff --git a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp deleted file mode 100644 index 3c58c945753..00000000000 --- a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.cpp +++ /dev/null @@ -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 - -#include -#include - -#include - -#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 - cb; - callbackHandles = &cb; -} -} // namespace hal::sim - -void SpiReadAutoReceiveBufferCallbackStore::create(JNIEnv* env, jobject obj) { - m_call = JGlobal(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(&env), JNI_VERSION_1_6); - if (tryGetEnv == JNI_EDETACHED) { - // Thread not attached - didAttachThread = true; - if (vm->AttachCurrentThread(reinterpret_cast(&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{buffer, static_cast(numToRead)}); - - jint ret = env->CallIntMethod(m_call, sim::GetBufferCallback(), - MakeJString(env, name), toCallbackArr, - static_cast(numToRead)); - - jint* fromCallbackArr = reinterpret_cast( - 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(); - - auto handle = callbackHandles->Allocate(callbackStore); - - if (handle == HAL_kInvalidHandle) { - return -1; - } - - uintptr_t handleAsPtr = static_cast(handle); - void* handleAsVoidPtr = reinterpret_cast(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(param); - SIM_JniHandle handle = static_cast(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); -} diff --git a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h b/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h deleted file mode 100644 index 18e8a441719..00000000000 --- a/hal/src/main/native/cpp/jni/simulation/SpiReadAutoReceiveBufferCallbackStore.h +++ /dev/null @@ -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 - -#include - -#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 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 diff --git a/hal/src/main/native/include/hal/HAL.h b/hal/src/main/native/include/hal/HAL.h index b8281d4609e..041fdd93024 100644 --- a/hal/src/main/native/include/hal/HAL.h +++ b/hal/src/main/native/include/hal/HAL.h @@ -28,7 +28,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" diff --git a/hal/src/main/native/include/hal/SPI.h b/hal/src/main/native/include/hal/SPI.h deleted file mode 100644 index 32644b6fab8..00000000000 --- a/hal/src/main/native/include/hal/SPI.h +++ /dev/null @@ -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 - -#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 -/** @} */ diff --git a/hal/src/main/native/include/hal/SPITypes.h b/hal/src/main/native/include/hal/SPITypes.h deleted file mode 100644 index 8427bf339f1..00000000000 --- a/hal/src/main/native/include/hal/SPITypes.h +++ /dev/null @@ -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 - -#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, -}; -/** @} */ diff --git a/hal/src/main/native/include/hal/handles/HandlesInternal.h b/hal/src/main/native/include/hal/handles/HandlesInternal.h index 13a56db68fd..6b31f6e30b3 100644 --- a/hal/src/main/native/include/hal/handles/HandlesInternal.h +++ b/hal/src/main/native/include/hal/handles/HandlesInternal.h @@ -181,20 +181,6 @@ inline int16_t getPortHandleModule(HAL_PortHandle handle) { return static_cast((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((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. * diff --git a/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h b/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h deleted file mode 100644 index 2018a40023b..00000000000 --- a/hal/src/main/native/include/hal/simulation/SPIAccelerometerData.h +++ /dev/null @@ -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 diff --git a/hal/src/main/native/include/hal/simulation/SPIData.h b/hal/src/main/native/include/hal/simulation/SPIData.h deleted file mode 100644 index 813b75f127c..00000000000 --- a/hal/src/main/native/include/hal/simulation/SPIData.h +++ /dev/null @@ -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 diff --git a/hal/src/main/native/sim/HAL.cpp b/hal/src/main/native/sim/HAL.cpp index 939b9c43e8d..d69422e9375 100644 --- a/hal/src/main/native/sim/HAL.cpp +++ b/hal/src/main/native/sim/HAL.cpp @@ -84,8 +84,6 @@ void InitializeHAL() { InitializePWMData(); InitializeRoboRioData(); InitializeSimDeviceData(); - InitializeSPIAccelerometerData(); - InitializeSPIData(); InitializeAccelerometer(); InitializeAddressableLED(); InitializeAnalogInput(); @@ -113,7 +111,6 @@ void InitializeHAL() { InitializePWM(); InitializeSerialPort(); InitializeSimDevice(); - InitializeSPI(); InitializeThreads(); } } // namespace hal::init diff --git a/hal/src/main/native/sim/HALInitializer.h b/hal/src/main/native/sim/HALInitializer.h index e2de7b6146b..4ccc10e8786 100644 --- a/hal/src/main/native/sim/HALInitializer.h +++ b/hal/src/main/native/sim/HALInitializer.h @@ -35,8 +35,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 InitializeAnalogInput(); @@ -64,7 +62,6 @@ extern void InitializeREVPH(); extern void InitializePWM(); extern void InitializeSerialPort(); extern void InitializeSimDevice(); -extern void InitializeSPI(); extern void InitializeThreads(); } // namespace hal::init diff --git a/hal/src/main/native/sim/SPI.cpp b/hal/src/main/native/sim/SPI.cpp deleted file mode 100644 index 70e2bde1dd5..00000000000 --- a/hal/src/main/native/sim/SPI.cpp +++ /dev/null @@ -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" diff --git a/hal/src/main/native/sim/mockdata/Reset.cpp b/hal/src/main/native/sim/mockdata/Reset.cpp index cca21117a21..776aeafdf08 100644 --- a/hal/src/main/native/sim/mockdata/Reset.cpp +++ b/hal/src/main/native/sim/mockdata/Reset.cpp @@ -18,8 +18,6 @@ #include #include #include -#include -#include #include #include "../PortsInternal.h" @@ -83,12 +81,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); - } } diff --git a/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp b/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp deleted file mode 100644 index fa445006518..00000000000 --- a/hal/src/main/native/sim/mockdata/SPIAccelerometerData.cpp +++ /dev/null @@ -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" diff --git a/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h b/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h deleted file mode 100644 index db405f88b98..00000000000 --- a/hal/src/main/native/sim/mockdata/SPIAccelerometerDataInternal.h +++ /dev/null @@ -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 active{false}; - SimDataValue range{0}; - SimDataValue x{0.0}; - SimDataValue y{0.0}; - SimDataValue z{0.0}; - - virtual void ResetData(); -}; -extern SPIAccelerometerData* SimSPIAccelerometerData; -} // namespace hal diff --git a/hal/src/main/native/sim/mockdata/SPIData.cpp b/hal/src/main/native/sim/mockdata/SPIData.cpp deleted file mode 100644 index 82f7650aba3..00000000000 --- a/hal/src/main/native/sim/mockdata/SPIData.cpp +++ /dev/null @@ -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" diff --git a/hal/src/main/native/sim/mockdata/SPIDataInternal.h b/hal/src/main/native/sim/mockdata/SPIDataInternal.h deleted file mode 100644 index eb5a5a7a733..00000000000 --- a/hal/src/main/native/sim/mockdata/SPIDataInternal.h +++ /dev/null @@ -1,37 +0,0 @@ -// Copyright (c) FIRST and other WPILib contributors. -// Open Source Software; you can modify and/or share it under the terms of -// the WPILib BSD license file in the root directory of this project. - -#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 initialized{ - false}; - SimCallbackRegistry read; - SimCallbackRegistry write; - SimCallbackRegistry - autoReceivedData; - - void ResetData(); -}; -extern SPIData* SimSPIData; -} // namespace hal diff --git a/hal/src/main/native/systemcore/HAL.cpp b/hal/src/main/native/systemcore/HAL.cpp index 4865023a289..4afdf94d3b9 100644 --- a/hal/src/main/native/systemcore/HAL.cpp +++ b/hal/src/main/native/systemcore/HAL.cpp @@ -73,7 +73,6 @@ void InitializeHAL() { InitializePWM(); InitializeSerialPort(); InitializeSmartIo(); - InitializeSPI(); InitializeThreads(); } } // namespace init diff --git a/hal/src/main/native/systemcore/HALInitializer.h b/hal/src/main/native/systemcore/HALInitializer.h index f3ae4a5c3bc..848ef0e84a9 100644 --- a/hal/src/main/native/systemcore/HALInitializer.h +++ b/hal/src/main/native/systemcore/HALInitializer.h @@ -47,6 +47,5 @@ extern void InitializePower(); extern void InitializePWM(); extern void InitializeSerialPort(); extern void InitializeSmartIo(); -extern void InitializeSPI(); extern void InitializeThreads(); } // namespace hal::init diff --git a/hal/src/main/native/systemcore/SPI.cpp b/hal/src/main/native/systemcore/SPI.cpp deleted file mode 100644 index bde55b8f1d4..00000000000 --- a/hal/src/main/native/systemcore/SPI.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#include -#include -#include -#include -#include - -#include -#include -#include - -#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" diff --git a/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp b/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp deleted file mode 100644 index d0aa97f1de3..00000000000 --- a/hal/src/main/native/systemcore/mockdata/SPIAccelerometerData.cpp +++ /dev/null @@ -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" diff --git a/hal/src/main/native/systemcore/mockdata/SPIData.cpp b/hal/src/main/native/systemcore/mockdata/SPIData.cpp deleted file mode 100644 index 433ca100ead..00000000000 --- a/hal/src/main/native/systemcore/mockdata/SPIData.cpp +++ /dev/null @@ -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" diff --git a/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp b/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp deleted file mode 100644 index 18e8ab3fbb6..00000000000 --- a/hal/src/test/native/cpp/mockdata/SPIDataTest.cpp +++ /dev/null @@ -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 - -#include - -#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 diff --git a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp deleted file mode 100644 index 7d42876c019..00000000000 --- a/wpilibc/src/main/native/cpp/ADIS16448_IMU.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#include -#include -#include - -#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(buf[0]) << 8) | buf[1]; -} - -static inline int16_t BuffToShort(const uint32_t* buf) { - return (static_cast(buf[0]) << 8) | buf[1]; -} - -using namespace frc; - -namespace { -template -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(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(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(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(buffer[i + k + 1]); - calc_crc = (calc_crc >> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ byte]; - // Process MSB - byte = static_cast(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((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); -} diff --git a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp b/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp deleted file mode 100644 index 3fef9505a55..00000000000 --- a/wpilibc/src/main/native/cpp/ADIS16470_IMU.cpp +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include - -#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((buf[0] << 24) | (buf[1] << 16) | (buf[2] << 8) | - buf[3]); -} - -static inline int16_t BuffToShort(const uint32_t* buf) { - return (static_cast(buf[0]) << 8) | buf[1]; -} - -using namespace frc; - -namespace { -template -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(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(new_cal_time)) { - return 1; - } - if (!SwitchToStandardSPI()) { - REPORT_ERROR("Failed to configure/reconfigure standard SPI."); - return 2; - } - m_calibration_time = static_cast(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(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); -} diff --git a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp b/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp deleted file mode 100644 index 1dc4c5b1651..00000000000 --- a/wpilibc/src/main/native/cpp/ADXL345_SPI.cpp +++ /dev/null @@ -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 -#include -#include -#include - -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(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(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); - }); -} diff --git a/wpilibc/src/main/native/cpp/ADXL362.cpp b/wpilibc/src/main/native/cpp/ADXL362.cpp deleted file mode 100644 index 425815e2fff..00000000000 --- a/wpilibc/src/main/native/cpp/ADXL362.cpp +++ /dev/null @@ -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 -#include -#include -#include - -#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((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(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); - }); -} diff --git a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp b/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp deleted file mode 100644 index 21e98e2ee3b..00000000000 --- a/wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp +++ /dev/null @@ -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 -#include -#include - -#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(buf[0]) << 24; - result |= static_cast(buf[1]) << 16; - result |= static_cast(buf[2]) << 8; - result |= static_cast(buf[3]); - return result; -} - -uint16_t ADXRS450_Gyro::ReadRegister(int reg) { - uint32_t cmd = 0x80000000 | static_cast(reg) << 17; - if (!CalcParity(cmd)) { - cmd |= 1u; - } - - // big endian - uint8_t buf[4] = {static_cast((cmd >> 24) & 0xff), - static_cast((cmd >> 16) & 0xff), - static_cast((cmd >> 8) & 0xff), - static_cast(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((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(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); -} diff --git a/wpilibc/src/main/native/cpp/SPI.cpp b/wpilibc/src/main/native/cpp/SPI.cpp deleted file mode 100644 index 88f8f1a9fde..00000000000 --- a/wpilibc/src/main/native/cpp/SPI.cpp +++ /dev/null @@ -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 -#include -#include - -#include -#include -#include -#include - -#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(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(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(m_validValue)) { - // valid sensor data; extract data field - int32_t data = static_cast(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(timestamp - m_lastTimestamp) * 1e-6 - - m_integratedCenter; - } else { - m_integratedValue += - dataNoCenter * - static_cast((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(port)) { - int32_t status = 0; - HAL_InitializeSPI(m_port, &status); - HAL_SetSPIMode(m_port, m_mode); - FRC_CheckErrorStatus(status, "Port {}", static_cast(m_port)); - - HAL_Report(HALUsageReporting::kResourceType_SPI, - static_cast(port) + 1); -} - -SPI::~SPI() = default; - -SPI::Port SPI::GetPort() const { - return static_cast(static_cast(m_port)); -} - -void SPI::SetClockRate(int hz) { - HAL_SetSPISpeed(m_port, hz); -} - -void SPI::SetMode(Mode mode) { - m_mode = static_cast(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(m_port)); -} - -void SPI::SetChipSelectActiveLow() { - int32_t status = 0; - HAL_SetSPIChipSelectActiveLow(m_port, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(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 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(m_port)); -} - -void SPI::FreeAuto() { - int32_t status = 0; - HAL_FreeSPIAuto(m_port, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(m_port)); -} - -void SPI::SetAutoTransmitData(std::span dataToSend, - int zeroSize) { - int32_t status = 0; - HAL_SetSPIAutoTransmitData(m_port, dataToSend.data(), dataToSend.size(), - zeroSize, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(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(m_port)); -} - -void SPI::StartAutoTrigger(DigitalSource& source, bool rising, bool falling) { - int32_t status = 0; - HAL_StartSPIAutoTrigger(m_port, source.GetPortHandleForRouting(), - static_cast( - source.GetAnalogTriggerTypeForRouting()), - rising, falling, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(m_port)); -} - -void SPI::StopAuto() { - int32_t status = 0; - HAL_StopSPIAuto(m_port, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(m_port)); -} - -void SPI::ForceAutoRead() { - int32_t status = 0; - HAL_ForceSPIAutoRead(m_port, &status); - FRC_CheckErrorStatus(status, "Port {}", static_cast(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(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(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(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(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(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); -} diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp deleted file mode 100644 index 46c412a4b1a..00000000000 --- a/wpilibc/src/main/native/cpp/simulation/ADIS16448_IMUSim.cpp +++ /dev/null @@ -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 -#include - -using namespace frc::sim; - -ADIS16448_IMUSim::ADIS16448_IMUSim(const frc::ADIS16448_IMU& imu) { - frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16448", imu.GetPort()}; - m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); - m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); - m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); - m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); - m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); - m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); - m_simAccelX = deviceSim.GetDouble("accel_x"); - m_simAccelY = deviceSim.GetDouble("accel_y"); - m_simAccelZ = deviceSim.GetDouble("accel_z"); -} - -void ADIS16448_IMUSim::SetGyroAngleX(units::degree_t angle) { - m_simGyroAngleX.Set(angle.value()); -} - -void ADIS16448_IMUSim::SetGyroAngleY(units::degree_t angle) { - m_simGyroAngleY.Set(angle.value()); -} - -void ADIS16448_IMUSim::SetGyroAngleZ(units::degree_t angle) { - m_simGyroAngleZ.Set(angle.value()); -} - -void ADIS16448_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) { - m_simGyroRateX.Set(angularRate.value()); -} - -void ADIS16448_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) { - m_simGyroRateY.Set(angularRate.value()); -} - -void ADIS16448_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) { - m_simGyroRateZ.Set(angularRate.value()); -} - -void ADIS16448_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { - m_simAccelX.Set(accel.value()); -} - -void ADIS16448_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) { - m_simAccelY.Set(accel.value()); -} - -void ADIS16448_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) { - m_simAccelZ.Set(accel.value()); -} diff --git a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp deleted file mode 100644 index 40c09c36260..00000000000 --- a/wpilibc/src/main/native/cpp/simulation/ADIS16470_IMUSim.cpp +++ /dev/null @@ -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 -#include - -using namespace frc::sim; - -ADIS16470_IMUSim::ADIS16470_IMUSim(const frc::ADIS16470_IMU& imu) { - frc::sim::SimDeviceSim deviceSim{"Gyro:ADIS16470", imu.GetPort()}; - m_simGyroAngleX = deviceSim.GetDouble("gyro_angle_x"); - m_simGyroAngleY = deviceSim.GetDouble("gyro_angle_y"); - m_simGyroAngleZ = deviceSim.GetDouble("gyro_angle_z"); - m_simGyroRateX = deviceSim.GetDouble("gyro_rate_x"); - m_simGyroRateY = deviceSim.GetDouble("gyro_rate_y"); - m_simGyroRateZ = deviceSim.GetDouble("gyro_rate_z"); - m_simAccelX = deviceSim.GetDouble("accel_x"); - m_simAccelY = deviceSim.GetDouble("accel_y"); - m_simAccelZ = deviceSim.GetDouble("accel_z"); -} - -void ADIS16470_IMUSim::SetGyroAngleX(units::degree_t angle) { - m_simGyroAngleX.Set(angle.value()); -} - -void ADIS16470_IMUSim::SetGyroAngleY(units::degree_t angle) { - m_simGyroAngleY.Set(angle.value()); -} - -void ADIS16470_IMUSim::SetGyroAngleZ(units::degree_t angle) { - m_simGyroAngleZ.Set(angle.value()); -} - -void ADIS16470_IMUSim::SetGyroRateX(units::degrees_per_second_t angularRate) { - m_simGyroRateX.Set(angularRate.value()); -} - -void ADIS16470_IMUSim::SetGyroRateY(units::degrees_per_second_t angularRate) { - m_simGyroRateY.Set(angularRate.value()); -} - -void ADIS16470_IMUSim::SetGyroRateZ(units::degrees_per_second_t angularRate) { - m_simGyroRateZ.Set(angularRate.value()); -} - -void ADIS16470_IMUSim::SetAccelX(units::meters_per_second_squared_t accel) { - m_simAccelX.Set(accel.value()); -} - -void ADIS16470_IMUSim::SetAccelY(units::meters_per_second_squared_t accel) { - m_simAccelY.Set(accel.value()); -} - -void ADIS16470_IMUSim::SetAccelZ(units::meters_per_second_squared_t accel) { - m_simAccelZ.Set(accel.value()); -} diff --git a/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp index 27f9c1fc560..f56570156fc 100644 --- a/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp +++ b/wpilibc/src/main/native/cpp/simulation/ADXL345Sim.cpp @@ -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); } diff --git a/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp deleted file mode 100644 index ac5bb5a0267..00000000000 --- a/wpilibc/src/main/native/cpp/simulation/ADXL362Sim.cpp +++ /dev/null @@ -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); -} diff --git a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp b/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp deleted file mode 100644 index a21d1b78ed8..00000000000 --- a/wpilibc/src/main/native/cpp/simulation/ADXRS450_GyroSim.cpp +++ /dev/null @@ -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()); -} diff --git a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp b/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp deleted file mode 100644 index d0737e642c0..00000000000 --- a/wpilibc/src/main/native/cpp/simulation/SPIAccelerometerSim.cpp +++ /dev/null @@ -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 - -#include - -using namespace frc; -using namespace frc::sim; - -SPIAccelerometerSim::SPIAccelerometerSim(int index) { - m_index = index; -} - -std::unique_ptr SPIAccelerometerSim::RegisterActiveCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - 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 SPIAccelerometerSim::RegisterRangeCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - 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 SPIAccelerometerSim::RegisterXCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - 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 SPIAccelerometerSim::RegisterYCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - 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 SPIAccelerometerSim::RegisterZCallback( - NotifyCallback callback, bool initialNotify) { - auto store = std::make_unique( - 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); -} diff --git a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h deleted file mode 100644 index 71d52b81603..00000000000 --- a/wpilibc/src/main/native/include/frc/ADIS16448_IMU.h +++ /dev/null @@ -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 - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#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 { - 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 m_thread_active = false; - std::atomic m_first_run = true; - std::atomic m_thread_idle = false; - std::atomic 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 diff --git a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h b/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h deleted file mode 100644 index a55800c416e..00000000000 --- a/wpilibc/src/main/native/include/frc/ADIS16470_IMU.h +++ /dev/null @@ -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 - -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include - -#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 { - 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. - * - * Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll - * will result in an error. - * - * @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. - * - * Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch, or - * kRoll will result in an error. - * - * @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 m_thread_active = false; - std::atomic m_first_run = true; - std::atomic 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 diff --git a/wpilibc/src/main/native/include/frc/ADXL345_SPI.h b/wpilibc/src/main/native/include/frc/ADXL345_SPI.h deleted file mode 100644 index 305be9280a2..00000000000 --- a/wpilibc/src/main/native/include/frc/ADXL345_SPI.h +++ /dev/null @@ -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 -#include -#include - -#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 { - 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 diff --git a/wpilibc/src/main/native/include/frc/ADXL362.h b/wpilibc/src/main/native/include/frc/ADXL362.h deleted file mode 100644 index 0332b3103fe..00000000000 --- a/wpilibc/src/main/native/include/frc/ADXL362.h +++ /dev/null @@ -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 -#include -#include - -#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 { - 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 diff --git a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h b/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h deleted file mode 100644 index d923e13ff27..00000000000 --- a/wpilibc/src/main/native/include/frc/ADXRS450_Gyro.h +++ /dev/null @@ -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 - -#include -#include -#include - -#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 { - 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 diff --git a/wpilibc/src/main/native/include/frc/SPI.h b/wpilibc/src/main/native/include/frc/SPI.h deleted file mode 100644 index 8f7f87349b4..00000000000 --- a/wpilibc/src/main/native/include/frc/SPI.h +++ /dev/null @@ -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 - -#include -#include - -#include -#include -#include - -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. - * - *

Mode 0 is Clock idle low, data sampled on rising edge - * - *

Mode 1 is Clock idle low, data sampled on falling edge - * - *

Mode 2 is Clock idle high, data sampled on falling edge - * - *

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 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 m_port; - HAL_SPIMode m_mode = HAL_SPIMode::HAL_SPI_kMode0; - - private: - void Init(); - - class Accumulator; - std::unique_ptr m_accum; -}; - -} // namespace frc diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h deleted file mode 100644 index 96186954ad3..00000000000 --- a/wpilibc/src/main/native/include/frc/simulation/ADIS16448_IMUSim.h +++ /dev/null @@ -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 -#include -#include -#include - -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 diff --git a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h b/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h deleted file mode 100644 index 7b43762b508..00000000000 --- a/wpilibc/src/main/native/include/frc/simulation/ADIS16470_IMUSim.h +++ /dev/null @@ -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 -#include -#include -#include - -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 diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h b/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h index 3cf6162a0a3..ea9f6f86563 100644 --- a/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h +++ b/wpilibc/src/main/native/include/frc/simulation/ADXL345Sim.h @@ -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. * diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h b/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h deleted file mode 100644 index cc63af27843..00000000000 --- a/wpilibc/src/main/native/include/frc/simulation/ADXL362Sim.h +++ /dev/null @@ -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 - -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 diff --git a/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h b/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h deleted file mode 100644 index 4cb47372ace..00000000000 --- a/wpilibc/src/main/native/include/frc/simulation/ADXRS450_GyroSim.h +++ /dev/null @@ -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 -#include -#include - -#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 diff --git a/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h b/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h deleted file mode 100644 index a84391e7bc9..00000000000 --- a/wpilibc/src/main/native/include/frc/simulation/SPIAccelerometerSim.h +++ /dev/null @@ -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 - -#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 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 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 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 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 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 diff --git a/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp deleted file mode 100644 index 2495ad04b18..00000000000 --- a/wpilibc/src/test/native/cpp/simulation/ADXL345SimTest.cpp +++ /dev/null @@ -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 -#include - -#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 diff --git a/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp deleted file mode 100644 index 3658ffd138c..00000000000 --- a/wpilibc/src/test/native/cpp/simulation/ADXL362SimTest.cpp +++ /dev/null @@ -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 -#include - -#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 diff --git a/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp b/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp deleted file mode 100644 index ea99f80a77e..00000000000 --- a/wpilibc/src/test/native/cpp/simulation/ADXRS450_GyroSimTest.cpp +++ /dev/null @@ -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 -#include - -#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 diff --git a/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp index 49622326bf0..7c850b3017f 100644 --- a/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp +++ b/wpilibc/src/test/native/cpp/simulation/SimInitializationTest.cpp @@ -20,7 +20,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; @@ -40,7 +39,6 @@ TEST(SimInitializationTest, AllInitialize) { PWMSim pwmsim{0}; RoboRioSim rrsim; (void)rrsim; - SPIAccelerometerSim sasim{0}; DutyCycleSim dcsim = DutyCycleSim::CreateForIndex(0); (void)dcsim; AddressableLEDSim adLED; diff --git a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h index df50dcda092..7b8ccae777a 100644 --- a/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/MecanumControllerCommand/include/subsystems/DriveSubsystem.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include #include #include @@ -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; diff --git a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h index 6a9c32b9782..633bf65a1e2 100644 --- a/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h +++ b/wpilibcExamples/src/main/cpp/examples/RapidReactCommandBot/include/subsystems/Drive.h @@ -6,7 +6,7 @@ #include -#include +#include #include #include #include @@ -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 m_controller{ DriveConstants::kTurnP, diff --git a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h index c428fd7f347..fd959307a7d 100644 --- a/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h +++ b/wpilibcExamples/src/main/cpp/examples/SwerveControllerCommand/include/subsystems/DriveSubsystem.h @@ -4,7 +4,7 @@ #pragma once -#include +#include #include #include #include @@ -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 diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java deleted file mode 100644 index 7ef4c17eb2c..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16448_IMU.java +++ /dev/null @@ -1,1078 +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 */ -/*----------------------------------------------------------------------------*/ - -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.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; - -// CHECKSTYLE.OFF: TypeName -// CHECKSTYLE.OFF: MemberName -// CHECKSTYLE.OFF: LocalVariableName -// CHECKSTYLE.OFF: ParameterName -// CHECKSTYLE.OFF: EmptyCatchBlock - -/** This class is for the ADIS16448 IMU that connects to the RoboRIO MXP port. */ -@SuppressWarnings({"PMD.RedundantFieldInitializer", "PMD.ImmutableField"}) -public class ADIS16448_IMU implements AutoCloseable, Sendable { - // ADIS16448 Register Map Declaration - private static final int FLASH_CNT = 0x00; // Flash memory write count - - private static final int XGYRO_OUT = 0x04; // X-axis gyroscope output - private static final int YGYRO_OUT = 0x06; // Y-axis gyroscope output - private static final int ZGYRO_OUT = 0x08; // Z-axis gyroscope output - private static final int XACCL_OUT = 0x0A; // X-axis accelerometer output - private static final int YACCL_OUT = 0x0C; // Y-axis accelerometer output - private static final int ZACCL_OUT = 0x0E; // Z-axis accelerometer output - private static final int XMAGN_OUT = 0x10; // X-axis magnetometer output - private static final int YMAGN_OUT = 0x12; // Y-axis magnetometer output - private static final int ZMAGN_OUT = 0x14; // Z-axis magnetometer output - private static final int BARO_OUT = 0x16; // Barometer pressure measurement, high word - private static final int TEMP_OUT = 0x18; // Temperature output - private static final int XGYRO_OFF = 0x1A; // X-axis gyroscope bias offset factor - private static final int YGYRO_OFF = 0x1C; // Y-axis gyroscope bias offset factor - private static final int ZGYRO_OFF = 0x1E; // Z-axis gyroscope bias offset factor - private static final int XACCL_OFF = 0x20; // X-axis acceleration bias offset factor - private static final int YACCL_OFF = 0x22; // Y-axis acceleration bias offset factor - private static final int ZACCL_OFF = 0x24; // Z-axis acceleration bias offset factor - private static final int XMAGN_HIC = 0x26; // X-axis magnetometer, hard iron factor - private static final int YMAGN_HIC = 0x28; // Y-axis magnetometer, hard iron factor - private static final int ZMAGN_HIC = 0x2A; // Z-axis magnetometer, hard iron factor - private static final int XMAGN_SIC = 0x2C; // X-axis magnetometer, soft iron factor - private static final int YMAGN_SIC = 0x2E; // Y-axis magnetometer, soft iron factor - private static final int ZMAGN_SIC = 0x30; // Z-axis magnetometer, soft iron factor - private static final int GPIO_CTRL = 0x32; // GPIO control - private static final int MSC_CTRL = 0x34; // MISC control - private static final int SMPL_PRD = 0x36; // Sample clock/Decimation filter control - private static final int SENS_AVG = 0x38; // Digital filter control - private static final int SEQ_CNT = 0x3A; // MAGN_OUT and BARO_OUT counter - private static final int DIAG_STAT = 0x3C; // System status - private static final int GLOB_CMD = 0x3E; // System command - private static final int ALM_MAG1 = 0x40; // Alarm 1 amplitude threshold - private static final int ALM_MAG2 = 0x42; // Alarm 2 amplitude threshold - private static final int ALM_SMPL1 = 0x44; // Alarm 1 sample size - private static final int ALM_SMPL2 = 0x46; // Alarm 2 sample size - private static final int ALM_CTRL = 0x48; // Alarm control - private static final int LOT_ID1 = 0x52; // Lot identification number - private static final int LOT_ID2 = 0x54; // Lot identification number - private static final int PROD_ID = 0x56; // Product identifier - private static final int SERIAL_NUM = 0x58; // Lot-specific serial number - - /** ADIS16448 calibration times. */ - public enum 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); - - private final int value; - - CalibrationTime(int value) { - this.value = value; - } - } - - /** IMU axes. */ - public enum IMUAxis { - /** The IMU's X axis. */ - kX, - /** The IMU's Y axis. */ - kY, - /** The IMU's Z axis. */ - kZ - } - - private static final double kGrav = 9.81; - - // User-specified yaw axis - private IMUAxis m_yaw_axis; - - // Offset data storage - private double[] m_offset_data_gyro_rate_x; - private double[] m_offset_data_gyro_rate_y; - private double[] m_offset_data_gyro_rate_z; - - // Instant raw output variables - private double m_gyro_rate_x = 0.0; - private double m_gyro_rate_y = 0.0; - private double m_gyro_rate_z = 0.0; - private double m_accel_x = 0.0; - private double m_accel_y = 0.0; - private double m_accel_z = 0.0; - private double m_mag_x = 0.0; - private double m_mag_y = 0.0; - private double m_mag_z = 0.0; - private double m_baro = 0.0; - private double m_temp = 0.0; - - // IMU gyro offset variables - private double m_gyro_rate_offset_x = 0.0; - private double m_gyro_rate_offset_y = 0.0; - private double m_gyro_rate_offset_z = 0.0; - private int m_avg_size = 0; - private int m_accum_count = 0; - - // Integrated gyro angle variables - private double m_integ_gyro_angle_x = 0.0; - private double m_integ_gyro_angle_y = 0.0; - private double m_integ_gyro_angle_z = 0.0; - - // Complementary filter variables - private double m_dt = 0.0; - private double m_alpha = 0.0; - private static final double kTau = 1.0; - private double m_compAngleX = 0.0; - private double m_compAngleY = 0.0; - private double m_accelAngleX = 0.0; - private double m_accelAngleY = 0.0; - - // State variables - private volatile boolean m_thread_active = false; - private CalibrationTime m_calibration_time = CalibrationTime._32ms; - private volatile boolean m_first_run = true; - private volatile boolean m_thread_idle = false; - private boolean m_auto_configured = false; - private boolean m_start_up_mode = true; - - // Resources - private SPI m_spi; - private SPI.Port m_spi_port; - private DigitalInput m_auto_interrupt; - private DigitalInput m_reset_in; - private DigitalOutput m_status_led; - private Thread m_acquire_task; - private boolean m_connected; - - private SimDevice m_simDevice; - private SimBoolean m_simConnected; - private SimDouble m_simGyroAngleX; - private SimDouble m_simGyroAngleY; - private SimDouble m_simGyroAngleZ; - private SimDouble m_simGyroRateX; - private SimDouble m_simGyroRateY; - private SimDouble m_simGyroRateZ; - private SimDouble m_simAccelX; - private SimDouble m_simAccelY; - private SimDouble m_simAccelZ; - - // CRC-16 Look-Up Table - private int[] m_adiscrc = { - 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 - }; - - /** Creates a new ADIS16448_IMU object. */ - public ADIS16448_IMU() { - this(IMUAxis.kZ, SPI.Port.kMXP, CalibrationTime._512ms); - } - - /** - * Creates a new ADIS16448_IMU object. - * - * @param yaw_axis The axis that measures the yaw - * @param port The SPI Port the gyro is plugged into - * @param cal_time Calibration time - */ - @SuppressWarnings("this-escape") - public ADIS16448_IMU(final IMUAxis yaw_axis, SPI.Port port, CalibrationTime cal_time) { - m_yaw_axis = yaw_axis; - m_spi_port = port; - - m_acquire_task = new Thread(this::acquire); - - m_simDevice = SimDevice.create("Gyro:ADIS16448", port.value); - if (m_simDevice != null) { - m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); - m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); - m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); - m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); - m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); - m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); - m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); - m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); - m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); - m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); - } - - if (m_simDevice == null) { - // Force the IMU reset pin to toggle on startup (doesn't require DS enable) - var reset_out = new DigitalOutput(18); // Drive MXP DIO8 low - Timer.delay(0.01); - reset_out.close(); - m_reset_in = new DigitalInput(18); // Set MXP DIO8 high - Timer.delay(0.25); // Wait for reset to complete - - configCalTime(cal_time); - - if (!switchToStandardSPI()) { - return; - } - boolean needsFlash = false; - // Set IMU internal decimation to 1 (ODR = 819.2 SPS / (1 + 1) = 409.6Hz), BW = 204.8Hz - if (readRegister(SMPL_PRD) != 0x0001) { - writeRegister(SMPL_PRD, 0x0001); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: SMPL_PRD register configuration inconsistent! Scheduling flash update.", - false); - } - - // Set data ready polarity (LOW = Good Data) on DIO1 (PWM0 on MXP) - if (readRegister(MSC_CTRL) != 0x0016) { - writeRegister(MSC_CTRL, 0x0016); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: MSC_CTRL register configuration inconsistent! Scheduling flash update.", - false); - } - - // Disable IMU internal Bartlett filter (204Hz BW is sufficient) and set IMU scale factor - if (readRegister(SENS_AVG) != 0x0400) { - writeRegister(SENS_AVG, 0x0400); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: SENS_AVG register configuration inconsistent! Scheduling flash update.", - false); - } - // Clear offset registers - if (readRegister(XGYRO_OFF) != 0x0000) { - writeRegister(XGYRO_OFF, 0x0000); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: XGYRO_OFF register configuration inconsistent! Scheduling flash update.", - false); - } - - if (readRegister(YGYRO_OFF) != 0x0000) { - writeRegister(YGYRO_OFF, 0x0000); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: YGYRO_OFF register configuration inconsistent! Scheduling flash update.", - false); - } - - if (readRegister(ZGYRO_OFF) != 0x0000) { - writeRegister(ZGYRO_OFF, 0x0000); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16448: ZGYRO_OFF register configuration inconsistent! Scheduling flash update.", - false); - } - - // If any registers on the IMU don't match the config, trigger a flash update - if (needsFlash) { - DriverStation.reportWarning( - "ADIS16448: Register configuration changed! Starting IMU flash update.", false); - writeRegister(GLOB_CMD, 0x0008); - // Wait long enough for the flash update to finish (75ms minimum as per the datasheet) - Timer.delay(0.5); - DriverStation.reportWarning("ADIS16448: Flash update finished!", false); - } else { - DriverStation.reportWarning( - "ADIS16448: Flash and RAM configuration consistent. No flash update required!", false); - } - - if (!switchToAutoSPI()) { - return; - } - // Notify DS that IMU calibration delay is active - DriverStation.reportWarning("ADIS16448: Starting initial calibration delay.", false); - // Wait for whatever time the user set as the start-up delay - try { - Thread.sleep((long) (m_calibration_time.value * 1.2 * 1000)); - } catch (InterruptedException e) { - } - // Execute calibration routine and reset accumulated offsets - calibrate(); - // Indicate to the acquire loop that we're done starting up - m_start_up_mode = false; - // Let the user know the IMU was initialized successfully - DriverStation.reportWarning("ADIS16448 IMU Successfully Initialized!", false); - // Drive MXP PWM5 (IMU ready LED) low (active low) - m_status_led = new DigitalOutput(19); - } - - // Report usage and post data to DS - HAL.report(tResourceType.kResourceType_ADIS16448, 0); - m_connected = true; - } - - /** - * Checks the connection status of the IMU. - * - * @return True if the IMU is connected, false otherwise. - */ - public boolean isConnected() { - if (m_simConnected != null) { - return m_simConnected.get(); - } - return m_connected; - } - - private static int toUShort(int upper, int lower) { - return ((upper & 0xFF) << 8) + (lower & 0xFF); - } - - private static int toShort(int upper, int lower) { - return (short) (((upper & 0xFF) << 8) + (lower & 0xFF)); - } - - private boolean 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) { - try { - Thread.sleep(10); - } catch (InterruptedException e) { - } - } - System.out.println("Paused the IMU processing thread successfully!"); - // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != null && 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. - int[] trashBuffer = new int[200]; - try { - Thread.sleep(100); - } catch (InterruptedException e) { - } - int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); - while (data_count > 0) { - // Dequeue 200 at a time, or the remainder of the buffer if less than 200 - m_spi.readAutoReceivedData(trashBuffer, Math.min(200, data_count), 0); - // Update remaining buffer count - data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); - } - System.out.println("Paused auto SPI successfully."); - } - } - if (m_spi == null) { - m_spi = new SPI(m_spi_port); - m_spi.setClockRate(1000000); - m_spi.setMode(SPI.Mode.kMode3); - m_spi.setChipSelectActiveLow(); - } - readRegister(PROD_ID); // Dummy read - // Validate the product ID - if (readRegister(PROD_ID) != 16448) { - DriverStation.reportError("Could not find ADIS16448", false); - close(); - return false; - } - return true; - } - - boolean switchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == null && !switchToStandardSPI()) { - DriverStation.reportError("Failed to start/restart auto SPI", false); - return false; - } - // Only set up the interrupt if needed. - if (m_auto_interrupt == null) { - m_auto_interrupt = new DigitalInput(10); // MXP DIO0 - } - // 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(new byte[] {GLOB_CMD}, 27); - // Configure auto stall time - m_spi.configureAutoStall(100, 1000, 255); - // Kick off auto SPI (Note: Device configuration impossible after auto SPI 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_acquire_task.isAlive()) { - m_first_run = true; - m_thread_active = true; - m_acquire_task.start(); - System.out.println("New IMU Processing thread activated!"); - } else { - // The thread was running, re-init run variables and start it up again. - m_first_run = true; - m_thread_active = true; - System.out.println("Old IMU Processing thread re-activated!"); - } - // Looks like the thread didn't start for some reason. Abort. - if (!m_acquire_task.isAlive()) { - DriverStation.reportError("Failed to start/restart the acquire() thread.", false); - close(); - return false; - } - return true; - } - - /** - * Configures the decimation rate of the IMU. - * - * @param decimationRate The new decimation value. - * @return 0 if success, 1 if no change, 2 if error. - */ - public int configDecRate(int 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()) { - DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); - return 2; - } - - // Check max - if (decimationRate > 9) { - DriverStation.reportError( - "Attempted to write an invalid decimation value. Capping at 9", false); - decimationRate = 9; - } - if (decimationRate < 0) { - DriverStation.reportError( - "Attempted to write an invalid decimation value. Capping at 0", false); - decimationRate = 0; - } - - // Shift decimation setting to correct position and select internal sync - int writeValue = (decimationRate << 8) | 0x1; - - // Apply to IMU - writeRegister(SMPL_PRD, writeValue); - - // Perform read back to verify write - int readbackValue = readRegister(SMPL_PRD); - - // Throw error for invalid write - if (readbackValue != writeValue) { - DriverStation.reportError("ADIS16448 SMPL_PRD write failed.", false); - } - - if (!switchToAutoSPI()) { - DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); - return 2; - } - return 0; - } - - /** - * Configures calibration time. - * - * @param new_cal_time New calibration time - * @return 1 if the new calibration time is the same as the current one else 0 - */ - public final int configCalTime(CalibrationTime new_cal_time) { - if (m_calibration_time == new_cal_time) { - return 1; - } else { - m_calibration_time = new_cal_time; - m_avg_size = m_calibration_time.value * 819; - initOffsetBuffer(m_avg_size); - return 0; - } - } - - private void 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; - synchronized (this) { - // Resize vector - m_offset_data_gyro_rate_x = new double[size]; - m_offset_data_gyro_rate_y = new double[size]; - m_offset_data_gyro_rate_z = new double[size]; - // Set accumulate count to 0 - m_accum_count = 0; - } - } - - /** - * Calibrate the gyro. It's important to make sure that the robot is not moving while the - * calibration is in progress, this is typically done when the robot is first turned on while it's - * sitting at rest before the match starts. - */ - public void calibrate() { - synchronized (this) { - int gyroAverageSize = Math.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_data_gyro_rate_x[i]; - accum_gyro_rate_y += m_offset_data_gyro_rate_y[i]; - accum_gyro_rate_z += m_offset_data_gyro_rate_z[i]; - } - 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; - reset(); - } - } - - /** - * Sets the yaw axis. - * - * @param yaw_axis The new yaw axis to use - * @return 1 if the new yaw axis is the same as the current one else 0. - */ - public int setYawAxis(IMUAxis yaw_axis) { - if (m_yaw_axis == yaw_axis) { - return 1; - } - m_yaw_axis = yaw_axis; - reset(); - return 0; - } - - private int readRegister(final int reg) { - final byte[] buf = {(byte) (reg & 0x7f), 0}; - - m_spi.write(buf, 2); - m_spi.read(false, buf, 2); - - return toUShort(buf[0], buf[1]); - } - - private void writeRegister(final int reg, final int val) { - final byte[] buf = new byte[2]; - // low byte - buf[0] = (byte) (0x80 | reg); - buf[1] = (byte) (val & 0xff); - m_spi.write(buf, 2); - // high byte - buf[0] = (byte) (0x81 | reg); - buf[1] = (byte) (val >> 8); - m_spi.write(buf, 2); - } - - /** - * 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. - */ - public void reset() { - synchronized (this) { - m_integ_gyro_angle_x = 0.0; - m_integ_gyro_angle_y = 0.0; - m_integ_gyro_angle_z = 0.0; - } - } - - /** Delete (free) the spi port used for the IMU. */ - @Override - public void close() { - if (m_thread_active) { - m_thread_active = false; - try { - if (m_acquire_task != null) { - m_acquire_task.join(); - m_acquire_task = null; - } - } catch (InterruptedException e) { - } - if (m_spi != null) { - if (m_auto_configured) { - m_spi.stopAuto(); - } - m_spi.close(); - m_auto_configured = false; - if (m_auto_interrupt != null) { - m_auto_interrupt.close(); - m_auto_interrupt = null; - } - m_spi = null; - } - } - if (m_simDevice != null) { - m_simDevice.close(); - m_simDevice = null; - } - System.out.println("Finished cleaning up after the IMU driver."); - } - - private void acquire() { - // Set data packet length - final int dataset_len = 29; // 18 data points + timestamp - final int BUFFER_SIZE = 4000; - - // Set up buffers and variables - int[] buffer = new int[BUFFER_SIZE]; - double previous_timestamp = 0.0; - double compAngleX = 0.0; - double compAngleY = 0.0; - while (true) { - // Wait for data - try { - Thread.sleep(5); - } catch (InterruptedException e) { - } - - 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); - // 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; - if (data_to_read > BUFFER_SIZE) { - DriverStation.reportWarning( - "ADIS16448 data processing thread overrun has occurred!", false); - 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); - - // 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 - int 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 - calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k + 1]]; - // Process MSB - calc_crc = (calc_crc >>> 8) ^ m_adiscrc[(calc_crc & 0xFF) ^ buffer[i + k]]; - } - // Complement - calc_crc = ~calc_crc & 0xFFFF; - // Flip LSB & MSB - calc_crc = ((calc_crc << 8) | (calc_crc >> 8)) & 0xFFFF; - // Extract DUT CRC from data buffer - int imu_crc = toUShort(buffer[i + 27], buffer[i + 28]); - - 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 = toShort(buffer[i + 5], buffer[i + 6]) * 0.04; - double gyro_rate_y = toShort(buffer[i + 7], buffer[i + 8]) * 0.04; - double gyro_rate_z = toShort(buffer[i + 9], buffer[i + 10]) * 0.04; - double accel_x = toShort(buffer[i + 11], buffer[i + 12]) * 0.833; - double accel_y = toShort(buffer[i + 13], buffer[i + 14]) * 0.833; - double accel_z = toShort(buffer[i + 15], buffer[i + 16]) * 0.833; - double mag_x = toShort(buffer[i + 17], buffer[i + 18]) * 0.1429; - double mag_y = toShort(buffer[i + 19], buffer[i + 20]) * 0.1429; - double mag_z = toShort(buffer[i + 21], buffer[i + 22]) * 0.1429; - double baro = toShort(buffer[i + 23], buffer[i + 24]) * 0.02; - double temp = toShort(buffer[i + 25], buffer[i + 26]) * 0.07386 + 31.0; - - // Convert scaled sensor data to SI units (for tilt calculations) - // TODO: Should the unit outputs be selectable? - double gyro_rate_x_si = Math.toRadians(gyro_rate_x); - double gyro_rate_y_si = Math.toRadians(gyro_rate_y); - // double gyro_rate_z_si = Math.toRadians(gyro_rate_z); - 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 = Math.atan2(-accel_x_si, Math.hypot(accel_y_si, -accel_z_si)); - double accelAngleY = Math.atan2(accel_y_si, Math.hypot(-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 - synchronized (this) { - // Ignore first, integrated sample - if (m_first_run) { - reset(); - } else { - // Accumulate gyro for offset calibration - // Add to buffer - int bufferAvgIndex = m_accum_count % m_avg_size; - m_offset_data_gyro_rate_x[bufferAvgIndex] = gyro_rate_x; - m_offset_data_gyro_rate_y[bufferAvgIndex] = gyro_rate_y; - m_offset_data_gyro_rate_z[bufferAvgIndex] = gyro_rate_z; - // Increment counter - m_accum_count++; - } - 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 = Math.toDegrees(compAngleX); - m_compAngleY = Math.toDegrees(compAngleY); - m_accelAngleX = Math.toDegrees(accelAngleX); - m_accelAngleY = Math.toDegrees(accelAngleY); - // 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; - } - } - } - - private double formatFastConverge(double compAngle, double accAngle) { - if (compAngle > accAngle + Math.PI) { - compAngle = compAngle - 2.0 * Math.PI; - } else if (accAngle > compAngle + Math.PI) { - compAngle = compAngle + 2.0 * Math.PI; - } - return compAngle; - } - - private double formatAccelRange(double accelAngle, double accelZ) { - if (accelZ < 0.0) { - accelAngle = Math.PI - accelAngle; - } else if (accelZ > 0.0 && accelAngle < 0.0) { - accelAngle = 2.0 * Math.PI + accelAngle; - } - return accelAngle; - } - - private double compFilterProcess(double compAngle, double accelAngle, double omega) { - compAngle = formatFastConverge(compAngle, accelAngle); - compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; - return MathUtil.angleModulus(compAngle); - } - - /** - * Returns the yaw axis angle in degrees (CCW positive). - * - * @return Yaw axis angle in degrees (CCW positive). - */ - public synchronized double getAngle() { - return switch (m_yaw_axis) { - case kX -> getGyroAngleX(); - case kY -> getGyroAngleY(); - case kZ -> getGyroAngleZ(); - }; - } - - /** - * Returns the yaw axis angular rate in degrees per second (CCW positive). - * - * @return Yaw axis angular rate in degrees per second (CCW positive). - */ - public synchronized double getRate() { - return switch (m_yaw_axis) { - case kX -> getGyroRateX(); - case kY -> getGyroRateY(); - case kZ -> getGyroRateZ(); - }; - } - - /** - * Returns which axis, kX, kY, or kZ, is set to the yaw axis. - * - * @return IMUAxis Yaw Axis - */ - public IMUAxis getYawAxis() { - return m_yaw_axis; - } - - /** - * Returns the accumulated gyro angle in the X axis in degrees. - * - * @return The accumulated gyro angle in the X axis in degrees. - */ - public synchronized double getGyroAngleX() { - if (m_simGyroAngleX != null) { - return m_simGyroAngleX.get(); - } - return m_integ_gyro_angle_x; - } - - /** - * Returns the accumulated gyro angle in the Y axis in degrees. - * - * @return The accumulated gyro angle in the Y axis in degrees. - */ - public synchronized double getGyroAngleY() { - if (m_simGyroAngleY != null) { - return m_simGyroAngleY.get(); - } - return m_integ_gyro_angle_y; - } - - /** - * Returns the accumulated gyro angle in the Z axis in degrees. - * - * @return The accumulated gyro angle in the Z axis in degrees. - */ - public synchronized double getGyroAngleZ() { - if (m_simGyroAngleZ != null) { - return m_simGyroAngleZ.get(); - } - return m_integ_gyro_angle_z; - } - - /** - * Returns the gyro angular rate in the X axis in degrees per second. - * - * @return The gyro angular rate in the X axis in degrees per second. - */ - public synchronized double getGyroRateX() { - if (m_simGyroRateX != null) { - return m_simGyroRateX.get(); - } - return m_gyro_rate_x; - } - - /** - * Returns the gyro angular rate in the Y axis in degrees per second. - * - * @return The gyro angular rate in the Y axis in degrees per second. - */ - public synchronized double getGyroRateY() { - if (m_simGyroRateY != null) { - return m_simGyroRateY.get(); - } - return m_gyro_rate_y; - } - - /** - * Returns the gyro angular rate in the Z axis in degrees per second. - * - * @return The gyro angular rate in the Z axis in degrees per second. - */ - public synchronized double getGyroRateZ() { - if (m_simGyroRateZ != null) { - return m_simGyroRateZ.get(); - } - return m_gyro_rate_z; - } - - /** - * Returns the acceleration in the X axis in meters per second squared. - * - * @return The acceleration in the X axis in meters per second squared. - */ - public synchronized double getAccelX() { - if (m_simAccelX != null) { - return m_simAccelX.get(); - } - return m_accel_x * kGrav; - } - - /** - * Returns the acceleration in the Y axis in meters per second squared. - * - * @return The acceleration in the Y axis in meters per second squared. - */ - public synchronized double getAccelY() { - if (m_simAccelY != null) { - return m_simAccelY.get(); - } - return m_accel_y * kGrav; - } - - /** - * Returns the acceleration in the Z axis in meters per second squared. - * - * @return The acceleration in the Z axis in meters per second squared. - */ - public synchronized double getAccelZ() { - if (m_simAccelZ != null) { - return m_simAccelZ.get(); - } - return m_accel_z * kGrav; - } - - /** - * Returns the magnetic field strength in the X axis in Tesla. - * - * @return The magnetic field strength in the X axis in Tesla. - */ - public synchronized double getMagneticFieldX() { - // mG to T - return m_mag_x * 1e-7; - } - - /** - * Returns the magnetic field strength in the Y axis in Tesla. - * - * @return The magnetic field strength in the Y axis in Tesla. - */ - public synchronized double getMagneticFieldY() { - // mG to T - return m_mag_y * 1e-7; - } - - /** - * Returns the magnetic field strength in the Z axis in Tesla. - * - * @return The magnetic field strength in the Z axis in Tesla. - */ - public synchronized double getMagneticFieldZ() { - // mG to T - return m_mag_z * 1e-7; - } - - /** - * Returns the complementary angle around the X axis computed from accelerometer and gyro rate - * measurements. - * - * @return The X-axis complementary angle in degrees. - */ - public synchronized double getXComplementaryAngle() { - return m_compAngleX; - } - - /** - * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate - * measurements. - * - * @return The Y-axis complementary angle in degrees. - */ - public synchronized double getYComplementaryAngle() { - return m_compAngleY; - } - - /** - * Returns the X-axis filtered acceleration angle in degrees. - * - * @return The X-axis filtered acceleration angle in degrees. - */ - public synchronized double getXFilteredAccelAngle() { - return m_accelAngleX; - } - - /** - * Returns the Y-axis filtered acceleration angle in degrees. - * - * @return The Y-axis filtered acceleration angle in degrees. - */ - public synchronized double getYFilteredAccelAngle() { - return m_accelAngleY; - } - - /** - * Returns the barometric pressure in PSI. - * - * @return The barometric pressure in PSI. - */ - public synchronized double getBarometricPressure() { - // mbar to PSI conversion - return m_baro * 0.0145; - } - - /** - * Returns the temperature in degrees Celsius. - * - * @return The temperature in degrees Celsius. - */ - public synchronized double getTemperature() { - return m_temp; - } - - /** - * Get the SPI port number. - * - * @return The SPI port number. - */ - public int getPort() { - return m_spi_port.value; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Value", this::getAngle, null); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java deleted file mode 100644 index c2700b82c9b..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADIS16470_IMU.java +++ /dev/null @@ -1,1084 +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.MathUtil; -import edu.wpi.first.util.sendable.Sendable; -import edu.wpi.first.util.sendable.SendableBuilder; - -// CHECKSTYLE.OFF: TypeName -// CHECKSTYLE.OFF: MemberName -// CHECKSTYLE.OFF: LocalVariableName -// CHECKSTYLE.OFF: ParameterName -// CHECKSTYLE.OFF: EmptyCatchBlock - -/** This class is for the ADIS16470 IMU that connects to the RoboRIO SPI port. */ -@SuppressWarnings("PMD.RedundantFieldInitializer") -public class ADIS16470_IMU implements AutoCloseable, Sendable { - /* ADIS16470 Register Map Declaration */ - private static final int FLASH_CNT = 0x00; // Flash memory write count - private static final int DIAG_STAT = 0x02; // Diagnostic and operational status - private static final int X_GYRO_LOW = 0x04; // X-axis gyroscope output, lower word - private static final int X_GYRO_OUT = 0x06; // X-axis gyroscope output, upper word - private static final int Y_GYRO_LOW = 0x08; // Y-axis gyroscope output, lower word - private static final int Y_GYRO_OUT = 0x0A; // Y-axis gyroscope output, upper word - private static final int Z_GYRO_LOW = 0x0C; // Z-axis gyroscope output, lower word - private static final int Z_GYRO_OUT = 0x0E; // Z-axis gyroscope output, upper word - private static final int X_ACCL_LOW = 0x10; // X-axis accelerometer output, lower word - private static final int X_ACCL_OUT = 0x12; // X-axis accelerometer output, upper word - private static final int Y_ACCL_LOW = 0x14; // Y-axis accelerometer output, lower word - private static final int Y_ACCL_OUT = 0x16; // Y-axis accelerometer output, upper word - private static final int Z_ACCL_LOW = 0x18; // Z-axis accelerometer output, lower word - private static final int Z_ACCL_OUT = 0x1A; // Z-axis accelerometer output, upper word - private static final int TEMP_OUT = 0x1C; // Temperature output (internal, not calibrated) - private static final int TIME_STAMP = 0x1E; // PPS mode time stamp - private static final int X_DELTANG_LOW = 0x24; // X-axis delta angle output, lower word - private static final int X_DELTANG_OUT = 0x26; // X-axis delta angle output, upper word - private static final int Y_DELTANG_LOW = 0x28; // Y-axis delta angle output, lower word - private static final int Y_DELTANG_OUT = 0x2A; // Y-axis delta angle output, upper word - private static final int Z_DELTANG_LOW = 0x2C; // Z-axis delta angle output, lower word - private static final int Z_DELTANG_OUT = 0x2E; // Z-axis delta angle output, upper word - private static final int X_DELTVEL_LOW = 0x30; // X-axis delta velocity output, lower word - private static final int X_DELTVEL_OUT = 0x32; // X-axis delta velocity output, upper word - private static final int Y_DELTVEL_LOW = 0x34; // Y-axis delta velocity output, lower word - private static final int Y_DELTVEL_OUT = 0x36; // Y-axis delta velocity output, upper word - private static final int Z_DELTVEL_LOW = 0x38; // Z-axis delta velocity output, lower word - private static final int Z_DELTVEL_OUT = 0x3A; // Z-axis delta velocity output, upper word - private static final int XG_BIAS_LOW = - 0x40; // X-axis gyroscope bias offset correction, lower word - private static final int XG_BIAS_HIGH = - 0x42; // X-axis gyroscope bias offset correction, upper word - private static final int YG_BIAS_LOW = - 0x44; // Y-axis gyroscope bias offset correction, lower word - private static final int YG_BIAS_HIGH = - 0x46; // Y-axis gyroscope bias offset correction, upper word - private static final int ZG_BIAS_LOW = - 0x48; // Z-axis gyroscope bias offset correction, lower word - private static final int ZG_BIAS_HIGH = - 0x4A; // Z-axis gyroscope bias offset correction, upper word - private static final int XA_BIAS_LOW = - 0x4C; // X-axis accelerometer bias offset correction, lower word - private static final int XA_BIAS_HIGH = - 0x4E; // X-axis accelerometer bias offset correction, upper word - private static final int YA_BIAS_LOW = - 0x50; // Y-axis accelerometer bias offset correction, lower word - private static final int YA_BIAS_HIGH = - 0x52; // Y-axis accelerometer bias offset correction, upper word - private static final int ZA_BIAS_LOW = - 0x54; // Z-axis accelerometer bias offset correction, lower word - private static final int ZA_BIAS_HIGH = - 0x56; // Z-axis accelerometer bias offset correction, upper word - private static final int FILT_CTRL = 0x5C; // Filter control - private static final int MSC_CTRL = 0x60; // Miscellaneous control - private static final int UP_SCALE = 0x62; // Clock scale factor, PPS mode - private static final int DEC_RATE = 0x64; // Decimation rate control (output data rate) - private static final int NULL_CNFG = 0x66; // Auto-null configuration control - private static final int GLOB_CMD = 0x68; // Global commands - private static final int FIRM_REV = 0x6C; // Firmware revision - private static final int FIRM_DM = 0x6E; // Firmware revision date, month and day - private static final int FIRM_Y = 0x70; // Firmware revision date, year - private static final int PROD_ID = 0x72; // Product identification - private static final int SERIAL_NUM = 0x74; // Serial number (relative to assembly lot) - private static final int USER_SCR1 = 0x76; // User scratch register 1 - private static final int USER_SCR2 = 0x78; // User scratch register 2 - private static final int USER_SCR3 = 0x7A; // User scratch register 3 - private static final int FLSHCNT_LOW = 0x7C; // Flash update count, lower word - private static final int FLSHCNT_HIGH = 0x7E; // Flash update count, upper word - - // Weight between the previous and current gyro angles represents 1 second for the timestamp, - // this is the point at which we ignore the previous angle because it is too old to be of use. - // The IMU timestamp conversion is 1 = 49.02us, the value 1_000_000 is the number of microseconds - // we average over. - private static final double AVERAGE_RATE_SCALING_FACTOR = 49.02 / 1_000_000; - - private static final byte[] m_autospi_allAngle_packet = { - 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 - }; - - /** ADIS16470 calibration times. */ - public enum 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); - - private final int value; - - CalibrationTime(int value) { - this.value = value; - } - } - - /** - * 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. - */ - public 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, - } - - // Static Constants - private static final double kDeltaAngleSf = 2160.0 / 2147483648.0; // 2160 / (2^31) - private static final double kGrav = 9.81; - - // User-specified axes - private IMUAxis m_yaw_axis; - private IMUAxis m_pitch_axis; - private IMUAxis m_roll_axis; - - // Instant raw outputs - private double m_gyro_rate_x = 0.0; - private double m_gyro_rate_y = 0.0; - private double m_gyro_rate_z = 0.0; - private double m_accel_x = 0.0; - private double m_accel_y = 0.0; - private double m_accel_z = 0.0; - - // Integrated gyro angle - private double m_integ_angle_x = 0.0; - private double m_integ_angle_y = 0.0; - private double m_integ_angle_z = 0.0; - - // Complementary filter variables - private double m_dt = 0.0; - private double m_alpha = 0.0; - private static final double kTau = 1.0; - private double m_compAngleX = 0.0; - private double m_compAngleY = 0.0; - private double m_accelAngleX = 0.0; - private double m_accelAngleY = 0.0; - - // State variables - private volatile boolean m_thread_active = false; - private int m_calibration_time = 0; - private volatile boolean m_first_run = true; - private volatile boolean m_thread_idle = false; - private boolean m_auto_configured = false; - private double m_scaled_sample_rate = 2500.0; - - // Resources - private SPI m_spi; - private SPI.Port m_spi_port; - private DigitalInput m_auto_interrupt; - private DigitalInput m_reset_in; - private DigitalOutput m_status_led; - private Thread m_acquire_task; - private boolean m_connected; - - private SimDevice m_simDevice; - private SimBoolean m_simConnected; - private SimDouble m_simGyroAngleX; - private SimDouble m_simGyroAngleY; - private SimDouble m_simGyroAngleZ; - private SimDouble m_simGyroRateX; - private SimDouble m_simGyroRateY; - private SimDouble m_simGyroRateZ; - private SimDouble m_simAccelX; - private SimDouble m_simAccelY; - private SimDouble m_simAccelZ; - - /** - * 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. - */ - public ADIS16470_IMU() { - this(IMUAxis.kZ, IMUAxis.kX, IMUAxis.kY, SPI.Port.kOnboardCS0, CalibrationTime._1s); - } - - /** - * Creates a new ADIS16740 IMU object. - * - *

The default setup is the onboard SPI port with a calibration time of 1 second. - * - *

Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in - * an error. - * - * @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 - */ - public ADIS16470_IMU(IMUAxis yaw_axis, IMUAxis pitch_axis, IMUAxis roll_axis) { - this(yaw_axis, pitch_axis, roll_axis, SPI.Port.kOnboardCS0, CalibrationTime._1s); - } - - /** - * Creates a new ADIS16740 IMU object. - * - *

Input axes limited to kX, kY and kZ. Specifying kYaw, kPitch,or kRoll will result in - * an error. - * - * @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 - */ - @SuppressWarnings("this-escape") - public ADIS16470_IMU( - IMUAxis yaw_axis, - IMUAxis pitch_axis, - IMUAxis roll_axis, - SPI.Port port, - CalibrationTime cal_time) { - if (yaw_axis == IMUAxis.kYaw - || yaw_axis == IMUAxis.kPitch - || yaw_axis == IMUAxis.kRoll - || pitch_axis == IMUAxis.kYaw - || pitch_axis == IMUAxis.kPitch - || pitch_axis == IMUAxis.kRoll - || roll_axis == IMUAxis.kYaw - || roll_axis == IMUAxis.kPitch - || roll_axis == IMUAxis.kRoll) { - DriverStation.reportError( - "ADIS16470 constructor only allows IMUAxis.kX, IMUAxis.kY or IMUAxis.kZ as arguments.", - false); - DriverStation.reportError( - "Constructing ADIS with default axes. (IMUAxis.kZ is defined as Yaw)", false); - yaw_axis = IMUAxis.kZ; - pitch_axis = IMUAxis.kY; - roll_axis = IMUAxis.kX; - } - - m_yaw_axis = yaw_axis; - m_pitch_axis = pitch_axis; - m_roll_axis = roll_axis; - - m_calibration_time = cal_time.value; - m_spi_port = port; - - m_acquire_task = new Thread(this::acquire); - - m_simDevice = SimDevice.create("Gyro:ADIS16470", port.value); - if (m_simDevice != null) { - m_simConnected = m_simDevice.createBoolean("connected", SimDevice.Direction.kInput, true); - m_simGyroAngleX = m_simDevice.createDouble("gyro_angle_x", SimDevice.Direction.kInput, 0.0); - m_simGyroAngleY = m_simDevice.createDouble("gyro_angle_y", SimDevice.Direction.kInput, 0.0); - m_simGyroAngleZ = m_simDevice.createDouble("gyro_angle_z", SimDevice.Direction.kInput, 0.0); - m_simGyroRateX = m_simDevice.createDouble("gyro_rate_x", SimDevice.Direction.kInput, 0.0); - m_simGyroRateY = m_simDevice.createDouble("gyro_rate_y", SimDevice.Direction.kInput, 0.0); - m_simGyroRateZ = m_simDevice.createDouble("gyro_rate_z", SimDevice.Direction.kInput, 0.0); - m_simAccelX = m_simDevice.createDouble("accel_x", SimDevice.Direction.kInput, 0.0); - m_simAccelY = m_simDevice.createDouble("accel_y", SimDevice.Direction.kInput, 0.0); - m_simAccelZ = m_simDevice.createDouble("accel_z", SimDevice.Direction.kInput, 0.0); - } - - if (m_simDevice == null) { - // 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. - var reset_out = new DigitalOutput(27); // Drive SPI CS2 (IMU RST) low - Timer.delay(0.01); - reset_out.close(); - m_reset_in = new DigitalInput(27); // Set SPI CS2 (IMU RST) high - Timer.delay(0.25); // Wait for reset to complete - - if (!switchToStandardSPI()) { - return; - } - boolean needsFlash = false; - // Set IMU internal decimation to 4 (ODR = 2000 SPS / (4 + 1) = 400Hz), BW = 200Hz - if (readRegister(DEC_RATE) != 0x0004) { - writeRegister(DEC_RATE, 0x0004); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16470: DEC_RATE register configuration inconsistent! Scheduling flash update.", - false); - } - - // Set data ready polarity (HIGH = Good Data), Disable gSense Compensation and PoP - if (readRegister(MSC_CTRL) != 0x0001) { - writeRegister(MSC_CTRL, 0x0001); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16470: MSC_CTRL register configuration inconsistent! Scheduling flash update.", - false); - } - - // Disable IMU internal Bartlett filter (200Hz bandwidth is sufficient) - if (readRegister(FILT_CTRL) != 0x0000) { - writeRegister(FILT_CTRL, 0x0000); - needsFlash = true; - DriverStation.reportWarning( - "ADIS16470: FILT_CTRL register configuration inconsistent! Scheduling flash update.", - false); - } - - // If any registers on the IMU don't match the config, trigger a flash update - if (needsFlash) { - DriverStation.reportWarning( - "ADIS16470: Register configuration changed! Starting IMU flash update.", false); - writeRegister(GLOB_CMD, 0x0008); - // Wait long enough for the flash update to finish (72ms minimum as per the datasheet) - Timer.delay(0.3); - DriverStation.reportWarning("ADIS16470: Flash update finished!", false); - } else { - DriverStation.reportWarning( - "ADIS16470: Flash and RAM configuration consistent. No flash update required!", false); - } - - // Configure continuous bias calibration time based on user setting - writeRegister(NULL_CNFG, m_calibration_time | 0x0700); - - // Notify DS that IMU calibration delay is active - DriverStation.reportWarning("ADIS16470: Starting initial calibration delay.", false); - - // Wait for samples to accumulate internal to the IMU (110% of user-defined - // time) - try { - Thread.sleep((long) (Math.pow(2.0, m_calibration_time) / 2000 * 64 * 1.1 * 1000)); - } catch (InterruptedException e) { - } - - // Write offset calibration command to IMU - writeRegister(GLOB_CMD, 0x0001); - - if (!switchToAutoSPI()) { - return; - } - - // Let the user know the IMU was initialized successfully - DriverStation.reportWarning("ADIS16470 IMU Successfully Initialized!", false); - - // Drive "Ready" LED low - m_status_led = new DigitalOutput(28); // Set SPI CS3 (IMU Ready LED) low - } - - // Report usage and post data to DS - HAL.report(tResourceType.kResourceType_ADIS16470, 0); - m_connected = true; - } - - /** - * Checks the connection status of the IMU. - * - * @return True if the IMU is connected, false otherwise. - */ - public boolean isConnected() { - if (m_simConnected != null) { - return m_simConnected.get(); - } - return m_connected; - } - - private static int toUShort(int upper, int lower) { - return ((upper & 0xFF) << 8) + (lower & 0xFF); - } - - private static int toShort(int upper, int lower) { - return (short) (((upper & 0xFF) << 8) + (lower & 0xFF)); - } - - private static int toInt(int msb, int byte2, int byte3, int lsb) { - return (msb & 0xFF) << 24 | (byte2 & 0xFF) << 16 | (byte3 & 0xFF) << 8 | (lsb & 0xFF); - } - - /** - * Switch to standard SPI mode. - * - * @return True if successful, false otherwise. - */ - private boolean 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) { - try { - Thread.sleep(10); - } catch (InterruptedException e) { - } - } - System.out.println("Paused the IMU processing thread successfully!"); - // Maybe we're in auto SPI mode? If so, kill auto SPI, and then SPI. - if (m_spi != null && 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. - int[] trashBuffer = new int[200]; - try { - Thread.sleep(100); - } catch (InterruptedException e) { - } - int data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); - while (data_count > 0) { - m_spi.readAutoReceivedData(trashBuffer, Math.min(data_count, 200), 0); - data_count = m_spi.readAutoReceivedData(trashBuffer, 0, 0); - } - System.out.println("Paused auto SPI successfully."); - } - } - if (m_spi == null) { - m_spi = new SPI(m_spi_port); - m_spi.setClockRate(2000000); - m_spi.setMode(SPI.Mode.kMode3); - m_spi.setChipSelectActiveLow(); - } - readRegister(PROD_ID); // Dummy read - // Validate the product ID - if (readRegister(PROD_ID) != 16982) { - DriverStation.reportError("Could not find an ADIS16470", false); - close(); - return false; - } - return true; - } - - /** - * Switch to auto SPI mode. - * - * @return True if successful, false otherwise. - */ - boolean switchToAutoSPI() { - // No SPI port has been set up. Go set one up first. - if (m_spi == null && !switchToStandardSPI()) { - DriverStation.reportError("Failed to start/restart auto SPI", false); - return false; - } - // Only set up the interrupt if needed. - if (m_auto_interrupt == null) { - // Configure interrupt on SPI CS1 - 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(5, 1000, 1); - // Kick off auto SPI (Note: Device configuration impossible after auto SPI 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_acquire_task.isAlive()) { - m_first_run = true; - m_thread_active = true; - m_acquire_task.start(); - System.out.println("Processing thread activated!"); - } else { - // The thread was running, re-init run variables and start it up again. - m_first_run = true; - m_thread_active = true; - System.out.println("Processing thread activated!"); - } - // Looks like the thread didn't start for some reason. Abort. - if (!m_acquire_task.isAlive()) { - DriverStation.reportError("Failed to start/restart the acquire() thread.", false); - close(); - return false; - } - return true; - } - - /** - * Configures calibration time. - * - * @param new_cal_time New calibration time - * @return 0 if success, 1 if no change, 2 if error. - */ - public int configCalTime(CalibrationTime new_cal_time) { - if (m_calibration_time == new_cal_time.value) { - return 1; - } - if (!switchToStandardSPI()) { - DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); - return 2; - } - m_calibration_time = new_cal_time.value; - writeRegister(NULL_CNFG, m_calibration_time | 0x700); - if (!switchToAutoSPI()) { - DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); - return 2; - } - return 0; - } - - /** - * Configures the decimation rate of the IMU. - * - * @param decimationRate The new decimation value. - * @return 0 if success, 1 if no change, 2 if error. - */ - public int configDecRate(int 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()) { - DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); - return 2; - } - if (decimationRate > 1999) { - DriverStation.reportError("Attempted to write an invalid decimation value.", false); - decimationRate = 1999; - } - m_scaled_sample_rate = (decimationRate + 1.0) / 2000.0 * 1000000.0; - writeRegister(DEC_RATE, decimationRate); - System.out.println("Decimation register: " + readRegister(DEC_RATE)); - if (!switchToAutoSPI()) { - DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); - return 2; - } - return 0; - } - - /** - * Calibrate the gyro. It's important to make sure that the robot is not moving while the - * calibration is in progress, this is typically done when the robot is first turned on while it's - * sitting at rest before the match starts. - */ - public void calibrate() { - if (!switchToStandardSPI()) { - DriverStation.reportError("Failed to configure/reconfigure standard SPI.", false); - } - writeRegister(GLOB_CMD, 0x0001); - if (!switchToAutoSPI()) { - DriverStation.reportError("Failed to configure/reconfigure auto SPI.", false); - } - } - - private int readRegister(int reg) { - final byte[] buf = {(byte) (reg & 0x7f), 0}; - - m_spi.write(buf, 2); - m_spi.read(false, buf, 2); - - return toUShort(buf[0], buf[1]); - } - - private void writeRegister(int reg, int val) { - // low byte - final byte[] buf = {(byte) (0x80 | reg), (byte) (val & 0xff)}; - m_spi.write(buf, 2); - // high byte - buf[0] = (byte) (0x81 | reg); - buf[1] = (byte) (val >> 8); - m_spi.write(buf, 2); - } - - /** Delete (free) the spi port used for the IMU. */ - @Override - public void close() { - if (m_thread_active) { - m_thread_active = false; - try { - if (m_acquire_task != null) { - m_acquire_task.join(); - m_acquire_task = null; - } - } catch (InterruptedException e) { - } - if (m_spi != null) { - if (m_auto_configured) { - m_spi.stopAuto(); - } - m_spi.close(); - m_auto_configured = false; - if (m_auto_interrupt != null) { - m_auto_interrupt.close(); - m_auto_interrupt = null; - } - m_spi = null; - } - } - if (m_simDevice != null) { - m_simDevice.close(); - m_simDevice = null; - } - System.out.println("Finished cleaning up after the IMU driver."); - } - - private void acquire() { - // Set data packet length - final int dataset_len = 27; // 26 data points + timestamp - final int BUFFER_SIZE = 4000; - - // Set up buffers and variables - int[] buffer = new int[BUFFER_SIZE]; - double previous_timestamp = 0.0; - double compAngleX = 0.0; - double compAngleY = 0.0; - while (true) { - // Wait for data - try { - Thread.sleep(10); - } catch (InterruptedException e) { - } - - 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); - // 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) { - DriverStation.reportWarning( - "ADIS16470 data processing thread overrun has occurred!", false); - 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); - - // 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 all 3 axes 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], buffer[i + 4], buffer[i + 5], buffer[i + 6]) - * kDeltaAngleSf - / elapsed_time; - double delta_angle_y = - toInt(buffer[i + 7], buffer[i + 8], buffer[i + 9], buffer[i + 10]) - * kDeltaAngleSf - / elapsed_time; - double delta_angle_z = - toInt(buffer[i + 11], buffer[i + 12], buffer[i + 13], buffer[i + 14]) - * kDeltaAngleSf - / elapsed_time; - - double gyro_rate_x = toShort(buffer[i + 15], buffer[i + 16]) / 10.0; - double gyro_rate_y = toShort(buffer[i + 17], buffer[i + 18]) / 10.0; - double gyro_rate_z = toShort(buffer[i + 19], buffer[i + 20]) / 10.0; - - double accel_x = toShort(buffer[i + 21], buffer[i + 22]) / 800.0; - double accel_y = toShort(buffer[i + 23], buffer[i + 24]) / 800.0; - double accel_z = toShort(buffer[i + 25], buffer[i + 26]) / 800.0; - - // Convert scaled sensor data to SI units (for tilt calculations) - // TODO: Should the unit outputs be selectable? - double gyro_rate_x_si = Math.toRadians(gyro_rate_x); - double gyro_rate_y_si = Math.toRadians(gyro_rate_y); - // double gyro_rate_z_si = Math.toRadians(gyro_rate_z); - 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 = Math.atan2(accel_x_si, Math.hypot(accel_y_si, accel_z_si)); - double accelAngleY = Math.atan2(accel_y_si, Math.hypot(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); - } - - synchronized (this) { - // 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 - reset(); - } 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 = Math.toDegrees(compAngleX); - m_compAngleY = Math.toDegrees(compAngleY); - m_accelAngleX = Math.toDegrees(accelAngleX); - m_accelAngleY = Math.toDegrees(accelAngleY); - } - m_first_run = false; - } - } else { - m_thread_idle = true; - previous_timestamp = 0.0; - compAngleX = 0.0; - compAngleY = 0.0; - } - } - } - - private double formatFastConverge(double compAngle, double accAngle) { - if (compAngle > accAngle + Math.PI) { - compAngle = compAngle - 2.0 * Math.PI; - } else if (accAngle > compAngle + Math.PI) { - compAngle = compAngle + 2.0 * Math.PI; - } - return compAngle; - } - - private double formatAccelRange(double accelAngle, double accelZ) { - if (accelZ < 0.0) { - accelAngle = Math.PI - accelAngle; - } else if (accelZ > 0.0 && accelAngle < 0.0) { - accelAngle = 2.0 * Math.PI + accelAngle; - } - return accelAngle; - } - - private double compFilterProcess(double compAngle, double accelAngle, double omega) { - compAngle = formatFastConverge(compAngle, accelAngle); - compAngle = m_alpha * (compAngle + omega * m_dt) + (1.0 - m_alpha) * accelAngle; - return MathUtil.angleModulus(compAngle); - } - - /** - * 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. - */ - public void reset() { - synchronized (this) { - m_integ_angle_x = 0.0; - m_integ_angle_y = 0.0; - m_integ_angle_z = 0.0; - } - } - - /** - * 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 A double in degrees (CCW positive) - */ - public void setGyroAngle(IMUAxis axis, double angle) { - axis = - switch (axis) { - case kYaw -> m_yaw_axis; - case kPitch -> m_pitch_axis; - case kRoll -> m_roll_axis; - default -> axis; - }; - - switch (axis) { - case kX -> setGyroAngleX(angle); - case kY -> setGyroAngleY(angle); - case kZ -> setGyroAngleZ(angle); - default -> { - // NOP - } - } - } - - /** - * 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 A double in degrees (CCW positive) - */ - public void setGyroAngleX(double angle) { - synchronized (this) { - m_integ_angle_x = 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 A double in degrees (CCW positive) - */ - public void setGyroAngleY(double angle) { - synchronized (this) { - m_integ_angle_y = 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 A double in degrees (CCW positive) - */ - public void setGyroAngleZ(double angle) { - synchronized (this) { - m_integ_angle_z = angle; - } - } - - /** - * Returns the axis angle in degrees (CCW positive). - * - * @param axis The IMUAxis whose angle to return. - * @return The axis angle in degrees (CCW positive). - */ - public synchronized double getAngle(IMUAxis axis) { - axis = - switch (axis) { - case kYaw -> m_yaw_axis; - case kPitch -> m_pitch_axis; - case kRoll -> m_roll_axis; - default -> axis; - }; - - return switch (axis) { - case kX -> { - if (m_simGyroAngleX != null) { - yield m_simGyroAngleX.get(); - } - yield m_integ_angle_x; - } - case kY -> { - if (m_simGyroAngleY != null) { - yield m_simGyroAngleY.get(); - } - yield m_integ_angle_y; - } - case kZ -> { - if (m_simGyroAngleZ != null) { - yield m_simGyroAngleZ.get(); - } - yield m_integ_angle_z; - } - default -> 0.0; - }; - } - - /** - * Returns the Yaw axis angle in degrees (CCW positive). - * - * @return The Yaw axis angle in degrees (CCW positive). - */ - public synchronized double getAngle() { - return getAngle(m_yaw_axis); - } - - /** - * Returns the axis angular rate in degrees per second (CCW positive). - * - * @param axis The IMUAxis whose rate to return. - * @return Axis angular rate in degrees per second (CCW positive). - */ - public synchronized double getRate(IMUAxis axis) { - axis = - switch (axis) { - case kYaw -> m_yaw_axis; - case kPitch -> m_pitch_axis; - case kRoll -> m_roll_axis; - default -> axis; - }; - - return switch (axis) { - case kX -> { - if (m_simGyroRateX != null) { - yield m_simGyroRateX.get(); - } - yield m_gyro_rate_x; - } - case kY -> { - if (m_simGyroRateY != null) { - yield m_simGyroRateY.get(); - } - yield m_gyro_rate_y; - } - case kZ -> { - if (m_simGyroRateZ != null) { - yield m_simGyroRateZ.get(); - } - yield m_gyro_rate_z; - } - default -> 0.0; - }; - } - - /** - * Returns the Yaw axis angular rate in degrees per second (CCW positive). - * - * @return Yaw axis angular rate in degrees per second (CCW positive). - */ - public synchronized double getRate() { - return getRate(m_yaw_axis); - } - - /** - * Returns which axis, kX, kY, or kZ, is set to the yaw axis. - * - * @return IMUAxis Yaw Axis - */ - public IMUAxis getYawAxis() { - return m_yaw_axis; - } - - /** - * Returns which axis, kX, kY, or kZ, is set to the pitch axis. - * - * @return IMUAxis Pitch Axis - */ - public IMUAxis getPitchAxis() { - return m_pitch_axis; - } - - /** - * Returns which axis, kX, kY, or kZ, is set to the roll axis. - * - * @return IMUAxis Roll Axis - */ - public IMUAxis getRollAxis() { - return m_roll_axis; - } - - /** - * Returns the acceleration in the X axis in meters per second squared. - * - * @return The acceleration in the X axis in meters per second squared. - */ - public synchronized double getAccelX() { - return m_accel_x * kGrav; - } - - /** - * Returns the acceleration in the Y axis in meters per second squared. - * - * @return The acceleration in the Y axis in meters per second squared. - */ - public synchronized double getAccelY() { - return m_accel_y * kGrav; - } - - /** - * Returns the acceleration in the Z axis in meters per second squared. - * - * @return The acceleration in the Z axis in meters per second squared. - */ - public synchronized double getAccelZ() { - return m_accel_z * kGrav; - } - - /** - * Returns the complementary angle around the X axis computed from accelerometer and gyro rate - * measurements. - * - * @return The X-axis complementary angle in degrees. - */ - public synchronized double getXComplementaryAngle() { - return m_compAngleX; - } - - /** - * Returns the complementary angle around the Y axis computed from accelerometer and gyro rate - * measurements. - * - * @return The Y-axis complementary angle in degrees. - */ - public synchronized double getYComplementaryAngle() { - return m_compAngleY; - } - - /** - * Returns the X-axis filtered acceleration angle in degrees. - * - * @return The X-axis filtered acceleration angle in degrees. - */ - public synchronized double getXFilteredAccelAngle() { - return m_accelAngleX; - } - - /** - * Returns the Y-axis filtered acceleration angle in degrees. - * - * @return The Y-axis filtered acceleration angle in degrees. - */ - public synchronized double getYFilteredAccelAngle() { - return m_accelAngleY; - } - - /** - * Gets the SPI port number. - * - * @return The SPI port number. - */ - public int getPort() { - return m_spi_port.value; - } - - @Override - public void initSendable(SendableBuilder builder) { - builder.setSmartDashboardType("Gyro"); - builder.addDoubleProperty("Value", () -> getAngle(m_yaw_axis), null); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java deleted file mode 100644 index a64d904d21c..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL345_SPI.java +++ /dev/null @@ -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); - }); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java deleted file mode 100644 index f33dc7c7a73..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXL362.java +++ /dev/null @@ -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. - * - *

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); - }); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java deleted file mode 100644 index d574a24824a..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/ADXRS450_Gyro.java +++ /dev/null @@ -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. - * - *

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. - * - *

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. - * - *

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. - * - *

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 clockwise when looked at from the top. - * It needs to follow the NED axis convention. - * - *

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. - * - *

The rate is based on the most recent reading of the gyro analog value - * - *

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}. - * - *

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. - * - *

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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java deleted file mode 100644 index bfdec752865..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/SPI.java +++ /dev/null @@ -1,767 +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.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. - * - *

Mode 0 is Clock idle low, data sampled on rising edge. - * - *

Mode 1 is Clock idle low, data sampled on falling edge. - * - *

Mode 2 is Clock idle high, data sampled on falling edge. - * - *

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. - * - *

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. - * - *

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. - * - *

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 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. - * - *

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 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. - * - *

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. - * - *

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. - * - *

{@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. - * - *

{@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. - * - *

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 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. - * - *

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 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. - * - *

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. - * - *

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; - } - } - - /** - * 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. - * - * @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); - } - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java index f4157aad875..2dba3ec0a26 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/shuffleboard/BuiltInWidgets.java @@ -351,8 +351,6 @@ public enum BuiltInWidgets implements WidgetType { * *

* *
@@ -374,7 +372,6 @@ public enum BuiltInWidgets implements WidgetType { * Supported types: * * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java deleted file mode 100644 index f94fa35b9c7..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16448_IMUSim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java deleted file mode 100644 index 57262440a69..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADIS16470_IMUSim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java index bcbf3e297d6..8a7e99e56df 100644 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java +++ b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL345Sim.java @@ -6,7 +6,6 @@ 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. * diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java deleted file mode 100644 index 148f914699e..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXL362Sim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java deleted file mode 100644 index 0743cbde0bb..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java deleted file mode 100644 index bd1490cea91..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPIAccelerometerSim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java b/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java deleted file mode 100644 index 7a3e2a5f4d1..00000000000 --- a/wpilibj/src/main/java/edu/wpi/first/wpilibj/simulation/SPISim.java +++ /dev/null @@ -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); - } -} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java deleted file mode 100644 index 16da98518af..00000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADIS16448SimTest.java +++ /dev/null @@ -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 provideEnumCombinations() { - return Stream.of(ADIS16448_IMU.IMUAxis.values()) - .flatMap( - imuAxis -> - Stream.of(ADIS16448_IMU.CalibrationTime.values()) - .map(calibrationTime -> arguments(imuAxis, calibrationTime))); - } -} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java index 2917ec27e83..ad1f2420c61 100644 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java +++ b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL345SimTest.java @@ -8,9 +8,7 @@ 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 @@ void testInitI2C(ADXL345_I2C.Range range) { 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); - } - } } diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java deleted file mode 100644 index 0f524b918a5..00000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXL362SimTest.java +++ /dev/null @@ -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); - } - } -} diff --git a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java b/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java deleted file mode 100644 index 301cef17a2c..00000000000 --- a/wpilibj/src/test/java/edu/wpi/first/wpilibj/simulation/ADXRS450_GyroSimTest.java +++ /dev/null @@ -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()); - } - } -} diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java index d6ff1eb734f..ba497e2069e 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/mecanumcontrollercommand/subsystems/DriveSubsystem.java @@ -9,7 +9,7 @@ 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 = diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java index dbb234a8d0a..b0d2670380c 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/rapidreactcommandbot/subsystems/Drive.java @@ -10,7 +10,7 @@ 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, diff --git a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java index 23d04a1aac6..20bf69aaee7 100644 --- a/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java +++ b/wpilibjExamples/src/main/java/edu/wpi/first/wpilibj/examples/swervecontrollercommand/subsystems/DriveSubsystem.java @@ -10,7 +10,7 @@ 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 =