Implement sim devices for ADXL345, ADXL362, ADXRS450, Ultrasonic

This makes the halsim_adx_gyro_accelerometer simulation plugin and
the accelerometer part of lowfi_simulation obsolete.

wpilibc/src/main/native/cpp/ADXL345_I2C.cpp

@@ -15,7 +15,15 @@
 using namespace frc;
 
 ADXL345_I2C::ADXL345_I2C(I2C::Port port, Range range, int deviceAddress)
-    : m_i2c(port, deviceAddress) {
+    : m_i2c(port, deviceAddress),
+      m_simDevice("ADXL345_I2C", port, deviceAddress) {
+  if (m_simDevice) {
+    m_simRange =
+        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+    m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+  }
   // Turn on the measurements
   m_i2c.Write(kPowerCtlRegister, kPowerCtl_Measure);
   // Specify the data format to read
@@ -39,6 +47,9 @@ double ADXL345_I2C::GetY() { return GetAcceleration(kAxis_Y); }
 double ADXL345_I2C::GetZ() { return GetAcceleration(kAxis_Z); }
 
 double ADXL345_I2C::GetAcceleration(ADXL345_I2C::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();
   int16_t rawAccel = 0;
   m_i2c.Read(kDataRegister + static_cast<int>(axis), sizeof(rawAccel),
              reinterpret_cast<uint8_t*>(&rawAccel));
@@ -46,7 +57,13 @@ double ADXL345_I2C::GetAcceleration(ADXL345_I2C::Axes axis) {
 }
 
 ADXL345_I2C::AllAxes ADXL345_I2C::GetAccelerations() {
-  AllAxes data = AllAxes();
+  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;
+  }
   int16_t rawData[3];
   m_i2c.Read(kDataRegister, sizeof(rawData),
              reinterpret_cast<uint8_t*>(rawData));

wpilibc/src/main/native/cpp/ADXL345_SPI.cpp

@@ -15,7 +15,14 @@
 using namespace frc;
 
 ADXL345_SPI::ADXL345_SPI(SPI::Port port, ADXL345_SPI::Range range)
-    : m_spi(port) {
+    : m_spi(port), m_simDevice("ADXL345_SPI", port) {
+  if (m_simDevice) {
+    m_simRange =
+        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+    m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+  }
   m_spi.SetClockRate(500000);
   m_spi.SetMSBFirst();
   m_spi.SetSampleDataOnTrailingEdge();
@@ -43,6 +50,8 @@ void ADXL345_SPI::SetRange(Range range) {
   commands[0] = kDataFormatRegister;
   commands[1] = kDataFormat_FullRes | static_cast<uint8_t>(range & 0x03);
   m_spi.Transaction(commands, commands, 2);
+
+  if (m_simRange) m_simRange.Set(range);
 }
 
 double ADXL345_SPI::GetX() { return GetAcceleration(kAxis_X); }
@@ -52,6 +61,9 @@ 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) +
@@ -64,7 +76,14 @@ double ADXL345_SPI::GetAcceleration(ADXL345_SPI::Axes axis) {
 }
 
 ADXL345_SPI::AllAxes ADXL345_SPI::GetAccelerations() {
-  AllAxes data = AllAxes();
+  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];
 

wpilibc/src/main/native/cpp/ADXL362.cpp

@@ -34,23 +34,34 @@ 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) {
+ADXL362::ADXL362(SPI::Port port, Range range)
+    : m_spi(port), m_simDevice("ADXL362", port) {
+  if (m_simDevice) {
+    m_simRange =
+        m_simDevice.CreateEnum("Range", true, {"2G", "4G", "8G", "16G"}, 0);
+    m_simX = m_simDevice.CreateDouble("X Accel", false, 0.0);
+    m_simX = m_simDevice.CreateDouble("Y Accel", false, 0.0);
+    m_simZ = m_simDevice.CreateDouble("Z Accel", false, 0.0);
+  }
+
   m_spi.SetClockRate(3000000);
   m_spi.SetMSBFirst();
   m_spi.SetSampleDataOnTrailingEdge();
   m_spi.SetClockActiveLow();
   m_spi.SetChipSelectActiveLow();
 
-  // Validate the part ID
   uint8_t commands[3];
-  commands[0] = kRegRead;
-  commands[1] = kPartIdRegister;
-  commands[2] = 0;
-  m_spi.Transaction(commands, commands, 3);
-  if (commands[2] != 0xF2) {
-    DriverStation::ReportError("could not find ADXL362");
-    m_gsPerLSB = 0.0;
-    return;
+  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) {
+      DriverStation::ReportError("could not find ADXL362");
+      m_gsPerLSB = 0.0;
+      return;
+    }
   }
 
   SetRange(range);
@@ -90,6 +101,8 @@ void ADXL362::SetRange(Range range) {
   commands[2] =
       kFilterCtl_ODR_100Hz | static_cast<uint8_t>((range & 0x03) << 6);
   m_spi.Write(commands, 3);
+
+  if (m_simRange) m_simRange.Set(range);
 }
 
 double ADXL362::GetX() { return GetAcceleration(kAxis_X); }
@@ -101,6 +114,10 @@ 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;
@@ -113,11 +130,17 @@ double ADXL362::GetAcceleration(ADXL362::Axes axis) {
 }
 
 ADXL362::AllAxes ADXL362::GetAccelerations() {
-  AllAxes data = AllAxes();
+  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];

wpilibc/src/main/native/cpp/ADXRS450_Gyro.cpp

@@ -31,24 +31,32 @@ static constexpr int kSNLowRegister = 0x10;
 
 ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
 
-ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
+ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port)
+    : m_spi(port), m_simDevice("ADXRS450_Gyro", port) {
+  if (m_simDevice) {
+    m_simAngle = m_simDevice.CreateDouble("Angle", false, 0.0);
+    m_simRate = m_simDevice.CreateDouble("Rate", false, 0.0);
+  }
+
   m_spi.SetClockRate(3000000);
   m_spi.SetMSBFirst();
   m_spi.SetSampleDataOnLeadingEdge();
   m_spi.SetClockActiveHigh();
   m_spi.SetChipSelectActiveLow();
 
-  // Validate the part ID
-  if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
-    DriverStation::ReportError("could not find ADXRS450 gyro");
-    return;
+  if (!m_simDevice) {
+    // Validate the part ID
+    if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
+      DriverStation::ReportError("could not find ADXRS450 gyro");
+      return;
+    }
+
+    m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu,
+                          0x04000000u, 10u, 16u, true, true);
+
+    Calibrate();
   }
 
-  m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c00000eu, 0x04000000u,
-                        10u, 16u, true, true);
-
-  Calibrate();
-
   HAL_Report(HALUsageReporting::kResourceType_ADXRS450, port);
 
   SendableRegistry::GetInstance().AddLW(this, "ADXRS450_Gyro", port);
@@ -88,15 +96,21 @@ uint16_t ADXRS450_Gyro::ReadRegister(int reg) {
 }
 
 double ADXRS450_Gyro::GetAngle() const {
+  if (m_simAngle) return m_simAngle.Get();
   return m_spi.GetAccumulatorIntegratedValue() * kDegreePerSecondPerLSB;
 }
 
 double ADXRS450_Gyro::GetRate() const {
+  if (m_simRate) return m_simRate.Get();
   return static_cast<double>(m_spi.GetAccumulatorLastValue()) *
          kDegreePerSecondPerLSB;
 }
 
-void ADXRS450_Gyro::Reset() { m_spi.ResetAccumulator(); }
+void ADXRS450_Gyro::Reset() {
+  if (m_simAngle) m_simAngle.Set(0.0);
+  if (m_simRate) m_simRate.Set(0.0);
+  m_spi.ResetAccumulator();
+}
 
 void ADXRS450_Gyro::Calibrate() {
   Wait(0.1);

wpilibc/src/main/native/cpp/Ultrasonic.cpp

@@ -98,7 +98,10 @@ void Ultrasonic::Ping() {
   m_pingChannel->Pulse(kPingTime);
 }
 
-bool Ultrasonic::IsRangeValid() const { return m_counter.Get() > 1; }
+bool Ultrasonic::IsRangeValid() const {
+  if (m_simRangeValid) return m_simRangeValid.Get();
+  return m_counter.Get() > 1;
+}
 
 void Ultrasonic::SetAutomaticMode(bool enabling) {
   if (enabling == m_automaticEnabled) return;  // ignore the case of no change
@@ -135,10 +138,12 @@ void Ultrasonic::SetAutomaticMode(bool enabling) {
 }
 
 double Ultrasonic::GetRangeInches() const {
-  if (IsRangeValid())
+  if (IsRangeValid()) {
+    if (m_simRange) return m_simRange.Get();
     return m_counter.GetPeriod() * kSpeedOfSoundInchesPerSec / 2.0;
-  else
+  } else {
     return 0;
+  }
 }
 
 double Ultrasonic::GetRangeMM() const { return GetRangeInches() * 25.4; }
@@ -177,6 +182,14 @@ void Ultrasonic::InitSendable(SendableBuilder& builder) {
 }
 
 void Ultrasonic::Initialize() {
+  m_simDevice = hal::SimDevice("Ultrasonic", m_echoChannel->GetChannel());
+  if (m_simDevice) {
+    m_simRangeValid = m_simDevice.CreateBoolean("Range Valid", false, true);
+    m_simRange = m_simDevice.CreateDouble("Range (in)", false, 0.0);
+    m_pingChannel->SetSimDevice(m_simDevice);
+    m_echoChannel->SetSimDevice(m_simDevice);
+  }
+
   bool originalMode = m_automaticEnabled;
   SetAutomaticMode(false);  // Kill task when adding a new sensor
   // Link this instance on the list