Moves Gyros to the HAL (#131)

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;
+}
+}