Team2890/allwpilib
4
0
Fork 0
mirror of https://github.com/wpilibsuite/allwpilib synced 2026-06-25 01:41:43 +00:00
Code Issues Packages Projects Releases Wiki Activity

[wpilib] Add simulation support to ADIS classes (#3857)

Browse Source
This commit is contained in:
Tyler Veness
2022-01-03 11:44:12 -08:00
committed by GitHub
parent c137569f91
commit 831052f118
12 changed files with 1077 additions and 223 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -23,16 +23,12 @@
#include <memory>
#include <thread>
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
#include <wpi/sendable/SendableHelper.h>
// Not always defined in cmath (not part of standard)
#ifndef M_PI
#define M_PI 3.14159265358979323846
#endif
namespace frc {
/**
* Use DMA SPI to read rate, acceleration, and magnetometer data from the
@@ -138,17 +134,17 @@ class ADIS16448_IMU : public nt::NTSendable,
double GetGyroAngleZ() const;
double GetGyroInstantX() const;
double GetGyroRateX() const;
double GetGyroInstantY() const;
double GetGyroRateY() const;
double GetGyroInstantZ() const;
double GetGyroRateZ() const;
double GetAccelInstantX() const;
double GetAccelX() const;
double GetAccelInstantY() const;
double GetAccelY() const;
double GetAccelInstantZ() const;
double GetAccelZ() const;
double GetXComplementaryAngle() const;
@@ -174,6 +170,13 @@ class ADIS16448_IMU : public nt::NTSendable,
int ConfigDecRate(uint16_t DecimationRate);
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
int GetPort() const;
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
@@ -320,6 +323,17 @@ class ADIS16448_IMU : public nt::NTSendable,
std::thread m_acquire_task;
hal::SimDevice m_simDevice;
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;

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

@@ -23,6 +23,7 @@
#include <memory>
#include <thread>
#include <hal/SimDevice.h>
#include <networktables/NTSendable.h>
#include <wpi/condition_variable.h>
#include <wpi/mutex.h>
@@ -134,17 +135,17 @@ class ADIS16470_IMU : public nt::NTSendable,
double GetRate() const;
double GetGyroInstantX() const;
double GetGyroRateX() const;
double GetGyroInstantY() const;
double GetGyroRateY() const;
double GetGyroInstantZ() const;
double GetGyroRateZ() const;
double GetAccelInstantX() const;
double GetAccelX() const;
double GetAccelInstantY() const;
double GetAccelY() const;
double GetAccelInstantZ() const;
double GetAccelZ() const;
double GetXComplementaryAngle() const;
@@ -161,6 +162,13 @@ class ADIS16470_IMU : public nt::NTSendable,
// IMU yaw axis
IMUAxis m_yaw_axis;
/**
* Get the SPI port number.
*
* @return The SPI port number.
*/
int GetPort() const;
void InitSendable(nt::NTSendableBuilder& builder) override;
private:
@@ -368,6 +376,17 @@ class ADIS16470_IMU : public nt::NTSendable,
std::thread m_acquire_task;
hal::SimDevice m_simDevice;
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;

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

@@ -0,0 +1,106 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
namespace frc {
class ADIS16448_IMU;
namespace sim {
/**
* Class to control a simulated ADIS16448 IMU.
*/
class ADIS16448_IMUSim {
public:
/**
* Constructs from a ADIS16448_IMU object.
*
* @param imu ADIS16448_IMU to simulate
*/
explicit ADIS16448_IMUSim(const ADIS16448_IMU& imu);
/**
* Sets the X axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleX(units::degree_t angle);
/**
* Sets the Y axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Sets the Z axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t rate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t rate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t rate);
/**
* Sets the X axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelX(units::meters_per_second_squared_t accel);
/**
* Sets the Y axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelY(units::meters_per_second_squared_t accel);
/**
* Sets the Z axis acceleration.
*
* @param accel The acceleration.
*/
void SetAccelZ(units::meters_per_second_squared_t accel);
private:
hal::SimDouble m_simGyroAngleX;
hal::SimDouble m_simGyroAngleY;
hal::SimDouble m_simGyroAngleZ;
hal::SimDouble m_simGyroRateX;
hal::SimDouble m_simGyroRateY;
hal::SimDouble m_simGyroRateZ;
hal::SimDouble m_simAccelX;
hal::SimDouble m_simAccelY;
hal::SimDouble m_simAccelZ;
};
} // namespace sim
} // namespace frc

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

@@ -0,0 +1,106 @@
// Copyright (c) FIRST and other WPILib contributors.
// Open Source Software; you can modify and/or share it under the terms of
// the WPILib BSD license file in the root directory of this project.
#pragma once
#include <hal/SimDevice.h>
#include <units/acceleration.h>
#include <units/angle.h>
#include <units/angular_velocity.h>
namespace frc {
class ADIS16470_IMU;
namespace sim {
/**
* Class to control a simulated ADIS16470 IMU.
*/
class ADIS16470_IMUSim {
public:
/**
* Constructs from a ADIS16470_IMU object.
*
* @param imu ADIS16470_IMU to simulate
*/
explicit ADIS16470_IMUSim(const ADIS16470_IMU& imu);
/**
* Sets the X axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleX(units::degree_t angle);
/**
* Sets the Y axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleY(units::degree_t angle);
/**
* Sets the Z axis angle (CCW positive).
*
* @param angle The angle.
*/
void SetGyroAngleZ(units::degree_t angle);
/**
* Sets the X axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateX(units::degrees_per_second_t rate);
/**
* Sets the Y axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateY(units::degrees_per_second_t rate);
/**
* Sets the Z axis angular rate (CCW positive).
*
* @param rate The angular rate.
*/
void SetGyroRateZ(units::degrees_per_second_t rate);
/**
* 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