Simulate ADX* family of accelerometers and gyros (#688)

simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_I2CAccelerometerData.cpp

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL345_I2CAccelerometerData.h"
+
+#include <cstring>
+
+#include "MockData/I2CData.h"
+
+using namespace hal;
+
+const double ADXL345_I2CData::LSB = 1 / 0.00390625;
+
+static void ADXL345I2C_ReadBufferCallback(const char* name, void* param,
+                                          uint8_t* buffer, uint32_t count) {
+  ADXL345_I2CData* sim = static_cast<ADXL345_I2CData*>(param);
+  sim->HandleRead(buffer, count);
+}
+
+static void ADXL345I2C_WriteBufferCallback(const char* name, void* param,
+                                           const uint8_t* buffer,
+                                           uint32_t count) {
+  ADXL345_I2CData* sim = static_cast<ADXL345_I2CData*>(param);
+  sim->HandleWrite(buffer, count);
+}
+
+ADXL345_I2CData::ADXL345_I2CData(int port) {
+  HALSIM_RegisterI2CReadCallback(port, ADXL345I2C_ReadBufferCallback, this);
+  HALSIM_RegisterI2CWriteCallback(port, ADXL345I2C_WriteBufferCallback, this);
+}
+
+ADXL345_I2CData::~ADXL345_I2CData() {}
+
+void ADXL345_I2CData::ADXL345_I2CData::HandleWrite(const uint8_t* buffer,
+                                                   uint32_t count) {
+  m_lastWriteAddress = buffer[0];
+}
+
+void ADXL345_I2CData::HandleRead(uint8_t* buffer, uint32_t count) {
+  bool writeAll = count == 6;
+  int byteIndex = 0;
+
+  if (writeAll || m_lastWriteAddress == 0x32) {
+    int16_t val = static_cast<int16_t>(GetX() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+
+  if (writeAll || m_lastWriteAddress == 0x34) {
+    int16_t val = static_cast<int16_t>(GetY() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+
+  if (writeAll || m_lastWriteAddress == 0x36) {
+    int16_t val = static_cast<int16_t>(GetZ() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+}

simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL345_SpiAccelerometerData.cpp

@@ -0,0 +1,64 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL345_SpiAccelerometerData.h"
+
+#include <cstring>
+
+#include "MockData/SPIData.h"
+
+using namespace hal;
+
+const double ADXL345_SpiAccelerometer::LSB = 1 / 0.00390625;
+
+static void ADXL345SPI_ReadBufferCallback(const char* name, void* param,
+                                          uint8_t* buffer, uint32_t count) {
+  ADXL345_SpiAccelerometer* sim = static_cast<ADXL345_SpiAccelerometer*>(param);
+  sim->HandleRead(buffer, count);
+}
+
+static void ADXL345SPI_WriteBufferCallback(const char* name, void* param,
+                                           const uint8_t* buffer,
+                                           uint32_t count) {
+  ADXL345_SpiAccelerometer* sim = static_cast<ADXL345_SpiAccelerometer*>(param);
+  sim->HandleWrite(buffer, count);
+}
+
+ADXL345_SpiAccelerometer::ADXL345_SpiAccelerometer(int port) {
+  HALSIM_RegisterSPIReadCallback(port, ADXL345SPI_ReadBufferCallback, this);
+  HALSIM_RegisterSPIWriteCallback(port, ADXL345SPI_WriteBufferCallback, this);
+}
+
+ADXL345_SpiAccelerometer::~ADXL345_SpiAccelerometer() {}
+
+void ADXL345_SpiAccelerometer::HandleWrite(const uint8_t* buffer,
+                                           uint32_t count) {
+  m_lastWriteAddress = buffer[0] & 0xF;
+}
+
+void ADXL345_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) {
+  bool writeAll = count == 7;
+  int byteIndex = 1;
+
+  if (writeAll || m_lastWriteAddress == 0x02) {
+    int16_t val = static_cast<int16_t>(GetX() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+
+  if (writeAll || m_lastWriteAddress == 0x04) {
+    int16_t val = static_cast<int16_t>(GetY() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+
+  if (writeAll || m_lastWriteAddress == 0x06) {
+    int16_t val = static_cast<int16_t>(GetZ() * LSB);
+    std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+    byteIndex += sizeof(val);
+  }
+}

simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXL362_SpiAccelerometerData.cpp

@@ -0,0 +1,71 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXL362_SpiAccelerometerData.h"
+
+#include <cstring>
+
+#include "MockData/SPIData.h"
+
+using namespace hal;
+
+const double ADXL362_SpiAccelerometer::LSB = 1 / 0.001;
+
+static void ADXL362SPI_ReadBufferCallback(const char* name, void* param,
+                                          uint8_t* buffer, uint32_t count) {
+  ADXL362_SpiAccelerometer* sim = static_cast<ADXL362_SpiAccelerometer*>(param);
+  sim->HandleRead(buffer, count);
+}
+
+static void ADXL362SPI_WriteBufferCallback(const char* name, void* param,
+                                           const uint8_t* buffer,
+                                           uint32_t count) {
+  ADXL362_SpiAccelerometer* sim = static_cast<ADXL362_SpiAccelerometer*>(param);
+  sim->HandleWrite(buffer, count);
+}
+
+ADXL362_SpiAccelerometer::ADXL362_SpiAccelerometer(int port) {
+  HALSIM_RegisterSPIReadCallback(port, ADXL362SPI_ReadBufferCallback, this);
+  HALSIM_RegisterSPIWriteCallback(port, ADXL362SPI_WriteBufferCallback, this);
+}
+
+ADXL362_SpiAccelerometer::~ADXL362_SpiAccelerometer() {}
+
+void ADXL362_SpiAccelerometer::HandleWrite(const uint8_t* buffer,
+                                           uint32_t count) {
+  m_lastWriteAddress = buffer[1];
+}
+
+void ADXL362_SpiAccelerometer::HandleRead(uint8_t* buffer, uint32_t count) {
+  // Init check
+  if (m_lastWriteAddress == 0x02) {
+    uint32_t numToPut = 0xF20000;
+    std::memcpy(&buffer[0], &numToPut, sizeof(numToPut));
+  } else {
+    // Get Accelerations
+    bool writeAll = count == 8;
+    int byteIndex = 2;
+
+    if (writeAll || m_lastWriteAddress == 0x0E) {
+      int16_t val = static_cast<int16_t>(GetX() * LSB);
+      std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+      byteIndex += sizeof(val);
+    }
+
+    if (writeAll || m_lastWriteAddress == 0x10) {
+      int16_t val = static_cast<int16_t>(GetY() * LSB);
+      std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+      byteIndex += sizeof(val);
+    }
+
+    if (writeAll || m_lastWriteAddress == 0x12) {
+      int16_t val = static_cast<int16_t>(GetZ() * LSB);
+      std::memcpy(&buffer[byteIndex], &val, sizeof(val));
+      byteIndex += sizeof(val);
+    }
+  }
+}

simulation/adx_gyro_accelerometer/src/main/native/cpp/ADXRS450_SpiGyroWrapperData.cpp

@@ -0,0 +1,73 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ADXRS450_SpiGyroWrapperData.h"
+
+#include <cstring>
+
+#include "MockData/NotifyCallbackHelpers.h"
+#include "MockData/SPIData.h"
+
+using namespace hal;
+
+const double ADXRS450_SpiGyroWrapper::ANGLE_LSB = 1 / 0.0125 / 0.001;
+
+static void ADXRS450SPI_ReadBufferCallback(const char* name, void* param,
+                                           uint8_t* buffer, uint32_t count) {
+  ADXRS450_SpiGyroWrapper* sim = static_cast<ADXRS450_SpiGyroWrapper*>(param);
+  sim->HandleRead(buffer, count);
+}
+
+ADXRS450_SpiGyroWrapper::ADXRS450_SpiGyroWrapper(int port) : m_port(port) {
+  HALSIM_RegisterSPIReadCallback(port, ADXRS450SPI_ReadBufferCallback, this);
+}
+
+ADXRS450_SpiGyroWrapper::~ADXRS450_SpiGyroWrapper() {}
+
+void ADXRS450_SpiGyroWrapper::ResetData() {
+  m_angle = 0;
+  m_angleCallbacks = nullptr;
+}
+
+void ADXRS450_SpiGyroWrapper::HandleRead(uint8_t* buffer, uint32_t count) {
+  int returnCode = 0x00400AE0;
+  std::memcpy(&buffer[0], &returnCode, sizeof(returnCode));
+}
+
+int32_t ADXRS450_SpiGyroWrapper::RegisterAngleCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+  // Must return -1 on a null callback for error handling
+  if (callback == nullptr) return -1;
+  int32_t newUid = 0;
+  {
+    std::lock_guard<wpi::mutex> lock(m_registerMutex);
+    m_angleCallbacks =
+        RegisterCallback(m_angleCallbacks, "Angle", callback, param, &newUid);
+  }
+  if (initialNotify) {
+    // We know that the callback is not null because of earlier null check
+    HAL_Value value = MakeDouble(GetAngle());
+    callback("Angle", param, &value);
+  }
+  return newUid;
+}
+void ADXRS450_SpiGyroWrapper::CancelAngleCallback(int32_t uid) {
+  m_angleCallbacks = CancelCallback(m_angleCallbacks, uid);
+}
+void ADXRS450_SpiGyroWrapper::InvokeAngleCallback(HAL_Value value) {
+  InvokeCallback(m_angleCallbacks, "Angle", &value);
+}
+double ADXRS450_SpiGyroWrapper::GetAngle() { return m_angle; }
+void ADXRS450_SpiGyroWrapper::SetAngle(double angle) {
+  int32_t oldValue = m_angle.exchange(angle);
+  if (oldValue != angle) {
+    InvokeAngleCallback(MakeDouble(angle));
+
+    int64_t accumValue = angle * ANGLE_LSB;
+    HALSIM_SetSPISetAccumulatorValue(m_port, accumValue);
+  }
+}

simulation/adx_gyro_accelerometer/src/main/native/cpp/ThreeAxisAccelerometerData.cpp

@@ -0,0 +1,129 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#include "ThreeAxisAccelerometerData.h"
+
+#include "MockData/NotifyCallbackHelpers.h"
+
+using namespace hal;
+
+ThreeAxisAccelerometerData::ThreeAxisAccelerometerData() {}
+
+ThreeAxisAccelerometerData::~ThreeAxisAccelerometerData() {}
+void ThreeAxisAccelerometerData::ResetData() {
+  m_x = 0.0;
+  m_xCallbacks = nullptr;
+  m_y = 0.0;
+  m_yCallbacks = nullptr;
+  m_z = 0.0;
+  m_zCallbacks = nullptr;
+}
+
+int32_t ThreeAxisAccelerometerData::RegisterXCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+  // Must return -1 on a null callback for error handling
+  if (callback == nullptr) return -1;
+  int32_t newUid = 0;
+  {
+    std::lock_guard<wpi::mutex> lock(m_registerMutex);
+    m_xCallbacks =
+        RegisterCallback(m_xCallbacks, "X", callback, param, &newUid);
+  }
+  if (initialNotify) {
+    // We know that the callback is not null because of earlier null check
+    HAL_Value value = MakeDouble(GetX());
+    callback("X", param, &value);
+  }
+  return newUid;
+}
+
+void ThreeAxisAccelerometerData::CancelXCallback(int32_t uid) {
+  m_xCallbacks = CancelCallback(m_xCallbacks, uid);
+}
+
+void ThreeAxisAccelerometerData::InvokeXCallback(HAL_Value value) {
+  InvokeCallback(m_xCallbacks, "X", &value);
+}
+
+double ThreeAxisAccelerometerData::GetX() { return m_x; }
+
+void ThreeAxisAccelerometerData::SetX(double x) {
+  double oldValue = m_x.exchange(x);
+  if (oldValue != x) {
+    InvokeXCallback(MakeDouble(x));
+  }
+}
+
+int32_t ThreeAxisAccelerometerData::RegisterYCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+  // Must return -1 on a null callback for error handling
+  if (callback == nullptr) return -1;
+  int32_t newUid = 0;
+  {
+    std::lock_guard<wpi::mutex> lock(m_registerMutex);
+    m_yCallbacks =
+        RegisterCallback(m_yCallbacks, "Y", callback, param, &newUid);
+  }
+  if (initialNotify) {
+    // We know that the callback is not null because of earlier null check
+    HAL_Value value = MakeDouble(GetY());
+    callback("Y", param, &value);
+  }
+  return newUid;
+}
+
+void ThreeAxisAccelerometerData::CancelYCallback(int32_t uid) {
+  m_yCallbacks = CancelCallback(m_yCallbacks, uid);
+}
+
+void ThreeAxisAccelerometerData::InvokeYCallback(HAL_Value value) {
+  InvokeCallback(m_yCallbacks, "Y", &value);
+}
+
+double ThreeAxisAccelerometerData::GetY() { return m_y; }
+
+void ThreeAxisAccelerometerData::SetY(double y) {
+  double oldValue = m_y.exchange(y);
+  if (oldValue != y) {
+    InvokeYCallback(MakeDouble(y));
+  }
+}
+
+int32_t ThreeAxisAccelerometerData::RegisterZCallback(
+    HAL_NotifyCallback callback, void* param, HAL_Bool initialNotify) {
+  // Must return -1 on a null callback for error handling
+  if (callback == nullptr) return -1;
+  int32_t newUid = 0;
+  {
+    std::lock_guard<wpi::mutex> lock(m_registerMutex);
+    m_zCallbacks =
+        RegisterCallback(m_zCallbacks, "Z", callback, param, &newUid);
+  }
+  if (initialNotify) {
+    // We know that the callback is not null because of earlier null check
+    HAL_Value value = MakeDouble(GetZ());
+    callback("Z", param, &value);
+  }
+  return newUid;
+}
+
+void ThreeAxisAccelerometerData::CancelZCallback(int32_t uid) {
+  m_zCallbacks = CancelCallback(m_zCallbacks, uid);
+}
+
+void ThreeAxisAccelerometerData::InvokeZCallback(HAL_Value value) {
+  InvokeCallback(m_zCallbacks, "Z", &value);
+}
+
+double ThreeAxisAccelerometerData::GetZ() { return m_z; }
+
+void ThreeAxisAccelerometerData::SetZ(double z) {
+  double oldValue = m_z.exchange(z);
+  if (oldValue != z) {
+    InvokeZCallback(MakeDouble(z));
+  }
+}

simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_I2CAccelerometerData.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "ThreeAxisAccelerometerData.h"
+
+namespace hal {
+class ADXL345_I2CData : public ThreeAxisAccelerometerData {
+ public:
+  explicit ADXL345_I2CData(int port);
+  virtual ~ADXL345_I2CData();
+
+  void HandleWrite(const uint8_t* buffer, uint32_t count);
+  void HandleRead(uint8_t* buffer, uint32_t count);
+
+ private:
+  int m_lastWriteAddress;
+
+  static const double LSB;
+};
+}  // namespace hal

simulation/adx_gyro_accelerometer/src/main/native/include/ADXL345_SpiAccelerometerData.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "ThreeAxisAccelerometerData.h"
+
+namespace hal {
+class ADXL345_SpiAccelerometer : public ThreeAxisAccelerometerData {
+ public:
+  explicit ADXL345_SpiAccelerometer(int port);
+  virtual ~ADXL345_SpiAccelerometer();
+
+  void HandleWrite(const uint8_t* buffer, uint32_t count);
+  void HandleRead(uint8_t* buffer, uint32_t count);
+
+ private:
+  int m_lastWriteAddress;
+
+  static const double LSB;
+};
+}  // namespace hal

simulation/adx_gyro_accelerometer/src/main/native/include/ADXL362_SpiAccelerometerData.h

@@ -0,0 +1,26 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include "ThreeAxisAccelerometerData.h"
+
+namespace hal {
+class ADXL362_SpiAccelerometer : public ThreeAxisAccelerometerData {
+ public:
+  explicit ADXL362_SpiAccelerometer(int port);
+  virtual ~ADXL362_SpiAccelerometer();
+
+  void HandleWrite(const uint8_t* buffer, uint32_t count);
+  void HandleRead(uint8_t* buffer, uint32_t count);
+
+ private:
+  int m_lastWriteAddress;
+
+  static const double LSB;
+};
+}  // namespace hal

simulation/adx_gyro_accelerometer/src/main/native/include/ADXRS450_SpiGyroWrapperData.h

@@ -0,0 +1,43 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <memory>
+
+#include <support/mutex.h>
+
+#include "MockData/NotifyListenerVector.h"
+
+namespace hal {
+class ADXRS450_SpiGyroWrapper {
+ public:
+  explicit ADXRS450_SpiGyroWrapper(int port);
+  virtual ~ADXRS450_SpiGyroWrapper();
+
+  void HandleRead(uint8_t* buffer, uint32_t count);
+
+  virtual void ResetData();
+
+  int32_t RegisterAngleCallback(HAL_NotifyCallback callback, void* param,
+                                HAL_Bool initialNotify);
+  void CancelAngleCallback(int32_t uid);
+  void InvokeAngleCallback(HAL_Value value);
+  double GetAngle();
+  void SetAngle(double angle);
+
+ private:
+  const int m_port;
+
+  wpi::mutex m_registerMutex;
+  std::atomic<double> m_angle{0.0};
+  std::shared_ptr<NotifyListenerVector> m_angleCallbacks = nullptr;
+
+  static const double ANGLE_LSB;
+};
+}  // namespace hal

simulation/adx_gyro_accelerometer/src/main/native/include/ThreeAxisAccelerometerData.h

@@ -0,0 +1,55 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) 2017 FIRST. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+#pragma once
+
+#include <atomic>
+#include <memory>
+
+#include <support/mutex.h>
+
+#include "MockData/NotifyListenerVector.h"
+
+namespace hal {
+class ThreeAxisAccelerometerData {
+ public:
+  ThreeAxisAccelerometerData();
+  virtual ~ThreeAxisAccelerometerData();
+
+  int32_t RegisterXCallback(HAL_NotifyCallback callback, void* param,
+                            HAL_Bool initialNotify);
+  void CancelXCallback(int32_t uid);
+  void InvokeXCallback(HAL_Value value);
+  double GetX();
+  void SetX(double x);
+
+  int32_t RegisterYCallback(HAL_NotifyCallback callback, void* param,
+                            HAL_Bool initialNotify);
+  void CancelYCallback(int32_t uid);
+  void InvokeYCallback(HAL_Value value);
+  double GetY();
+  void SetY(double y);
+
+  int32_t RegisterZCallback(HAL_NotifyCallback callback, void* param,
+                            HAL_Bool initialNotify);
+  void CancelZCallback(int32_t uid);
+  void InvokeZCallback(HAL_Value value);
+  double GetZ();
+  void SetZ(double z);
+
+  virtual void ResetData();
+
+ protected:
+  wpi::mutex m_registerMutex;
+  std::atomic<double> m_x{0.0};
+  std::shared_ptr<NotifyListenerVector> m_xCallbacks = nullptr;
+  std::atomic<double> m_y{0.0};
+  std::shared_ptr<NotifyListenerVector> m_yCallbacks = nullptr;
+  std::atomic<double> m_z{0.0};
+  std::shared_ptr<NotifyListenerVector> m_zCallbacks = nullptr;
+};
+}  // namespace hal