Moves Gyros to the HAL (#131)

hal/include/HAL/AnalogGyro.h

@@ -0,0 +1,32 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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 <stdint.h>
+
+#include "HAL/Handles.h"
+
+extern "C" {
+HalGyroHandle initializeAnalogGyro(HalAnalogInputHandle handle,
+                                   int32_t* status);
+void setupAnalogGyro(HalGyroHandle handle, int32_t* status);
+void freeAnalogGyro(HalGyroHandle handle);
+void setAnalogGyroParameters(HalGyroHandle handle,
+                             float voltsPerDegreePerSecond, float offset,
+                             uint32_t center, int32_t* status);
+void setAnalogGyroVoltsPerDegreePerSecond(HalGyroHandle handle,
+                                          float voltsPerDegreePerSecond,
+                                          int32_t* status);
+void resetAnalogGyro(HalGyroHandle handle, int32_t* status);
+void calibrateAnalogGyro(HalGyroHandle handle, int32_t* status);
+void setAnalogGyroDeadband(HalGyroHandle handle, float volts, int32_t* status);
+float getAnalogGyroAngle(HalGyroHandle handle, int32_t* status);
+double getAnalogGyroRate(HalGyroHandle handle, int32_t* status);
+float getAnalogGyroOffset(HalGyroHandle handle, int32_t* status);
+uint32_t getAnalogGyroCenter(HalGyroHandle handle, int32_t* status);
+}

hal/include/HAL/HAL.h

@@ -11,6 +11,7 @@
 
 #include "Accelerometer.h"
 #include "AnalogAccumulator.h"
+#include "AnalogGyro.h"
 #include "AnalogInput.h"
 #include "AnalogOutput.h"
 #include "AnalogTrigger.h"

hal/include/HAL/Handles.h

@@ -40,3 +40,5 @@ typedef HalHandle HalSolenoidHandle;
 typedef HalHandle HalFPGAEncoderHandle;
 
 typedef HalHandle HalEncoderHandle;
+
+typedef HalHandle HalGyroHandle;

hal/lib/athena/AnalogGyro.cpp

@@ -0,0 +1,232 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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 "HAL/AnalogGyro.h"
+
+#include <chrono>
+#include <thread>
+
+#include "AnalogInternal.h"
+#include "HAL/AnalogAccumulator.h"
+#include "HAL/AnalogInput.h"
+#include "handles/IndexedHandleResource.h"
+
+namespace {
+struct AnalogGyro {
+  HalAnalogInputHandle handle;
+  float voltsPerDegreePerSecond;
+  float offset;
+  uint32_t center;
+};
+}
+
+static constexpr uint32_t kOversampleBits = 10;
+static constexpr uint32_t kAverageBits = 0;
+static constexpr float kSamplesPerSecond = 50.0;
+static constexpr float kCalibrationSampleTime = 5.0;
+static constexpr float kDefaultVoltsPerDegreePerSecond = 0.007;
+
+using namespace hal;
+
+static IndexedHandleResource<HalGyroHandle, AnalogGyro, kNumAccumulators,
+                             HalHandleEnum::AnalogGyro>
+    analogGyroHandles;
+
+static void Wait(double seconds) {
+  if (seconds < 0.0) return;
+  std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
+}
+
+extern "C" {
+HalGyroHandle initializeAnalogGyro(HalAnalogInputHandle analog_handle,
+                                   int32_t* status) {
+  if (!isAccumulatorChannel(analog_handle, status)) {
+    if (*status == 0) {
+      *status = PARAMETER_OUT_OF_RANGE;
+    }
+    return HAL_INVALID_HANDLE;
+  }
+
+  // handle known to be correct, so no need to type check
+  int16_t channel = getHandleIndex(analog_handle);
+
+  auto handle = analogGyroHandles.Allocate(channel, status);
+
+  if (*status != 0)
+    return HAL_INVALID_HANDLE;  // failed to allocate. Pass error back.
+
+  // Initialize port structure
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {  // would only error on thread issue
+    *status = HAL_HANDLE_ERROR;
+    return HAL_INVALID_HANDLE;
+  }
+
+  gyro->handle = analog_handle;
+  gyro->voltsPerDegreePerSecond = 0;
+  gyro->offset = 0;
+  gyro->center = 0;
+
+  return handle;
+}
+
+void setupAnalogGyro(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+
+  setAnalogAverageBits(gyro->handle, kAverageBits, status);
+  if (*status != 0) return;
+  setAnalogOversampleBits(gyro->handle, kOversampleBits, status);
+  if (*status != 0) return;
+  float sampleRate =
+      kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+  setAnalogSampleRate(sampleRate, status);
+  if (*status != 0) return;
+  Wait(0.1);
+
+  setAnalogGyroDeadband(handle, 0.0f, status);
+  if (*status != 0) return;
+}
+
+void freeAnalogGyro(HalGyroHandle handle) { analogGyroHandles.Free(handle); }
+
+void setAnalogGyroParameters(HalGyroHandle handle,
+                             float voltsPerDegreePerSecond, float offset,
+                             uint32_t center, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+  gyro->offset = offset;
+  gyro->center = center;
+  setAccumulatorCenter(gyro->handle, center, status);
+}
+
+void setAnalogGyroVoltsPerDegreePerSecond(HalGyroHandle handle,
+                                          float voltsPerDegreePerSecond,
+                                          int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+}
+
+void resetAnalogGyro(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  resetAccumulator(gyro->handle, status);
+  if (*status != 0) return;
+
+  const float sampleTime = 1.0f / getAnalogSampleRate(status);
+  const float overSamples = 1 << getAnalogOversampleBits(gyro->handle, status);
+  const float averageSamples = 1 << getAnalogAverageBits(gyro->handle, status);
+  if (*status != 0) return;
+  Wait(sampleTime * overSamples * averageSamples);
+}
+
+void calibrateAnalogGyro(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+
+  initAccumulator(gyro->handle, status);
+  if (*status != 0) return;
+  Wait(kCalibrationSampleTime);
+
+  int64_t value;
+  uint32_t count;
+  getAccumulatorOutput(gyro->handle, &value, &count, status);
+  if (*status != 0) return;
+
+  gyro->center = (uint32_t)((float)value / (float)count + .5);
+
+  gyro->offset = ((float)value / (float)count) - (float)gyro->center;
+  setAccumulatorCenter(gyro->handle, gyro->center, status);
+  if (*status != 0) return;
+  resetAnalogGyro(handle, status);
+}
+
+void setAnalogGyroDeadband(HalGyroHandle handle, float volts, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return;
+  }
+  int32_t deadband = volts * 1e9 / getAnalogLSBWeight(gyro->handle, status) *
+                     (1 << getAnalogOversampleBits(gyro->handle, status));
+  if (*status != 0) return;
+  setAccumulatorDeadband(gyro->handle, deadband, status);
+}
+
+float getAnalogGyroAngle(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  int64_t rawValue = 0;
+  uint32_t count = 0;
+  getAccumulatorOutput(gyro->handle, &rawValue, &count, status);
+
+  int64_t value = rawValue - (int64_t)((float)count * gyro->offset);
+
+  double scaledValue =
+      value * 1e-9 * (double)getAnalogLSBWeight(gyro->handle, status) *
+      (double)(1 << getAnalogAverageBits(gyro->handle, status)) /
+      (getAnalogSampleRate(status) * gyro->voltsPerDegreePerSecond);
+
+  return (float)scaledValue;
+}
+
+double getAnalogGyroRate(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+
+  return (getAnalogAverageValue(gyro->handle, status) -
+          ((double)gyro->center + gyro->offset)) *
+         1e-9 * getAnalogLSBWeight(gyro->handle, status) /
+         ((1 << getAnalogOversampleBits(gyro->handle, status)) *
+          gyro->voltsPerDegreePerSecond);
+}
+
+float getAnalogGyroOffset(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  return gyro->offset;
+}
+
+uint32_t getAnalogGyroCenter(HalGyroHandle handle, int32_t* status) {
+  auto gyro = analogGyroHandles.Get(handle);
+  if (gyro == nullptr) {
+    *status = HAL_HANDLE_ERROR;
+    return 0;
+  }
+  return gyro->center;
+}
+}

