sync dma code with Jerry's version and add analog input support
This is commit 5bfd789db4119f8e387734f7d0de1e2860b639d2 in the dma
repositories.
Change-Id: Ida340f89b03ca53b62c093e6de2cee3231ae2182
diff --git a/aos/externals/forwpilib/README b/aos/externals/forwpilib/README
index d7ecb6c..faa1a78 100644
--- a/aos/externals/forwpilib/README
+++ b/aos/externals/forwpilib/README
@@ -1,2 +1,4 @@
This directory contains files that we plan to eventually push to upstream
WPILib but haven't yet.
+
+The canonical location for dma.* is robotics.mvla.net:/www/https/git/frc971/jerry/dma.git.
diff --git a/aos/externals/forwpilib/dma.cc b/aos/externals/forwpilib/dma.cc
index 279929f..f775ff5 100644
--- a/aos/externals/forwpilib/dma.cc
+++ b/aos/externals/forwpilib/dma.cc
@@ -1,6 +1,13 @@
#include "dma.h"
#include <algorithm>
+#include <type_traits>
+
+#include "DigitalSource.h"
+#include "AnalogInput.h"
+#include "Encoder.h"
+
+// Interface to the roboRIO FPGA's DMA features.
// Like tEncoder::tOutput with the bitfields reversed.
typedef union {
@@ -102,6 +109,27 @@
wpi_setErrorWithContext(status, getHALErrorMessage(status));
}
+void DMA::Add(AnalogInput *input) {
+ tRioStatusCode status = 0;
+
+ if (manager_) {
+ wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
+ "DMA::Add() only works before DMA::Start()");
+ return;
+ }
+
+ fprintf(stderr, "DMA::Add(AnalogInput*) needs testing. aborting\n");
+ abort();
+
+ // TODO(brian): Figure out if this math is actually correct.
+ if (input->GetChannel() <= 3) {
+ tdma_config_->writeConfig_Enable_AI0_Low(true, &status);
+ } else {
+ tdma_config_->writeConfig_Enable_AI0_High(true, &status);
+ }
+ wpi_setErrorWithContext(status, getHALErrorMessage(status));
+}
+
void DMA::SetExternalTrigger(DigitalSource *input, bool rising, bool falling) {
tRioStatusCode status = 0;
@@ -163,10 +191,10 @@
}
DMA::ReadStatus DMA::Read(DMASample *sample, uint32_t timeout_ms,
- size_t *remaining) {
+ size_t *remaining_out) {
tRioStatusCode status = 0;
size_t remainingBytes = 0;
- *remaining = 0;
+ *remaining_out = 0;
if (!manager_.get()) {
wpi_setErrorWithContext(NiFpga_Status_InvalidParameter,
@@ -174,6 +202,7 @@
return STATUS_ERROR;
}
+ sample->dma_ = this;
// memset(&sample->read_buffer_, 0, sizeof(read_buffer_));
manager_->read(sample->read_buffer_, capture_size_, timeout_ms,
&remainingBytes, &status);
@@ -192,11 +221,10 @@
}
// TODO(jerry): Do this only if status == 0?
- *remaining = remainingBytes / capture_size_;
- sample->dma_ = this;
+ *remaining_out = remainingBytes / capture_size_;
if (0) { // DEBUG
- printf("Remaining samples = %d\n", *remaining);
+ printf("Remaining samples = %d\n", *remaining_out);
}
// TODO(austin): Check that *remainingBytes % capture_size_ == 0 and deal
@@ -211,6 +239,15 @@
}
}
+const char *DMA::NameOfReadStatus(ReadStatus s) {
+ switch (s) {
+ case STATUS_OK: return "OK";
+ case STATUS_TIMEOUT: return "TIMEOUT";
+ case STATUS_ERROR: return "ERROR";
+ default: return "(bad ReadStatus code)";
+ }
+}
+
void DMA::Start(size_t queue_depth) {
tRioStatusCode status = 0;
tconfig_ = tdma_config_->readConfig(&status);
@@ -274,6 +311,8 @@
}
}
+static_assert(::std::is_pod<DMASample>::value, "DMASample needs to be POD");
+
ssize_t DMASample::offset(int index) const { return dma_->channel_offsets_[index]; }
double DMASample::GetTimestamp() const {
@@ -322,8 +361,7 @@
} else if (1) {
// Extract the 31-bit signed tEncoder::tOutput Value using right-shift.
// This works even though C/C++ doesn't guarantee whether signed >> does
- // arithmetic or logical shift. (dmaWord / 2) is not a great alternative
- // since it rounds.
+ // arithmetic or logical shift. (dmaWord / 2) would fix that but it rounds.
result = static_cast<int32_t>(dmaWord) >> 1;
}
#if 0 // This approach was recommended but it doesn't return the right value.
@@ -343,6 +381,33 @@
int32_t DMASample::Get(Encoder *input) const {
int32_t raw = GetRaw(input);
- // TODO(austin): Really bad... DecodingScaleFactor?
- return raw / 4.0;
+ return raw / input->GetEncodingScale();
+}
+
+int16_t DMASample::GetValue(AnalogInput *input) const {
+ if (offset(kEnable_Encoders) == -1) {
+ wpi_setStaticErrorWithContext(dma_,
+ NiFpga_Status_ResourceNotFound,
+ getHALErrorMessage(NiFpga_Status_ResourceNotFound));
+ return -1;
+ }
+
+ uint32_t dmaWord;
+ // TODO(brian): Figure out if this math is actually correct.
+ if (input->GetChannel() <= 3) {
+ dmaWord = read_buffer_[offset(kEnable_AI0_Low) + input->GetChannel()];
+ } else {
+ dmaWord = read_buffer_[offset(kEnable_AI0_High) + input->GetChannel() - 4];
+ }
+ // TODO(brian): Verify that this is correct.
+ return static_cast<int16_t>(dmaWord);
+}
+
+float DMASample::GetVoltage(AnalogInput *input) const {
+ int16_t value = GetValue(input);
+ if (value == -1) return 0.0;
+ uint32_t lsb_weight = input->GetLSBWeight();
+ int32_t offset = input->GetOffset();
+ float voltage = lsb_weight * 1.0e-9 * value - offset * 1.0e-9;
+ return voltage;
}
diff --git a/aos/externals/forwpilib/dma.h b/aos/externals/forwpilib/dma.h
index 0baf473..446ded4 100644
--- a/aos/externals/forwpilib/dma.h
+++ b/aos/externals/forwpilib/dma.h
@@ -1,20 +1,26 @@
#ifndef _DMA_H_
#define _DMA_H_
+// Interface to the roboRIO FPGA's DMA features.
+
#include <stdint.h>
#include <array>
#include <memory>
#include "ChipObject.h"
-#include "DigitalSource.h"
-#include "Encoder.h"
+#include "ErrorBase.h"
class DMA;
+class DigitalSource;
+class AnalogInput;
+class Encoder;
+// A POD class which stores the data from a DMA sample and provides safe ways to
+// access it.
class DMASample {
public:
- DMASample() {}
+ DMASample() = default;
// Returns the FPGA timestamp of the sample.
double GetTimestamp() const;
@@ -27,6 +33,10 @@
int32_t GetRaw(Encoder *input) const;
// Returns the {1, 2, or 4} X scaled value of the encoder in the sample.
int32_t Get(Encoder *input) const;
+ // Returns the raw 12-bit value from the ADC.
+ int16_t GetValue(AnalogInput *input) const;
+ // Returns the scaled value of an analog input.
+ float GetVoltage(AnalogInput *input) const;
private:
friend DMA;
@@ -42,6 +52,7 @@
uint32_t read_buffer_[64];
};
+// TODO(austin): ErrorBase...
class DMA : public ErrorBase {
public:
DMA();
@@ -54,9 +65,12 @@
void SetRate(uint32_t cycles);
// Adds the input signal to the state to snapshot on the trigger event.
+ // It is safe to add the same input multiple times, but there is currently
+ // no way to remove one once it has been added.
// Call Add() and SetExternalTrigger() before Start().
void Add(Encoder *encoder);
void Add(DigitalSource *input);
+ void Add(AnalogInput *input);
// Configures DMA to trigger on an external trigger. There can only be 4
// external triggers.
@@ -77,9 +91,15 @@
// Reads a sample from the DMA buffer, waiting up to timeout_ms for it.
// Returns a status code indicating whether the read worked, timed out, or
// failed.
+ // Returns in *remaining_out the number of DMA samples still queued after this
+ // Read().
// Call Add() and SetExternalTrigger() then Start() before Read().
// The sample is only usable while this DMA object is left started.
- ReadStatus Read(DMASample *sample, uint32_t timeout_ms, size_t *remaining);
+ ReadStatus Read(DMASample *sample, uint32_t timeout_ms,
+ size_t *remaining_out);
+
+ // Translates a ReadStatus code to a string name.
+ static const char *NameOfReadStatus(ReadStatus s);
private:
::std::unique_ptr<nFPGA::tDMAManager> manager_; // set by Start()
diff --git a/aos/externals/forwpilib/dma_test.cc b/aos/externals/forwpilib/dma_test.cc
deleted file mode 100644
index 7ef7ab1..0000000
--- a/aos/externals/forwpilib/dma_test.cc
+++ /dev/null
@@ -1,222 +0,0 @@
-#include <memory>
-#include <thread>
-#define __STDC_FORMAT_MACROS
-#include <inttypes.h>
-#include <atomic>
-#include <mutex>
-#include <sched.h>
-#include <assert.h>
-#include <WPILib.h>
-#include "dma.h"
-#include <signal.h>
-
-::std::atomic<double> last_time;
-
-class priority_mutex {
- public:
- typedef pthread_mutex_t *native_handle_type;
-
- // TODO(austin): Write a test case for the mutex, and make the constructor
- // constexpr.
- priority_mutex() {
- pthread_mutexattr_t attr;
- // Turn on priority inheritance.
- assert_perror(pthread_mutexattr_init(&attr));
- assert_perror(pthread_mutexattr_settype(&attr, PTHREAD_MUTEX_NORMAL));
- assert_perror(pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT));
-
- assert_perror(pthread_mutex_init(native_handle(), &attr));
-
- assert_perror(pthread_mutexattr_destroy(&attr));
- }
-
- ~priority_mutex() { pthread_mutex_destroy(&handle_); }
-
- void lock() { assert_perror(pthread_mutex_lock(&handle_)); }
- bool try_lock() {
- int ret = pthread_mutex_trylock(&handle_);
- if (ret == 0) {
- return true;
- } else if (ret == EBUSY) {
- return false;
- } else {
- assert_perror(ret);
- }
- }
- void unlock() { assert_perror(pthread_mutex_unlock(&handle_)); }
-
- native_handle_type native_handle() { return &handle_; }
-
- private:
- DISALLOW_COPY_AND_ASSIGN(priority_mutex);
- pthread_mutex_t handle_;
-};
-
-class EdgePrinter {
- public:
- EdgePrinter(DigitalInput *sensor)
- : quit_(false),
- sensor_(sensor),
- interrupt_count_(0) {
- }
-
- void Start() {
- printf("Creating thread %d\n", sensor_->GetChannel());
- thread_.reset(new ::std::thread(::std::ref(*this)));
- }
-
- void operator ()() {
- struct sched_param param;
- param.sched_priority = 55;
- if (sched_setscheduler(0, SCHED_FIFO, ¶m) == -1) {
- perror("sched_setscheduler failed");
- exit(-1);
- }
-
- printf("Started thread %d\n", sensor_->GetChannel());
-
- sensor_->RequestInterrupts();
- sensor_->SetUpSourceEdge(true, false);
-
- InterruptableSensorBase::WaitResult result = InterruptableSensorBase::kBoth;
- while (!quit_) {
- result = sensor_->WaitForInterrupt(
- 0.1, result != InterruptableSensorBase::kTimeout);
- if (result != InterruptableSensorBase::kTimeout) {
- ++interrupt_count_;
- printf("Got %d edges on %d\n", interrupt_count_.load(),
- sensor_->GetChannel());
- }
- }
- }
-
- int interrupt_count() const { return interrupt_count_; }
-
- void quit() {
- quit_ = true;
- thread_->join();
- }
-
- DigitalInput *sensor() { return sensor_.get(); }
-
- private:
- ::std::atomic<bool> quit_;
- ::std::unique_ptr<DigitalInput> sensor_;
- ::std::atomic<int> interrupt_count_;
- ::std::unique_ptr<::std::thread> thread_;
-};
-
-class TestRobot;
-static TestRobot *my_robot;
-
-class TestRobot : public RobotBase {
- public:
- static void HandleSigIntStatic(int signal) { my_robot->HandleSigInt(signal); }
- void HandleSigInt(int /*signal*/) { quit_ = true; }
-
- ::std::unique_ptr<Encoder> MakeEncoder(int index) {
- return ::std::unique_ptr<Encoder>(
- new Encoder(sensor(10 + 2 * index), sensor(11 + 2 * index)));
- }
-
- ::std::vector<::std::unique_ptr<EdgePrinter>> printers;
- ::std::vector<::std::unique_ptr<DigitalInput>> dio;
-
- DigitalInput *sensor(int i) {
- if (i < 8) {
- return printers[i]->sensor();
- } else {
- return dio[i - 8].get();
- }
- }
-
- void AllEdgeTests() {
- my_robot = this;
- quit_ = false;
- struct sigaction sa;
-
- memset(&sa, 0, sizeof(sa));
- // Setup the sighub handler
- sa.sa_handler = &TestRobot::HandleSigIntStatic;
-
- // Restart the system call, if at all possible
- sa.sa_flags = SA_RESTART;
-
- // Block every signal during the handler
- sigfillset(&sa.sa_mask);
-
- for (int i = 0; i < 8; ++i) {
- printers.emplace_back(new EdgePrinter(new DigitalInput(i)));
- }
- printf("Created all objects\n");
- for (auto &printer : printers) {
- printer->Start();
- }
-
- for (int i = 8; i < 26; ++i) {
- dio.emplace_back(new DigitalInput(i));
- }
-
- ::std::unique_ptr<Encoder> e0 = MakeEncoder(0);
- ::std::unique_ptr<Encoder> e1 = MakeEncoder(1);
- ::std::unique_ptr<Encoder> e2 = MakeEncoder(2);
- ::std::unique_ptr<Encoder> e3 = MakeEncoder(3);
-
- DMA dma;
-
- dma.Add(sensor(6));
- dma.Add(e0.get());
- dma.SetExternalTrigger(sensor(6), true, true);
- dma.Start();
- while (!quit_) {
- printf("Battery voltage %f\n",
- DriverStation::GetInstance()->GetBatteryVoltage());
-
- DMASample dma_sample;
- size_t left;
- DMA::ReadStatus dma_read_return = dma.Read(&dma_sample, 1000, &left);
- printf("dma_read %d, items left %d\n", dma_read_return, left);
-
- if (left >= 0) {
- uint32_t sensor_value = 0;
- uint32_t dma_sensor_value = 0;
- for (int i = 0; i < 26; ++i) {
- int j = i;
- if (j >= 10) j += 6;
- sensor_value |= (static_cast<uint32_t>(sensor(i)->Get()) << j);
- dma_sensor_value |= (static_cast<uint32_t>(dma_sample.Get(sensor(i)) << j));
- }
-
- printf("dio 0x%x\n", sensor_value);
- printf("dma 0x%x\n", dma_sensor_value);
- printf("e0 %d, e0_dma %d\n", e0->GetRaw(), dma_sample.GetRaw(e0.get()));
- printf("e1 %d, e1_dma %d\n", e1->GetRaw(), dma_sample.GetRaw(e1.get()));
- printf("e2 %d, e2_dma %d\n", e2->GetRaw(), dma_sample.GetRaw(e2.get()));
- printf("e3 %d, e3_dma %d\n", e3->GetRaw(), dma_sample.GetRaw(e3.get()));
- printf("timestamp %f %f\n", dma_sample.GetTimestamp(),
- static_cast<double>(GetFPGATime()) * 0.000001);
-
- printf("Remaining is %d\n", left);
- }
- }
- // Wait(0.5);
- for (auto &printer : printers) {
- printer->quit();
- }
- }
-
- virtual void StartCompetition() {
- AllEdgeTests();
- }
-
- private:
- ::std::unique_ptr<Encoder> encoder_;
- ::std::unique_ptr<Encoder> test_encoder_;
- ::std::unique_ptr<Talon> talon_;
- ::std::unique_ptr<DigitalInput> hall_;
-
- ::std::atomic<bool> quit_;
- ::std::mutex encoder_mutex_;
-};
-
-START_ROBOT_CLASS(TestRobot);