Add ADXRS450_Gyro.

This is a digital gyro interfaced via SPI.  Uses the HAL SPI accumulator.

Basic code function successfully tested on a ADXRS453.

Change-Id: Ibc0c7db9964c041fb1e04af4db17e3310ea83c04

wpilibc/Athena/include/ADXRS450_Gyro.h

@@ -0,0 +1,44 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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 "GyroBase.h"
+#include "Notifier.h"
+#include "SPI.h"
+#include "HAL/cpp/priority_mutex.h"
+
+#include <memory>
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ * The Gyro class tracks the robots heading based on the starting position. As
+ * the robot rotates the new heading is computed by integrating the rate of
+ * rotation returned by the sensor. When the class is instantiated, it does a
+ * short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to
+ * determine the heading.
+ *
+ * This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ */
+class ADXRS450_Gyro : public GyroBase {
+ public:
+  ADXRS450_Gyro();
+  explicit ADXRS450_Gyro(SPI::Port port);
+  virtual ~ADXRS450_Gyro() = default;
+
+  float GetAngle() const override;
+  double GetRate() const override;
+  void Reset() override;
+  void Calibrate() override;
+
+  std::string GetSmartDashboardType() const override;
+
+ private:
+  SPI m_spi;
+
+  uint16_t ReadRegister(uint8_t reg);
+};

wpilibc/Athena/include/WPILib.h

@@ -14,6 +14,7 @@
 #include "ADXL345_I2C.h"
 #include "ADXL345_SPI.h"
 #include "ADXL362.h"
+#include "ADXRS450_Gyro.h"
 #include "AnalogAccelerometer.h"
 #include "AnalogGyro.h"
 #include "AnalogInput.h"

wpilibc/Athena/src/ADXRS450_Gyro.cpp