hal/lib/athena/handles/HandlesInternal.h

@@ -41,7 +41,8 @@ enum class HalHandleEnum {
   FPGAEncoder = 12,
   Encoder = 13,
   Compressor = 14,
-  Solenoid = 15
+  Solenoid = 15,
+  AnalogGyro = 16
 };
 
 static inline int16_t getHandleIndex(HalHandle handle) {

wpilibc/athena/include/AnalogGyro.h

@@ -9,6 +9,8 @@
 
 #include "GyroBase.h"
 
+#include "HAL/Handles.h"
+
 class AnalogInput;
 
 /**
@@ -38,7 +40,7 @@ class AnalogGyro : public GyroBase {
   AnalogGyro(int32_t channel, uint32_t center, float offset);
   AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center,
              float offset);
-  virtual ~AnalogGyro() = default;
+  virtual ~AnalogGyro();
 
   float GetAngle() const override;
   double GetRate() const override;
@@ -54,7 +56,5 @@ class AnalogGyro : public GyroBase {
   std::shared_ptr<AnalogInput> m_analog;
 
  private:
-  float m_voltsPerDegreePerSecond;
-  float m_offset;
-  uint32_t m_center;
+  HalGyroHandle m_gyroHandle = HAL_INVALID_HANDLE;
 };

wpilibc/athena/include/AnalogInput.h

@@ -30,6 +30,7 @@ class AnalogInput : public SensorBase,
                     public PIDSource,
                     public LiveWindowSendable {
   friend class AnalogTrigger;
+  friend class AnalogGyro;
 
  public:
   static const uint8_t kAccumulatorModuleNumber = 1;

wpilibc/athena/src/AnalogGyro.cpp

@@ -79,10 +79,15 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel)
 AnalogGyro::AnalogGyro(int32_t channel, uint32_t center, float offset) {
   m_analog = std::make_shared<AnalogInput>(channel);
   InitGyro();
-  m_center = center;
-  m_offset = offset;
-  m_analog->SetAccumulatorCenter(m_center);
-  m_analog->ResetAccumulator();
+  int32_t status = 0;
+  setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, offset,
+                          center, &status);
+  if (status != 0) {
+    wpi_setErrorWithContext(status, getHALErrorMessage(status));
+    m_gyroHandle = HAL_INVALID_HANDLE;
+    return;
+  }
+  Reset();
 }
 
 /**
@@ -103,13 +108,24 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center,
     wpi_setWPIError(NullParameter);
   } else {
     InitGyro();
-    m_center = center;
-    m_offset = offset;
-    m_analog->SetAccumulatorCenter(m_center);
-    m_analog->ResetAccumulator();
+    int32_t status = 0;
+    setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond,
+                            offset, center, &status);
+    if (status != 0) {
+      wpi_setErrorWithContext(status, getHALErrorMessage(status));
+      m_gyroHandle = HAL_INVALID_HANDLE;
+      return;
+    }
+    Reset();
   }
 }
 
+/**
+ * AnalogGyro Destructor
+ *
+ */
+AnalogGyro::~AnalogGyro() { freeAnalogGyro(m_gyroHandle); }
+
 /**
  * Reset the gyro.
  *
@@ -119,7 +135,9 @@ AnalogGyro::AnalogGyro(std::shared_ptr<AnalogInput> channel, uint32_t center,
  */
 void AnalogGyro::Reset() {
   if (StatusIsFatal()) return;
-  m_analog->ResetAccumulator();
+  int32_t status = 0;
+  resetAnalogGyro(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
 }
 
 /**
@@ -127,25 +145,32 @@ void AnalogGyro::Reset() {
  */
 void AnalogGyro::InitGyro() {
   if (StatusIsFatal()) return;
-
-  if (!m_analog->IsAccumulatorChannel()) {
-    wpi_setWPIErrorWithContext(ParameterOutOfRange,
-                               " channel (must be accumulator channel)");
-    m_analog = nullptr;
-    return;
+  if (m_gyroHandle == HAL_INVALID_HANDLE) {
+    int32_t status = 0;
+    m_gyroHandle = initializeAnalogGyro(m_analog->m_port, &status);
+    if (status == PARAMETER_OUT_OF_RANGE) {
+      wpi_setWPIErrorWithContext(ParameterOutOfRange,
+                                 " channel (must be accumulator channel)");
+      m_analog = nullptr;
+      m_gyroHandle = HAL_INVALID_HANDLE;
+      return;
+    }
+    if (status != 0) {
+      wpi_setErrorWithContext(status, getHALErrorMessage(status));
+      m_analog = nullptr;
+      m_gyroHandle = HAL_INVALID_HANDLE;
+      return;
+    }
   }
 
-  m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
-  m_analog->SetAverageBits(kAverageBits);
-  m_analog->SetOversampleBits(kOversampleBits);
-  float sampleRate =
-      kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
-  m_analog->SetSampleRate(sampleRate);
-  Wait(0.1);
-
-  SetDeadband(0.0f);
-
-  SetPIDSourceType(PIDSourceType::kDisplacement);
+  int32_t status = 0;
+  setupAnalogGyro(m_gyroHandle, &status);
+  if (status != 0) {
+    wpi_setErrorWithContext(status, getHALErrorMessage(status));
+    m_analog = nullptr;
+    m_gyroHandle = HAL_INVALID_HANDLE;
+    return;
+  }
 
   HALReport(HALUsageReporting::kResourceType_Gyro, m_analog->GetChannel());
   LiveWindow::GetInstance()->AddSensor("AnalogGyro", m_analog->GetChannel(),
@@ -154,20 +179,9 @@ void AnalogGyro::InitGyro() {
 
 void AnalogGyro::Calibrate() {
   if (StatusIsFatal()) return;
-
-  m_analog->InitAccumulator();
-
-  Wait(kCalibrationSampleTime);
-
-  int64_t value;
-  uint32_t count;
-  m_analog->GetAccumulatorOutput(value, count);
-
-  m_center = (uint32_t)((float)value / (float)count + .5);
-
-  m_offset = ((float)value / (float)count) - (float)m_center;
-  m_analog->SetAccumulatorCenter(m_center);
-  m_analog->ResetAccumulator();
+  int32_t status = 0;
+  calibrateAnalogGyro(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
 }
 
 /**
@@ -184,18 +198,10 @@ void AnalogGyro::Calibrate() {
  */
 float AnalogGyro::GetAngle() const {
   if (StatusIsFatal()) return 0.f;
-
-  int64_t rawValue;
-  uint32_t count;
-  m_analog->GetAccumulatorOutput(rawValue, count);
-
-  int64_t value = rawValue - (int64_t)((float)count * m_offset);
-
-  double scaledValue = value * 1e-9 * (double)m_analog->GetLSBWeight() *
-                       (double)(1 << m_analog->GetAverageBits()) /
-                       (m_analog->GetSampleRate() * m_voltsPerDegreePerSecond);
-
-  return (float)scaledValue;
+  int32_t status = 0;
+  float value = getAnalogGyroAngle(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
+  return value;
 }
 
 /**
@@ -207,10 +213,10 @@ float AnalogGyro::GetAngle() const {
  */
 double AnalogGyro::GetRate() const {
   if (StatusIsFatal()) return 0.0;
-
-  return (m_analog->GetAverageValue() - ((double)m_center + m_offset)) * 1e-9 *
-         m_analog->GetLSBWeight() /
-         ((1 << m_analog->GetOversampleBits()) * m_voltsPerDegreePerSecond);
+  int32_t status = 0;
+  double value = getAnalogGyroRate(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
+  return value;
 }
 
 /**
@@ -219,7 +225,13 @@ double AnalogGyro::GetRate() const {
  *
  * @return the current offset value
  */
-float AnalogGyro::GetOffset() const { return m_offset; }
+float AnalogGyro::GetOffset() const {
+  if (StatusIsFatal()) return 0.0;
+  int32_t status = 0;
+  float value = getAnalogGyroOffset(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
+  return value;
+}
 
 /**
  * Return the gyro center value. If run after calibration,
@@ -227,7 +239,13 @@ float AnalogGyro::GetOffset() const { return m_offset; }
  *
  * @return the current center value
  */
-uint32_t AnalogGyro::GetCenter() const { return m_center; }
+uint32_t AnalogGyro::GetCenter() const {
+  if (StatusIsFatal()) return 0;
+  int32_t status = 0;
+  uint32_t value = getAnalogGyroCenter(m_gyroHandle, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
+  return value;
+}
 
 /**
  * Set the gyro sensitivity.
@@ -239,7 +257,10 @@ uint32_t AnalogGyro::GetCenter() const { return m_center; }
  * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second
  */
 void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) {
-  m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+  int32_t status = 0;
+  setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, voltsPerDegreePerSecond,
+                                       &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
 }
 
 /**
@@ -253,8 +274,7 @@ void AnalogGyro::SetSensitivity(float voltsPerDegreePerSecond) {
  */
 void AnalogGyro::SetDeadband(float volts) {
   if (StatusIsFatal()) return;
-
-  int32_t deadband = volts * 1e9 / m_analog->GetLSBWeight() *
-                     (1 << m_analog->GetOversampleBits());
-  m_analog->SetAccumulatorDeadband(deadband);
+  int32_t status = 0;
+  setAnalogGyroDeadband(m_gyroHandle, volts, &status);
+  wpi_setErrorWithContext(status, getHALErrorMessage(status));
 }

wpilibj/athena.gradle

@@ -158,6 +158,7 @@ task jniHeaders {
             args 'edu.wpi.first.wpilibj.hal.JNIWrapper'
             args 'edu.wpi.first.wpilibj.hal.AccelerometerJNI'
             args 'edu.wpi.first.wpilibj.hal.AnalogJNI'
+            args 'edu.wpi.first.wpilibj.hal.AnalogGyroJNI'
             args 'edu.wpi.first.wpilibj.hal.CanTalonJNI'
             args 'edu.wpi.first.wpilibj.hal.CounterJNI'
             args 'edu.wpi.first.wpilibj.hal.DigitalGlitchFilterJNI'

wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp

@@ -0,0 +1,214 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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 <assert.h>
+#include <jni.h>
+#include "Log.h"
+
+#include "edu_wpi_first_wpilibj_hal_AnalogGyroJNI.h"
+
+#include "HAL/AnalogGyro.h"
+#include "HALUtil.h"
+
+// set the logging level
+TLogLevel analogGyroJNILogLevel = logWARNING;
+
+#define ANALOGGYROJNI_LOG(level)     \
+  if (level > analogGyroJNILogLevel) \
+    ;                           \
+  else                          \
+  Log().Get(level)
+
+extern "C" {
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    initializeAnalogGyro
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_initializeAnalogGyro(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI initializeAnalogGyro";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Analog Input Handle = " << (HalAnalogInputHandle)id;
+  int32_t status = 0;
+  HalGyroHandle handle = initializeAnalogGyro((HalAnalogInputHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << handle;
+  return (jint) handle;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    setupAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setupAnalogGyro(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setupAnalogGyro";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  setupAnalogGyro((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    freeAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_freeAnalogGyro(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI freeAnalogGyro";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  freeAnalogGyro((HalGyroHandle)id);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    setAnalogGyroParameters
+ * Signature: (IFFI)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroParameters(
+    JNIEnv* env, jclass, jint id, jfloat vPDPS, jfloat offset, jint center) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroParameters";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  setAnalogGyroParameters((HalGyroHandle)id, vPDPS, offset, center, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    setAnalogGyroVoltsPerDegreePerSecond
+ * Signature: (IF)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroVoltsPerDegreePerSecond(
+    JNIEnv* env, jclass, jint id, jfloat vPDPS) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroVoltsPerDegreePerSecond";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  ANALOGGYROJNI_LOG(logDEBUG) << "vPDPS = " << vPDPS;
+  int32_t status = 0;
+  setAnalogGyroVoltsPerDegreePerSecond((HalGyroHandle)id, vPDPS, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    resetAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_resetAnalogGyro(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI resetAnalogGyro";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  resetAnalogGyro((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    calibrateAnalogGyro
+ * Signature: (I)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_calibrateAnalogGyro(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI calibrateAnalogGyro";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  calibrateAnalogGyro((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    setAnalogGyroDeadband
+ * Signature: (IF)V
+ */
+JNIEXPORT void JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_setAnalogGyroDeadband(
+    JNIEnv* env, jclass, jint id, jfloat deadband) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI setAnalogGyroDeadband";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  setAnalogGyroDeadband((HalGyroHandle)id, deadband, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  CheckStatus(env, status);
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    getAnalogGyroAngle
+ * Signature: (I)F
+ */
+JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroAngle(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroAngle";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  jfloat value = getAnalogGyroAngle((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value;
+  CheckStatus(env, status);
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    getAnalogGyroRate
+ * Signature: (I)D
+ */
+JNIEXPORT jdouble JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroRate(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroRate";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  jdouble value = getAnalogGyroRate((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value;
+  CheckStatus(env, status);
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    getAnalogGyroOffset
+ * Signature: (I)F
+ */
+JNIEXPORT jfloat JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroOffset(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroOffset";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  jfloat value = getAnalogGyroOffset((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value;
+  CheckStatus(env, status);
+  return value;
+}
+
+/*
+ * Class:     edu_wpi_first_wpilibj_hal_AnalogGyroJNI
+ * Method:    getAnalogGyroCenter
+ * Signature: (I)I
+ */
+JNIEXPORT jint JNICALL Java_edu_wpi_first_wpilibj_hal_AnalogGyroJNI_getAnalogGyroCenter(
+    JNIEnv* env, jclass, jint id) {
+  ANALOGGYROJNI_LOG(logDEBUG) << "Calling ANALOGGYROJNI getAnalogGyroCenter";
+  ANALOGGYROJNI_LOG(logDEBUG) << "Gyro Handle = " << (HalGyroHandle)id;
+  int32_t status = 0;
+  jint value = getAnalogGyroCenter((HalGyroHandle)id, &status);
+  ANALOGGYROJNI_LOG(logDEBUG) << "Status = " << status;
+  ANALOGGYROJNI_LOG(logDEBUG) << "Result = " << value;
+  CheckStatus(env, status);
+  return value;
+}
+
+}  // extern "C"

wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java

@@ -9,6 +9,7 @@ package edu.wpi.first.wpilibj;
 
 import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
 import edu.wpi.first.wpilibj.communication.UsageReporting;
+import edu.wpi.first.wpilibj.hal.AnalogGyroJNI;
 import edu.wpi.first.wpilibj.interfaces.Gyro;
 import edu.wpi.first.wpilibj.livewindow.LiveWindow;
 import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
@@ -24,34 +25,22 @@ import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
  */
 public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable {
 
-  private static final int kOversampleBits = 10;
-  private static final int kAverageBits = 0;
-  private static final double kSamplesPerSecond = 50.0;
-  private static final double kCalibrationSampleTime = 5.0;
-  private static final double kDefaultVoltsPerDegreePerSecond = 0.007;
+  private static final float kDefaultVoltsPerDegreePerSecond = 0.007f;
   protected AnalogInput m_analog;
-  private double m_voltsPerDegreePerSecond;
-  private double m_offset;
-  private int m_center;
   private boolean m_channelAllocated = false;
-  private AccumulatorResult m_result;
+  
+  private int m_gyroHandle = 0;
 
   /**
    * Initialize the gyro. Calibration is handled by calibrate().
    */
   public void initGyro() {
-    m_result = new AccumulatorResult();
+    
+    if (m_gyroHandle == 0) {
+      m_gyroHandle = AnalogGyroJNI.initializeAnalogGyro(m_analog.m_port);
+    }
 
-    m_voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
-    m_analog.setAverageBits(kAverageBits);
-    m_analog.setOversampleBits(kOversampleBits);
-    double sampleRate = kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
-    AnalogInput.setGlobalSampleRate(sampleRate);
-    Timer.delay(0.1);
-
-    setDeadband(0.0);
-
-    setPIDSourceType(PIDSourceType.kDisplacement);
+    AnalogGyroJNI.setupAnalogGyro(m_gyroHandle);
 
     UsageReporting.report(tResourceType.kResourceType_Gyro, m_analog.getChannel());
     LiveWindow.addSensor("AnalogGyro", m_analog.getChannel(), this);
@@ -59,19 +48,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
 
   @Override
   public void calibrate() {
-    m_analog.initAccumulator();
-    m_analog.resetAccumulator();
-
-    Timer.delay(kCalibrationSampleTime);
-
-    m_analog.getAccumulatorOutput(m_result);
-
-    m_center = (int) ((double) m_result.value / (double) m_result.count + .5);
-
-    m_offset = ((double) m_result.value / (double) m_result.count) - m_center;
-
-    m_analog.setAccumulatorCenter(m_center);
-    m_analog.resetAccumulator();
+    AnalogGyroJNI.calibrateAnalogGyro(m_gyroHandle);
   }
 
   /**
@@ -130,18 +107,14 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
       throw new NullPointerException("AnalogInput supplied to Gyro constructor is null");
     }
     initGyro();
-    m_offset = offset;
-    m_center = center;
-
-    m_analog.setAccumulatorCenter(m_center);
-    m_analog.resetAccumulator();
+    AnalogGyroJNI.setAnalogGyroParameters(m_gyroHandle, kDefaultVoltsPerDegreePerSecond, 
+                                          (float)offset, center);
+    reset();
   }
 
   @Override
   public void reset() {
-    if (m_analog != null) {
-      m_analog.resetAccumulator();
-    }
+    AnalogGyroJNI.resetAnalogGyro(m_gyroHandle);
   }
 
   /**
@@ -153,22 +126,15 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
       m_analog.free();
     }
     m_analog = null;
+    AnalogGyroJNI.freeAnalogGyro(m_gyroHandle);
   }
 
   @Override
-  public synchronized double getAngle() {
+  public double getAngle() {
     if (m_analog == null) {
       return 0.0;
     } else {
-      m_analog.getAccumulatorOutput(m_result);
-
-      long value = m_result.value - (long) (m_result.count * m_offset);
-
-      double scaledValue =
-          value * 1e-9 * m_analog.getLSBWeight() * (1 << m_analog.getAverageBits())
-              / (AnalogInput.getGlobalSampleRate() * m_voltsPerDegreePerSecond);
-
-      return scaledValue;
+      return AnalogGyroJNI.getAnalogGyroAngle(m_gyroHandle);
     }
   }
 
@@ -177,8 +143,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
     if (m_analog == null) {
       return 0.0;
     } else {
-      return (m_analog.getAverageValue() - (m_center + m_offset)) * 1e-9 * m_analog.getLSBWeight()
-          / ((1 << m_analog.getOversampleBits()) * m_voltsPerDegreePerSecond);
+      return AnalogGyroJNI.getAnalogGyroRate(m_gyroHandle);
     }
   }
 
@@ -188,7 +153,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
    * @return the current offset value
    */
   public double getOffset() {
-    return m_offset;
+    return AnalogGyroJNI.getAnalogGyroOffset(m_gyroHandle);
   }
 
   /**
@@ -197,7 +162,7 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
    * @return the current center value
    */
   public int getCenter() {
-    return m_center;
+    return AnalogGyroJNI.getAnalogGyroCenter(m_gyroHandle);
   }
 
   /**
@@ -208,7 +173,8 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
    * @param voltsPerDegreePerSecond The sensitivity in Volts/degree/second.
    */
   public void setSensitivity(double voltsPerDegreePerSecond) {
-    m_voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+    AnalogGyroJNI.setAnalogGyroVoltsPerDegreePerSecond(m_gyroHandle, 
+                                                      (float)voltsPerDegreePerSecond);
   }
 
   /**
@@ -219,8 +185,6 @@ public class AnalogGyro extends GyroBase implements Gyro, PIDSource, LiveWindowS
    * @param volts The size of the deadband in volts
    */
   void setDeadband(double volts) {
-    int deadband =
-        (int) (volts * 1e9 / m_analog.getLSBWeight() * (1 << m_analog.getOversampleBits()));
-    m_analog.setAccumulatorDeadband(deadband);
+    AnalogGyroJNI.setAnalogGyroDeadband(m_gyroHandle, (float)volts);
   }
 }

wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java

@@ -0,0 +1,37 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016. 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.                                                               */
+/*----------------------------------------------------------------------------*/
+
+package edu.wpi.first.wpilibj.hal;
+
+public class AnalogGyroJNI extends JNIWrapper {
+  public static native int initializeAnalogGyro(int halAnalogInputHandle);
+  
+  public static native void setupAnalogGyro(int handle);
+  
+  public static native void freeAnalogGyro(int handle);
+  
+  public static native void setAnalogGyroParameters(int handle, 
+                                                    float voltsPerDegreePerSecond, 
+                                                    float offset, int center);
+
+  public static native void setAnalogGyroVoltsPerDegreePerSecond(int handle,
+                                                                 float voltsPerDegreePerSecond);
+  
+  public static native void resetAnalogGyro(int handle);
+  
+  public static native void calibrateAnalogGyro(int handle);
+  
+  public static native void setAnalogGyroDeadband(int handle, float volts);
+  
+  public static native float getAnalogGyroAngle(int handle);
+  
+  public static native double getAnalogGyroRate(int handle);
+  
+  public static native float getAnalogGyroOffset(int handle);
+  
+  public static native int getAnalogGyroCenter(int handle);
+}