Moves Gyros to the HAL (#131)

2026-07-05 03:21:42 +00:00 · 2016-07-07 21:31:45 -07:00
parent b036bf2e34
commit 0a983eeeb8
12 changed files with 632 additions and 127 deletions
--- a/wpilibj/src/athena/cpp/lib/AnalogGyroJNI.cpp
+++ b/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"
--- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/AnalogGyro.java
+++ b/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);
  }
 }
--- a/wpilibj/src/athena/java/edu/wpi/first/wpilibj/hal/AnalogGyroJNI.java
+++ b/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);
+}