@@ -0,0 +1,157 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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_Gyro.h"
+#include "DriverStation.h"
+#include "LiveWindow/LiveWindow.h"
+#include "Timer.h"
+
+static constexpr double kSamplePeriod = 0.001;
+static constexpr double kCalibrationSampleTime = 5.0;
+static constexpr double kDegreePerSecondPerLSB = 0.0125;
+
+static constexpr uint8_t kRateRegister = 0x00;
+static constexpr uint8_t kTemRegister = 0x02;
+static constexpr uint8_t kLoCSTRegister = 0x04;
+static constexpr uint8_t kHiCSTRegister = 0x06;
+static constexpr uint8_t kQuadRegister = 0x08;
+static constexpr uint8_t kFaultRegister = 0x0A;
+static constexpr uint8_t kPIDRegister = 0x0C;
+static constexpr uint8_t kSNHighRegister = 0x0E;
+static constexpr uint8_t kSNLowRegister = 0x10;
+
+/**
+ * Initialize the gyro.
+ * Calibrate the gyro by running for a number of samples and computing the
+ * center value.
+ * Then use the center value as the Accumulator center value for subsequent
+ * measurements.
+ * It's important to make sure that the robot is not moving while the centering
+ * calculations are in progress, this is typically done when the robot is first
+ * turned on while it's sitting at rest before the competition starts.
+ */
+void ADXRS450_Gyro::Calibrate() {
+  Wait(0.1);
+
+  m_spi.SetAccumulatorCenter(0);
+  m_spi.ResetAccumulator();
+
+  Wait(kCalibrationSampleTime);
+
+  m_spi.SetAccumulatorCenter((int32_t)m_spi.GetAccumulatorAverage());
+  m_spi.ResetAccumulator();
+}
+
+/**
+ * Gyro constructor on onboard CS0.
+ */
+ADXRS450_Gyro::ADXRS450_Gyro() : ADXRS450_Gyro(SPI::kOnboardCS0) {}
+
+/**
+ * Gyro constructor on the specified SPI port.
+ *
+ * @param port The SPI port the gyro is attached to.
+ */
+ADXRS450_Gyro::ADXRS450_Gyro(SPI::Port port) : m_spi(port) {
+  m_spi.SetClockRate(3000000);
+  m_spi.SetMSBFirst();
+  m_spi.SetSampleDataOnRising();
+  m_spi.SetClockActiveHigh();
+  m_spi.SetChipSelectActiveLow();
+
+  // Validate the part ID
+  if ((ReadRegister(kPIDRegister) & 0xff00) != 0x5200) {
+    DriverStation::ReportError("could not find ADXRS450 gyro");
+    return;
+  }
+
+  m_spi.InitAccumulator(kSamplePeriod, 0x20000000u, 4, 0x0c000000u, 0x04000000u,
+                        10u, 16u, true, true);
+
+  Calibrate();
+
+  //HALReport(HALUsageReporting::kResourceType_ADXRS450, port);
+  LiveWindow::GetInstance()->AddSensor("ADXRS450_Gyro", port, this);
+}
+
+static bool CalcParity(uint32_t v) {
+  bool parity = false;
+  while (v != 0) {
+    parity = !parity;
+    v = v & (v - 1);
+  }
+  return parity;
+}
+
+static inline uint32_t BytesToIntBE(uint8_t* buf) {
+  uint32_t result = ((uint32_t)buf[0]) << 24;
+  result |= ((uint32_t)buf[1]) << 16;
+  result |= ((uint32_t)buf[2]) << 8;
+  result |= (uint32_t)buf[3];
+  return result;
+}
+
+uint16_t ADXRS450_Gyro::ReadRegister(uint8_t reg) {
+  uint32_t cmd = 0x80000000 | (((uint32_t)reg) << 17);
+  if (!CalcParity(cmd)) cmd |= 1u;
+
+  // big endian
+  uint8_t buf[4] = {(uint8_t)((cmd >> 24) & 0xff),
+                    (uint8_t)((cmd >> 16) & 0xff),
+                    (uint8_t)((cmd >> 8) & 0xff),
+                    (uint8_t)(cmd & 0xff)};
+
+  m_spi.Write(buf, 4);
+  m_spi.Read(false, buf, 4);
+  if ((buf[0] & 0xe0) == 0) return 0;  // error, return 0
+  return (uint16_t)((BytesToIntBE(buf) >> 5) & 0xffff);
+}
+
+/**
+ * Reset the gyro.
+ * Resets the gyro to a heading of zero. This can be used if there is
+ * significant
+ * drift in the gyro and it needs to be recalibrated after it has been running.
+ */
+void ADXRS450_Gyro::Reset() {
+  m_spi.ResetAccumulator();
+}
+
+/**
+ * Return the actual angle in degrees that the robot is currently facing.
+ *
+ * The angle is based on the current accumulator value corrected by the
+ * oversampling rate, the
+ * gyro type and the A/D calibration values.
+ * The angle is continuous, that is it will continue from 360->361 degrees. This
+ * allows algorithms that wouldn't
+ * want to see a discontinuity in the gyro output as it sweeps from 360 to 0 on
+ * the second time around.
+ *
+ * @return the current heading of the robot in degrees. This heading is based on
+ * integration
+ * of the returned rate from the gyro.
+ */
+float ADXRS450_Gyro::GetAngle() const {
+  return (float)(m_spi.GetAccumulatorValue() * kDegreePerSecondPerLSB *
+                 kSamplePeriod);
+}
+
+/**
+ * Return the rate of rotation of the gyro
+ *
+ * The rate is based on the most recent reading of the gyro analog value
+ *
+ * @return the current rate in degrees per second
+ */
+double ADXRS450_Gyro::GetRate() const {
+  return (double)m_spi.GetAccumulatorLastValue() * kDegreePerSecondPerLSB;
+}
+
+std::string ADXRS450_Gyro::GetSmartDashboardType() const {
+  return "ADXRS450_Gyro";
+}

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

