Squashed 'third_party/allwpilib_2017/' content from commit 35ac87d
Change-Id: I7bb6f5556c30d3f5a092e68de0be9c710c60c9f4
git-subtree-dir: third_party/allwpilib_2017
git-subtree-split: 35ac87d6ff8b7f061c4f18c9ea316e5dccd4888a
diff --git a/hal/lib/athena/AnalogGyro.cpp b/hal/lib/athena/AnalogGyro.cpp
new file mode 100644
index 0000000..8d3d82d
--- /dev/null
+++ b/hal/lib/athena/AnalogGyro.cpp
@@ -0,0 +1,242 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2016-2017. 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 "HAL/handles/IndexedHandleResource.h"
+
+namespace {
+struct AnalogGyro {
+ HAL_AnalogInputHandle handle;
+ double voltsPerDegreePerSecond;
+ double offset;
+ int32_t center;
+};
+}
+
+static constexpr uint32_t kOversampleBits = 10;
+static constexpr uint32_t kAverageBits = 0;
+static constexpr double kSamplesPerSecond = 50.0;
+static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr double kDefaultVoltsPerDegreePerSecond = 0.007;
+
+using namespace hal;
+
+static IndexedHandleResource<HAL_GyroHandle, AnalogGyro, kNumAccumulators,
+ HAL_HandleEnum::AnalogGyro>
+ analogGyroHandles;
+
+static void Wait(double seconds) {
+ if (seconds < 0.0) return;
+ std::this_thread::sleep_for(std::chrono::duration<double>(seconds));
+}
+
+extern "C" {
+HAL_GyroHandle HAL_InitializeAnalogGyro(HAL_AnalogInputHandle analogHandle,
+ int32_t* status) {
+ if (!HAL_IsAccumulatorChannel(analogHandle, status)) {
+ if (*status == 0) {
+ *status = HAL_INVALID_ACCUMULATOR_CHANNEL;
+ }
+ return HAL_kInvalidHandle;
+ }
+
+ // handle known to be correct, so no need to type check
+ int16_t channel = getHandleIndex(analogHandle);
+
+ auto handle = analogGyroHandles.Allocate(channel, status);
+
+ if (*status != 0)
+ return HAL_kInvalidHandle; // 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_kInvalidHandle;
+ }
+
+ gyro->handle = analogHandle;
+ gyro->voltsPerDegreePerSecond = 0;
+ gyro->offset = 0;
+ gyro->center = 0;
+
+ return handle;
+}
+
+void HAL_SetupAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ gyro->voltsPerDegreePerSecond = kDefaultVoltsPerDegreePerSecond;
+
+ HAL_SetAnalogAverageBits(gyro->handle, kAverageBits, status);
+ if (*status != 0) return;
+ HAL_SetAnalogOversampleBits(gyro->handle, kOversampleBits, status);
+ if (*status != 0) return;
+ double sampleRate =
+ kSamplesPerSecond * (1 << (kAverageBits + kOversampleBits));
+ HAL_SetAnalogSampleRate(sampleRate, status);
+ if (*status != 0) return;
+ Wait(0.1);
+
+ HAL_SetAnalogGyroDeadband(handle, 0.0, status);
+ if (*status != 0) return;
+}
+
+void HAL_FreeAnalogGyro(HAL_GyroHandle handle) {
+ analogGyroHandles.Free(handle);
+}
+
+void HAL_SetAnalogGyroParameters(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond, double offset,
+ int32_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;
+ HAL_SetAccumulatorCenter(gyro->handle, center, status);
+}
+
+void HAL_SetAnalogGyroVoltsPerDegreePerSecond(HAL_GyroHandle handle,
+ double voltsPerDegreePerSecond,
+ int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ gyro->voltsPerDegreePerSecond = voltsPerDegreePerSecond;
+}
+
+void HAL_ResetAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ HAL_ResetAccumulator(gyro->handle, status);
+ if (*status != 0) return;
+
+ const double sampleTime = 1.0 / HAL_GetAnalogSampleRate(status);
+ const double overSamples =
+ 1 << HAL_GetAnalogOversampleBits(gyro->handle, status);
+ const double averageSamples =
+ 1 << HAL_GetAnalogAverageBits(gyro->handle, status);
+ if (*status != 0) return;
+ Wait(sampleTime * overSamples * averageSamples);
+}
+
+void HAL_CalibrateAnalogGyro(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+
+ HAL_InitAccumulator(gyro->handle, status);
+ if (*status != 0) return;
+ Wait(kCalibrationSampleTime);
+
+ int64_t value;
+ int64_t count;
+ HAL_GetAccumulatorOutput(gyro->handle, &value, &count, status);
+ if (*status != 0) return;
+
+ gyro->center = static_cast<int32_t>(
+ static_cast<double>(value) / static_cast<double>(count) + .5);
+
+ gyro->offset = static_cast<double>(value) / static_cast<double>(count) -
+ static_cast<double>(gyro->center);
+ HAL_SetAccumulatorCenter(gyro->handle, gyro->center, status);
+ if (*status != 0) return;
+ HAL_ResetAnalogGyro(handle, status);
+}
+
+void HAL_SetAnalogGyroDeadband(HAL_GyroHandle handle, double volts,
+ int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return;
+ }
+ int32_t deadband = static_cast<int32_t>(
+ volts * 1e9 / HAL_GetAnalogLSBWeight(gyro->handle, status) *
+ (1 << HAL_GetAnalogOversampleBits(gyro->handle, status)));
+ if (*status != 0) return;
+ HAL_SetAccumulatorDeadband(gyro->handle, deadband, status);
+}
+
+double HAL_GetAnalogGyroAngle(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ int64_t rawValue = 0;
+ int64_t count = 0;
+ HAL_GetAccumulatorOutput(gyro->handle, &rawValue, &count, status);
+
+ int64_t value = rawValue - static_cast<int64_t>(static_cast<double>(count) *
+ gyro->offset);
+
+ double scaledValue =
+ value * 1e-9 *
+ static_cast<double>(HAL_GetAnalogLSBWeight(gyro->handle, status)) *
+ static_cast<double>(1 << HAL_GetAnalogAverageBits(gyro->handle, status)) /
+ (HAL_GetAnalogSampleRate(status) * gyro->voltsPerDegreePerSecond);
+
+ return scaledValue;
+}
+
+double HAL_GetAnalogGyroRate(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+
+ return (HAL_GetAnalogAverageValue(gyro->handle, status) -
+ (static_cast<double>(gyro->center) + gyro->offset)) *
+ 1e-9 * HAL_GetAnalogLSBWeight(gyro->handle, status) /
+ ((1 << HAL_GetAnalogOversampleBits(gyro->handle, status)) *
+ gyro->voltsPerDegreePerSecond);
+}
+
+double HAL_GetAnalogGyroOffset(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return gyro->offset;
+}
+
+int32_t HAL_GetAnalogGyroCenter(HAL_GyroHandle handle, int32_t* status) {
+ auto gyro = analogGyroHandles.Get(handle);
+ if (gyro == nullptr) {
+ *status = HAL_HANDLE_ERROR;
+ return 0;
+ }
+ return gyro->center;
+}
+}