@@ -0,0 +1,169 @@
+/*----------------------------------------------------------------------------*/
+/* Copyright (c) FIRST 2015. 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;
+
+import java.nio.ByteOrder;
+import java.nio.ByteBuffer;
+
+import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tInstances;
+import edu.wpi.first.wpilibj.communication.FRCNetworkCommunicationsLibrary.tResourceType;
+import edu.wpi.first.wpilibj.communication.UsageReporting;
+import edu.wpi.first.wpilibj.interfaces.Gyro;
+import edu.wpi.first.wpilibj.livewindow.LiveWindow;
+import edu.wpi.first.wpilibj.livewindow.LiveWindowSendable;
+import edu.wpi.first.wpilibj.tables.ITable;
+
+/**
+ * Use a rate gyro to return the robots heading relative to a starting position.
+ * The Gyro class tracks the robots heading based on the starting position. As
+ * the robot rotates the new heading is computed by integrating the rate of
+ * rotation returned by the sensor. When the class is instantiated, it does a
+ * short calibration routine where it samples the gyro while at rest to
+ * determine the default offset. This is subtracted from each sample to
+ * determine the heading.
+ *
+ * This class is for the digital ADXRS450 gyro sensor that connects via SPI.
+ */
+public class ADXRS450_Gyro extends GyroBase implements Gyro, PIDSource, LiveWindowSendable {
+  private static final double kSamplePeriod = 0.001;
+  private static final double kCalibrationSampleTime = 5.0;
+  private static final double kDegreePerSecondPerLSB = 0.0125;
+
+  private static final int kRateRegister = 0x00;
+  private static final int kTemRegister = 0x02;
+  private static final int kLoCSTRegister = 0x04;
+  private static final int kHiCSTRegister = 0x06;
+  private static final int kQuadRegister = 0x08;
+  private static final int kFaultRegister = 0x0A;
+  private static final int kPIDRegister = 0x0C;
+  private static final int kSNHighRegister = 0x0E;
+  private static final int kSNLowRegister = 0x10;
+
+  private SPI m_spi;
+
+  /**
+   * Constructor.  Uses the onboard CS0.
+   */
+  public ADXRS450_Gyro() {
+    this(SPI.Port.kOnboardCS0);
+  }
+
+  /**
+   * Constructor.
+   *
+   * @param port The SPI port that the gyro is connected to
+   */
+  public ADXRS450_Gyro(SPI.Port port) {
+    m_spi = new SPI(port);
+    m_spi.setClockRate(3000000);
+    m_spi.setMSBFirst();
+    m_spi.setSampleDataOnRising();
+    m_spi.setClockActiveHigh();
+    m_spi.setChipSelectActiveLow();
+
+    // Validate the part ID
+    if ((readRegister(kPIDRegister) & 0xff00) != 0x5200) {
+      m_spi.free();
+      m_spi = null;
+      DriverStation.reportError("could not find ADXRS450 gyro on SPI port " + port.getValue(), false);
+      return;
+    }
+
+    m_spi.initAccumulator(kSamplePeriod, 0x20000000, 4, 0x0c000000, 0x04000000,
+        10, 16, true, true);
+
+    calibrate();
+
+    //UsageReporting.report(tResourceType.kResourceType_ADXRS450, port.getValue());
+    LiveWindow.addSensor("ADXRS450_Gyro", port.getValue(), this);
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  @Override
+  public void calibrate() {
+    if (m_spi == null) return;
+
+    Timer.delay(0.1);
+
+    m_spi.setAccumulatorCenter(0);
+    m_spi.resetAccumulator();
+
+    Timer.delay(kCalibrationSampleTime);
+
+    m_spi.setAccumulatorCenter((int)m_spi.getAccumulatorAverage());
+    m_spi.resetAccumulator();
+  }
+
+  private boolean calcParity(int v) {
+    boolean parity = false;
+    while (v != 0) {
+      parity = !parity;
+      v = v & (v - 1);
+    }
+    return parity;
+  }
+
+  private int readRegister(int reg) {
+    int cmdhi = 0x8000 | (reg << 1);
+    boolean parity = calcParity(cmdhi);
+
+    ByteBuffer buf = ByteBuffer.allocateDirect(4);
+    buf.order(ByteOrder.BIG_ENDIAN);
+    buf.put(0, (byte) (cmdhi >> 8));
+    buf.put(1, (byte) (cmdhi & 0xff));
+    buf.put(2, (byte) 0);
+    buf.put(3, (byte) (parity ? 0 : 1));
+
+    m_spi.write(buf, 4);
+    m_spi.read(false, buf, 4);
+
+    if ((buf.get(0) & 0xe0) == 0) {
+      return 0;  // error, return 0
+    }
+    return (buf.getInt(0) >> 5) & 0xffff;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public void reset() {
+    m_spi.resetAccumulator();
+  }
+
+  /**
+   * Delete (free) the spi port used for the gyro and stop accumulating.
+   */
+  @Override
+  public void free() {
+    if (m_spi != null) {
+      m_spi.free();
+      m_spi = null;
+    }
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public double getAngle() {
+    if (m_spi == null) return 0.0;
+    return m_spi.getAccumulatorValue() * kDegreePerSecondPerLSB * kSamplePeriod;
+  }
+
+  /**
+   * {@inheritDoc}
+   */
+  public double getRate() {
+    if (m_spi == null) return 0.0;
+    return m_spi.getAccumulatorLastValue() * kDegreePerSecondPerLSB;
+  }
+
+  public String getSmartDashboardType() {
+    return "ADXRS450_Gyro";
+  }
+}