Merge changes Ic134ff93,Id13bc733
* changes:
Add code to trigger ADC samples with hardware
Add Kinetis-K UART code
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
index 10fef28..ef2e833 100644
--- a/motors/peripheral/BUILD
+++ b/motors/peripheral/BUILD
@@ -1,43 +1,91 @@
load("//tools:environments.bzl", "mcu_cpus")
cc_library(
- name = 'adc',
- visibility = ['//visibility:public'],
- hdrs = [
- 'adc.h',
- ],
- srcs = [
- 'adc.cc',
- ],
- deps = [
- ':configuration',
- '//motors:util',
- '//motors/core',
- ],
- restricted_to = mcu_cpus,
+ name = "adc",
+ srcs = [
+ "adc.cc",
+ ],
+ hdrs = [
+ "adc.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":configuration",
+ "//motors:util",
+ "//motors/core",
+ ],
)
cc_library(
- name = 'configuration',
- visibility = ['//visibility:public'],
- hdrs = [
- 'configuration.h',
- ],
- restricted_to = mcu_cpus,
+ name = "configuration",
+ hdrs = [
+ "configuration.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
)
cc_library(
- name = 'can',
- visibility = ['//visibility:public'],
- hdrs = [
- 'can.h',
- ],
- srcs = [
- 'can.c',
- ],
- deps = [
- '//motors/core',
- '//motors:util',
- ],
- restricted_to = mcu_cpus,
+ name = "can",
+ srcs = [
+ "can.c",
+ ],
+ hdrs = [
+ "can.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//motors:util",
+ "//motors/core",
+ ],
+)
+
+cc_library(
+ name = "uart_buffer",
+ hdrs = ["uart_buffer.h"],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//third_party/GSL",
+ ],
+)
+
+cc_test(
+ name = "uart_buffer_test",
+ srcs = [
+ "uart_buffer_test.cc",
+ ],
+ deps = [
+ ":uart_buffer",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
+ name = "uart",
+ srcs = ["uart.cc"],
+ hdrs = ["uart.h"],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":uart_buffer",
+ "//motors:util",
+ "//motors/core",
+ "//third_party/GSL",
+ ],
+)
+
+cc_library(
+ name = "adc_dma",
+ srcs = ["adc_dma.cc"],
+ hdrs = ["adc_dma.h"],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ ":configuration",
+ "//motors:util",
+ "//motors/core",
+ ],
)
diff --git a/motors/peripheral/adc_dma.cc b/motors/peripheral/adc_dma.cc
new file mode 100644
index 0000000..e59a96f
--- /dev/null
+++ b/motors/peripheral/adc_dma.cc
@@ -0,0 +1,274 @@
+#include "motors/peripheral/adc_dma.h"
+
+#include <assert.h>
+
+// Design notes:
+// * Want to grab 3 differential-pair captures in rapid succession (aka 3 pairs
+// each containing 2 differential ADC input values)
+// * Use hardware triggering to allow triggering captures on both ADCs at the
+// exact same time
+// * Need to use both A and B channels within each ADC because writing to SC1n
+// while it's doing a capture (like to set up the next one) aborts the capture
+// * Can't use alternate (non-PDB) ADC triggers, because we want to use both A
+// and B channels within each ADC, and only the PDB triggers know how to use
+// two pre-triggers to choose which ADC channel to trigger
+// * Back-to-back connections aren't directly helpful because they're only
+// set up to go ADC0.A, ADC0.B, ADC1.A, ADC1.B, in a loop
+// * Setup:
+// * Set ADC0 and ADC1 for hardware triggering
+// * Write to ADC0_SC1A and ADC1_SC1A (with CPU) with initial values
+// * PDB:
+// * One-shot mode
+// * A output bypassed (so it goes right after the trigger)
+// * Triggered by FTM trigger outputs (twice per cycle)
+// * B output in back-to-back mode
+// * Delays don't matter
+// * A doesn't re-trigger off of B, so PDB+ADC by themselves stop after
+// doing two samples
+// * FTM triggers it at the appropriate points in the cycle (using two
+// otherwise-unused FTM channels)
+// * DMA:
+// * One result triggers off of ADC.COCO (either one)
+// * SOFF moves between RA and RB
+// * 1 minor loop = reading Rn from one ADC
+// * Trigger other result channel or first reconfigure channel after
+// (link on both major and minor completion)
+// * SMOD wraps back to RB after RA
+// * One major iteration is all four samples
+// * Can't use SOFF and SMOD to read all results with one channel because
+// SOFF is too small
+// * Same idea for two reconfigure channels
+// * Use DREQ so disabled after doing all four for CPU to read results
+// * Configure to reset everything after the major iteration so CPU just
+// has to re-enable
+// * Desired sequence:
+// 1. Trigger ADC0.A and ADC1.A
+// 2. [Reading ADC0.RB and ADC1.RB would be OK]
+// 3. Wait for ADC0.A and ADC1.A to finish
+// 4. Trigger ADC0.B and ADC0.B
+// 5. Read ADC0.RA and ADC1.RA
+// 6. Wait for ADC0.B and ADC1.B to finish
+// 7. Trigger ADC0.A and ADC0.A
+// 8. Read ADC0.RB and ADC1.RB
+// 9. Go back to step #3
+
+namespace frc971 {
+namespace teensy {
+namespace {
+
+constexpr uint32_t pdb_sc(int pdb_input) {
+ return V_PDB_LDMOD(0) /* Load immediately after LDOK */ |
+ V_PDB_PRESCALER(0) /* Doesn't matter */ | V_PDB_TRGSEL(pdb_input) |
+ M_PDB_PDBEN /* Enable it */ | V_PDB_MULT(0) /* Doesn't matter */ |
+ M_PDB_LDOK /* Load new values now */;
+}
+
+} // namespace
+
+AdcDmaSampler::AdcDmaSampler() {
+ for (int adc = 0; adc < 2; ++adc) {
+ for (int i = 0; i < 2; ++i) {
+ adc_sc1s_[adc][kNumberAdcSamples + i] = ADC_SC1_ADCH(0x1f);
+ }
+ }
+}
+
+void AdcDmaSampler::set_adc0_samples(
+ const ::std::array<uint32_t, kNumberAdcSamples> &adc0_samples) {
+ for (int i = 0; i < kNumberAdcSamples; ++i) {
+ adc_sc1s_[0][i] = adc0_samples[i] | ADC_SC1_AIEN;
+ }
+}
+
+void AdcDmaSampler::set_adc1_samples(
+ const ::std::array<uint32_t, kNumberAdcSamples> &adc1_samples) {
+ for (int i = 0; i < kNumberAdcSamples; ++i) {
+ adc_sc1s_[1][i] = adc1_samples[i] | ADC_SC1_AIEN;
+ }
+}
+
+void AdcDmaSampler::Initialize() {
+ // TODO(Brian): Put this somewhere better?
+ SIM_SCGC6 |= SIM_SCGC6_DMAMUX | SIM_SCGC6_PDB;
+
+ assert(ftm_delays_[0] != nullptr);
+ assert(ftm_delays_[1] != nullptr);
+ {
+ // See "Figure 33-92. Conversion time equation" for details on this math.
+ // All the math is in bus clock cycles, until being divided into FTM
+ // clock cycles at the end.
+
+ // Divider from bus clock to FTM clock.
+ // TODO(Brian): Make it so this can actually change.
+ const int ftm_clock_divider = 1;
+
+ // Divider from bus clock to ADC clock.
+ // TODO(Brian): Make sure this stays in sync with what adc.cc actually
+ // configures.
+ static constexpr int kAdcClockDivider = 4;
+
+ static constexpr int kSfcAdder = 5 * kAdcClockDivider + 5;
+
+ // 12-bit single-ended is only 20, but waiting a bit too long for some of
+ // the samples doesn't hurt anything.
+ static constexpr int kBct = 30 /* 13-bit differential */ * kAdcClockDivider;
+
+ static constexpr int kLstAdder = 0 * kAdcClockDivider;
+
+ static constexpr int kHscAdder = 2 * kAdcClockDivider;
+
+ static constexpr int kConversionTime =
+ kSfcAdder + 1 /* AverageNum */ * (kBct + kLstAdder + kHscAdder);
+
+ const int ftm_delay =
+ (kConversionTime * 2 /* 2 ADC samples */ + ftm_clock_divider - 1) /
+ ftm_clock_divider;
+ // TODO(Brian): Center on the start of the cycle instead. Need to think
+ // through how to get it started synced correctly though. Specifically,
+ // the second of the four samples should happen on the cycle boundary.
+ *ftm_delays_[0] = 0;
+ *ftm_delays_[1] = ftm_delay;
+ }
+
+ InitializePdbChannel(&PDB0.CH[0]);
+ InitializePdbChannel(&PDB0.CH[1]);
+ PDB0.MOD = 1 /* Doesn't matter */;
+ PDB0.SC = pdb_sc(pdb_input_);
+
+ DMAMUX0.CHCFG[result_dma_channel(0)] = M_DMAMUX_ENBL | DMAMUX_SOURCE_ADC0;
+ DMAMUX0.CHCFG[result_dma_channel(1)] = 0;
+ DMAMUX0.CHCFG[reconfigure_dma_channel(0)] = 0;
+ DMAMUX0.CHCFG[reconfigure_dma_channel(1)] = 0;
+
+ static constexpr ssize_t kResultABOffset =
+ static_cast<ssize_t>(offsetof(KINETIS_ADC_t, RB)) -
+ static_cast<ssize_t>(offsetof(KINETIS_ADC_t, RA));
+ static_assert(kResultABOffset > 0, "Offset is backwards");
+ static constexpr ssize_t kResultOffsetBits =
+ ConstexprLog2(kResultABOffset * 2);
+ static constexpr ssize_t kSC1ABOffset =
+ static_cast<ssize_t>(offsetof(KINETIS_ADC_t, SC1B)) -
+ static_cast<ssize_t>(offsetof(KINETIS_ADC_t, SC1A));
+ static_assert(kSC1ABOffset > 0, "Offset is backwards");
+ static constexpr ssize_t kSC1OffsetBits = ConstexprLog2(kSC1ABOffset * 2);
+ for (int adc = 0; adc < 2; ++adc) {
+ // Make sure we can actually write to all the fields in the DMA channels.
+ DMA.CDNE = result_dma_channel(adc);
+ DMA.CDNE = reconfigure_dma_channel(adc);
+ DMA.CERQ = result_dma_channel(adc);
+ DMA.CERQ = reconfigure_dma_channel(adc);
+
+ ADC(adc)
+ ->SC2 |= ADC_SC2_ADTRG /* Use hardware triggering */ |
+ ADC_SC2_DMAEN /* Enable DMA triggers */;
+
+ int next_result_channel, next_reconfigure_channel;
+ if (adc == 0) {
+ next_result_channel = result_dma_channel(1);
+ next_reconfigure_channel = reconfigure_dma_channel(1);
+ } else {
+ next_result_channel = reconfigure_dma_channel(0);
+#if 0
+ // TODO(Brian): Use this once we're actually doing encoder captures.
+ next_reconfigure_channel = encoder_value_dma_channel();
+#else
+ next_reconfigure_channel = -1;
+#endif
+ }
+ result_dma(adc)->SOFF = kResultABOffset;
+ reconfigure_dma(adc)->SOFF = sizeof(uint32_t);
+ result_dma(adc)->SADDR = &ADC(adc)->RA;
+ reconfigure_dma(adc)->SADDR = &adc_sc1s_[adc][2];
+ result_dma(adc)->ATTR =
+ V_TCD_SMOD(kResultOffsetBits) | V_TCD_SSIZE(TCD_SIZE_16BIT) |
+ V_TCD_DMOD(0) /* No DMOD */ | V_TCD_DSIZE(TCD_SIZE_16BIT);
+ reconfigure_dma(adc)->ATTR =
+ V_TCD_SMOD(0) /* No SMOD */ | V_TCD_SSIZE(TCD_SIZE_32BIT) |
+ V_TCD_DMOD(kSC1OffsetBits) | V_TCD_DSIZE(TCD_SIZE_32BIT);
+ result_dma(adc)->NBYTES = sizeof(uint16_t);
+ reconfigure_dma(adc)->NBYTES = sizeof(uint32_t);
+ result_dma(adc)->SLAST = 0;
+ reconfigure_dma(adc)->SLAST = -(kNumberAdcSamples * sizeof(uint32_t));
+ result_dma(adc)->DADDR = &adc_results_[adc][0];
+ reconfigure_dma(adc)->DADDR = &ADC(adc)->SC1A;
+ result_dma(adc)->DOFF = sizeof(uint16_t);
+ reconfigure_dma(adc)->DOFF = kSC1ABOffset;
+ {
+ uint16_t link = 0;
+ if (next_result_channel != -1) {
+ link = M_TCD_ELINK | V_TCD_LINKCH(next_result_channel);
+ }
+ result_dma(adc)->CITER = result_dma(adc)->BITER =
+ link | 4 /* 4 minor iterations */;
+ }
+ {
+ uint16_t link = 0;
+ if (next_reconfigure_channel != -1) {
+ link = M_TCD_ELINK | V_TCD_LINKCH(next_reconfigure_channel);
+ }
+ reconfigure_dma(adc)->CITER = reconfigure_dma(adc)->BITER =
+ link | 4 /* 4 minor iterations */;
+ }
+ result_dma(adc)->DLASTSGA = -(kNumberAdcSamples * sizeof(uint16_t));
+ reconfigure_dma(adc)->DLASTSGA = 0;
+ {
+ uint16_t link = 0;
+ if (next_result_channel != -1) {
+ link = V_TCD_MAJORLINKCH(next_result_channel) | M_TCD_MAJORELINK;
+ }
+ result_dma(adc)->CSR =
+ V_TCD_BWC(0) /* No extra stalls */ | link |
+ M_TCD_DREQ /* Disable requests after major iteration */;
+ }
+ {
+ uint16_t link = 0;
+ if (next_reconfigure_channel != -1) {
+ link = V_TCD_MAJORLINKCH(next_reconfigure_channel) | M_TCD_MAJORELINK;
+ }
+ reconfigure_dma(adc)->CSR =
+ V_TCD_BWC(0) /* No extra stalls */ | link |
+ M_TCD_DREQ /* Disable requests after major iteration */ |
+ M_TCD_INTMAJOR;
+ }
+ }
+}
+
+void AdcDmaSampler::Reset() {
+ // Disable the PDB. This both prevents weird interference with what we're
+ // trying to do and is part of the process to clear its internal
+ // (unobservable) "locks". If we get out of sync, then the DMA won't read from
+ // the ADC so the ADC's COCO will stay asserted and the PDB will stay "locked"
+ // on that channel. Disabling then re-enabling the PDB is the easiest way to
+ // clear that. See "37.4.1 PDB pre-trigger and trigger outputs" in the
+ // reference manual for details.
+ PDB0.SC = 0;
+
+ // Set the initial ADC sample configs.
+ for (int adc = 0; adc < 2; ++adc) {
+ // Tell the DMA it should go again.
+ DMA.CDNE = result_dma_channel(adc);
+ DMA.CDNE = reconfigure_dma_channel(adc);
+
+ for (int i = 0; i < 2; ++i) {
+ ADC(adc)->SC1[i] = adc_sc1s_[adc][i];
+ }
+ }
+
+ // Re-enable the first DMA channel (which is the only one triggered by
+ // hardware, and disables itself once it's done).
+ DMA.SERQ = result_dma_channel(0);
+
+ // Now re-enable the PDB.
+ PDB0.SC = pdb_sc(pdb_input_);
+ // Both channels' S registers are now 0, empirically, regardless of what they
+ // were before. Reference manual isn't super clear what's supposed to happen.
+}
+
+void AdcDmaSampler::InitializePdbChannel(KINETIS_PDB_CHANNEL_t *channel) {
+ channel->C1 = V_PDB_BB(2) /* Back-to-back enabled for channel 1 */ |
+ V_PDB_TOS(2) /* Bypass 0, and 1 doesn't matter (?) */ |
+ V_PDB_EN(3) /* Enable our two */;
+}
+
+} // namespace teensy
+} // namespace frc971
diff --git a/motors/peripheral/adc_dma.h b/motors/peripheral/adc_dma.h
new file mode 100644
index 0000000..8d98dde
--- /dev/null
+++ b/motors/peripheral/adc_dma.h
@@ -0,0 +1,112 @@
+#ifndef MOTORS_PERIPHERAL_ADC_DMA_H_
+#define MOTORS_PERIPHERAL_ADC_DMA_H_
+
+#include <array>
+
+#include <stdint.h>
+
+#include "motors/core/kinetis.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace teensy {
+
+// This class manages configuring the hardware to automatically capture various
+// sensor values periodically with tight timing. Currently it's only 4 samples
+// on each of ADC0 and ADC1.
+// TODO(Brian): Capture the encoder value (and velocity?) too.
+//
+// Uses:
+// * Both PDBs
+// * Both ADCs
+// * ADC_RESULT_DMA_CHANNEL and ADC_RECONFIGURE_DMA_CHANNEL
+class AdcDmaSampler {
+ public:
+ static constexpr int kNumberAdcSamples = 4;
+
+ // Corresponding samples in adc0_samples and adc1_samples happen
+ // simultaneously. Second sample happens at the cycle boundary. Elements
+ // should include the appropriate ADCH and DIFF values.
+ AdcDmaSampler();
+
+ // Must be called before Initialize().
+ void set_adc0_samples(
+ const ::std::array<uint32_t, kNumberAdcSamples> &adc0_samples);
+ void set_adc1_samples(
+ const ::std::array<uint32_t, kNumberAdcSamples> &adc1_samples);
+ // pdb_input is the trigger-in index for PDB0 to use the attached FTM. This
+ // FTM must be set to do trigger-outs on exactly two of its channels, which
+ // will be dedicated to this object. This FTM must also start counting at 0 at
+ // the beginning of each cycle and have MOD set to match one cycle time.
+ void set_pdb_input(int pdb_input) { pdb_input_ = pdb_input; }
+ // ftm_delays is pointers to the two FTM.SCn registers which are set up to do
+ // trigger-outs.
+ void set_ftm_delays(const ::std::array<volatile uint32_t *, 2> &ftm_delays) {
+ ftm_delays_ = ftm_delays;
+ }
+
+ AdcDmaSampler(const AdcDmaSampler &) = delete;
+ AdcDmaSampler &operator=(const AdcDmaSampler &) = delete;
+
+ void Initialize();
+
+ // Checks if the current cycle has finished reading out results.
+ // After this returns true, it is safe to read the results until Reset() is
+ // called.
+ bool CheckDone() {
+ const uint32_t mask = 1 << reconfigure_dma_channel(1);
+ if (!(DMA.INT & mask)) {
+ return false;
+ }
+ DmaMemoryBarrier();
+ DMA.INT = mask;
+ (void)DMA.INT;
+ return true;
+ }
+
+ // Must be called after CheckDone() returns true each time.
+ void Reset();
+
+ int16_t adc_result(int adc, int i) { return adc_results_[adc][i]; }
+
+ private:
+ void InitializePdbChannel(KINETIS_PDB_CHANNEL_t *channel);
+
+ static constexpr int result_dma_channel(int adc) {
+ if (adc == 0) {
+ return ADC_RESULT_DMA_CHANNEL0;
+ }
+ return ADC_RESULT_DMA_CHANNEL1;
+ }
+ static KINETIS_TCD_t *result_dma(int adc) {
+ return &DMA.TCD[result_dma_channel(adc)];
+ }
+
+ static constexpr int reconfigure_dma_channel(int adc) {
+ if (adc == 0) {
+ return ADC_RECONFIGURE_DMA_CHANNEL0;
+ }
+ return ADC_RECONFIGURE_DMA_CHANNEL1;
+ }
+ static KINETIS_TCD_t *reconfigure_dma(int adc) {
+ return &DMA.TCD[reconfigure_dma_channel(adc)];
+ }
+
+ static constexpr int encoder_value_dma_channel() {
+ return ENCODER_VALUE_DMA_CHANNEL;
+ }
+
+ ::std::array<::std::array<volatile uint32_t, kNumberAdcSamples + 2>, 2>
+ adc_sc1s_{};
+ ::std::array<::std::array<volatile uint16_t, kNumberAdcSamples>, 2>
+ adc_results_{};
+
+ int pdb_input_ = 0xF;
+ ::std::array<volatile uint32_t *, 2> ftm_delays_{nullptr, nullptr};
+};
+
+} // namespace teensy
+} // namespace frc971
+
+#endif // MOTORS_PERIPHERAL_ADC_DMA_H_
diff --git a/motors/peripheral/configuration.h b/motors/peripheral/configuration.h
index 3fa686a..3e21f17 100644
--- a/motors/peripheral/configuration.h
+++ b/motors/peripheral/configuration.h
@@ -12,13 +12,13 @@
// The frequency we switch the motor FETs at in hz.
#define SWITCHING_FREQUENCY 20000
-#if 0
-// Which PDB the ADC triggering uses.
-#define ADC_TRIGGER_PDB 0
-// The DMA channel which copies ADC results.
-#define ADC_RESULT_DMA_CHANNEL 7
-// The DMA channel which reconfigures the ADCs to take the next samples.
-#define ADC_RECONFIGURE_DMA_CHANNEL 8
-#endif
+// The DMA channels which copy ADC results.
+#define ADC_RESULT_DMA_CHANNEL0 7
+#define ADC_RESULT_DMA_CHANNEL1 8
+// The DMA channels which reconfigure the ADCs to take the next samples.
+#define ADC_RECONFIGURE_DMA_CHANNEL0 9
+#define ADC_RECONFIGURE_DMA_CHANNEL1 10
+// The DMA channel which reads encoder values.
+#define ENCODER_VALUE_DMA_CHANNEL 11
#endif // MOTORS_PERIPHERAL_CONFIGURATION_H_
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
new file mode 100644
index 0000000..f910651
--- /dev/null
+++ b/motors/peripheral/uart.cc
@@ -0,0 +1,97 @@
+#include "motors/peripheral/uart.h"
+
+#include <stdint.h>
+
+namespace frc971 {
+namespace teensy {
+
+// Currently hard-coded for 8-bit + odd parity + start bit + stop bit.
+void Uart::Initialize(int baud_rate) {
+ {
+ // UART baud rate = UART module clock / (16 * (SBR[12:0] + BRFD))
+ // BRFD = BRFA (bitfield) / 32
+ const int desired_receiver_clock = baud_rate * 16;
+ const int sbr_and_brfd32 =
+ ((static_cast<int64_t>(module_clock_frequency_) * UINT64_C(64) /
+ static_cast<int64_t>(desired_receiver_clock)) +
+ 1) /
+ 2;
+ const int sbr = sbr_and_brfd32 / 32;
+ const int brfa = sbr_and_brfd32 % 32;
+
+ module_->BDH = (sbr >> 8) & 0x1F;
+ module_->BDL = sbr & 0xFF;
+ module_->C1 = M_UART_M /* 9 data bits */ |
+ M_UART_ILT /* only detect idle after stop bit */ |
+ M_UART_PE /* enable parity */ | M_UART_PT /* odd parity */;
+ module_->C4 = V_UART_BRFA(brfa);
+ }
+ {
+ const uint8_t pfifo = module_->PFIFO;
+ tx_fifo_size_ = G_UART_TXFIFOSIZE(pfifo);
+ rx_fifo_size_ = G_UART_RXFIFOSIZE(pfifo);
+ }
+
+ // When C1[M] is set and C4[M10] is cleared, the UART is configured for 9-bit
+ // data characters. If C1[PE] is enabled, the ninth bit is either C3[T8/R8] or
+ // the internally generated parity bit
+
+ // TODO(Brian): M_UART_TIE /* Enable TX interrupt or DMA */ |
+ // M_UART_RIE /* Enable RX interrupt or DMA */
+ // Also set in C5: M_UART_TDMAS /* Do DMA for TX */ |
+ // M_UART_RDMAS /* Do DMA for RX */
+ c2_value_ = M_UART_TE;
+ module_->C2 = c2_value_;
+ module_->PFIFO =
+ M_UART_TXFE /* Enable TX FIFO */ | M_UART_RXFE /* Enable RX FIFO */;
+ module_->CFIFO =
+ M_UART_TXFLUSH /* Flush TX FIFO */ | M_UART_RXFLUSH /* Flush RX FIFO */;
+ // TODO(Brian): Adjust for DMA?
+ module_->TWFIFO = tx_fifo_size_ - 1;
+ module_->RWFIFO = rx_fifo_size_ - 1;
+}
+
+void Uart::DoWrite(gsl::span<char> data) {
+ for (int i = 0; i < data.size(); ++i) {
+ while (!SpaceAvailable()) {
+ }
+ WriteCharacter(data[i]);
+ }
+}
+
+Uart::~Uart() { DisableTransmitInterrupt(); }
+
+void InterruptBufferedUart::Initialize(int baud_rate) {
+ uart_.Initialize(baud_rate);
+}
+
+void InterruptBufferedUart::Write(gsl::span<char> data,
+ const DisableInterrupts &disable_interrupts) {
+ uart_.EnableTransmitInterrupt();
+ static_assert(buffer_.size() >= 8,
+ "Need enough space to not turn the interrupt off each time");
+ while (!data.empty()) {
+ const int bytes_written = buffer_.PushSpan(data);
+ data = data.subspan(bytes_written);
+ WriteCharacters(data.empty(), disable_interrupts);
+ }
+}
+
+void InterruptBufferedUart::WriteCharacters(bool disable_empty,
+ const DisableInterrupts &) {
+ while (true) {
+ if (buffer_.empty()) {
+ if (disable_empty) {
+ uart_.DisableTransmitInterrupt();
+ }
+ return;
+ }
+ if (!uart_.SpaceAvailable()) {
+ return;
+ }
+ uart_.WriteCharacter(buffer_.PopSingle());
+ }
+}
+
+} // namespace teensy
+} // namespace frc971
diff --git a/motors/peripheral/uart.h b/motors/peripheral/uart.h
new file mode 100644
index 0000000..cf79815
--- /dev/null
+++ b/motors/peripheral/uart.h
@@ -0,0 +1,78 @@
+#ifndef MOTORS_PERIPHERAL_UART_H_
+#define MOTORS_PERIPHERAL_UART_H_
+
+#include "motors/core/kinetis.h"
+#include "motors/peripheral/uart_buffer.h"
+#include "motors/util.h"
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace frc971 {
+namespace teensy {
+
+// Simple synchronous interface to a UART.
+class Uart {
+ public:
+ Uart(KINETISK_UART_t *module, int module_clock_frequency)
+ : module_(module), module_clock_frequency_(module_clock_frequency) {}
+ Uart(const Uart &) = delete;
+ ~Uart();
+ Uart &operator=(const Uart &) = delete;
+
+ void Initialize(int baud_rate);
+
+ // Blocks until all of the data is at least queued.
+ void Write(gsl::span<char> data, const DisableInterrupts &) { DoWrite(data); }
+
+ bool SpaceAvailable() const { return module_->S1 & M_UART_TDRE; }
+ // Only call this if SpaceAvailable() has just returned true.
+ void WriteCharacter(char c) { module_->D = c; }
+
+ void EnableTransmitInterrupt() {
+ c2_value_ |= M_UART_TIE;
+ module_->C2 = c2_value_;
+ }
+
+ void DisableTransmitInterrupt() {
+ c2_value_ &= ~M_UART_TIE;
+ module_->C2 = c2_value_;
+ }
+
+ private:
+ void DoWrite(gsl::span<char> data);
+
+ KINETISK_UART_t *const module_;
+ const int module_clock_frequency_;
+ // What we put in C2 except TE.
+ uint8_t c2_value_;
+
+ int tx_fifo_size_, rx_fifo_size_;
+};
+
+// Interrupt-based buffered interface to a UART.
+class InterruptBufferedUart {
+ public:
+ InterruptBufferedUart(KINETISK_UART_t *module, int module_clock_frequency)
+ : uart_(module, module_clock_frequency) {}
+
+ void Initialize(int baud_rate);
+
+ void Write(gsl::span<char> data, const DisableInterrupts &disable_interrupts);
+
+ // Should be called as the body of the interrupt handler.
+ void HandleInterrupt(const DisableInterrupts &disable_interrupts) {
+ WriteCharacters(true, disable_interrupts);
+ }
+
+ private:
+ void WriteCharacters(bool disable_empty, const DisableInterrupts &);
+
+ Uart uart_;
+ UartBuffer<1024> buffer_;
+
+ bool interrupt_enabled_ = false;
+};
+
+} // namespace teensy
+} // namespace frc971
+
+#endif // MOTORS_PERIPHERAL_UART_H_
diff --git a/motors/peripheral/uart_buffer.h b/motors/peripheral/uart_buffer.h
new file mode 100644
index 0000000..fb7d126
--- /dev/null
+++ b/motors/peripheral/uart_buffer.h
@@ -0,0 +1,66 @@
+#ifndef MOTORS_PERIPHERAL_UART_BUFFER_H_
+#define MOTORS_PERIPHERAL_UART_BUFFER_H_
+
+#include <array>
+
+#include "third_party/GSL/include/gsl/gsl"
+
+namespace frc971 {
+namespace teensy {
+
+// Manages a circular buffer of data to send out.
+template<int kSize>
+class UartBuffer {
+ public:
+ // Returns the number of characters added.
+ __attribute__((warn_unused_result)) int PushSpan(gsl::span<char> data);
+
+ bool empty() const { return size_ == 0; }
+
+ // This may only be called when !empty().
+ char PopSingle();
+
+ static constexpr int size() { return kSize; }
+
+ private:
+ // The index at which we will push the next character.
+ int start_ = 0;
+ // How many characters we currently have.
+ int size_ = 0;
+
+ ::std::array<char, kSize> data_;
+};
+
+template<int kSize>
+int UartBuffer<kSize>::PushSpan(gsl::span<char> data) {
+ const int end_location = (start_ + size_) % kSize;
+ const int remaining_end = ::std::min(kSize - size_, kSize - end_location);
+ const int on_end = ::std::min<int>(data.size(), remaining_end);
+ if (on_end > 0) {
+ memcpy(&data_[end_location], data.data(), on_end);
+ }
+ size_ += on_end;
+ const int not_on_end = data.size() - on_end;
+ if (not_on_end == 0) {
+ return data.size();
+ }
+
+ const int remaining_start = ::std::min(kSize - size_, start_);
+ const int on_start = ::std::min(not_on_end, remaining_start);
+ memcpy(data_.data(), &data[on_end], on_start);
+ size_ += on_start;
+ return on_end + on_start;
+}
+
+template<int kSize>
+char UartBuffer<kSize>::PopSingle() {
+ const char r = data_[start_];
+ --size_;
+ start_ = (start_ + 1) % kSize;
+ return r;
+}
+
+} // namespace teensy
+} // namespace frc971
+
+#endif // MOTORS_PERIPHERAL_UART_BUFFER_H_
diff --git a/motors/peripheral/uart_buffer_test.cc b/motors/peripheral/uart_buffer_test.cc
new file mode 100644
index 0000000..5ab7c98
--- /dev/null
+++ b/motors/peripheral/uart_buffer_test.cc
@@ -0,0 +1,186 @@
+#include "motors/peripheral/uart_buffer.h"
+
+#include "gtest/gtest.h"
+
+namespace frc971 {
+namespace teensy {
+namespace testing {
+
+// Tests that using PushSpan with single characters works correctly.
+TEST(UartBufferTest, PushSpanSingle) {
+ UartBuffer<1024> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 1> data{{1}};
+ ASSERT_EQ(1, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(1, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 1> data{{2}};
+ ASSERT_EQ(1, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(2, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan with multiple characters works correctly.
+TEST(UartBufferTest, PushSpanMultiple) {
+ UartBuffer<1024> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 4> data{{1, 2, 4, 8}};
+ ASSERT_EQ(4, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(1, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(2, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(4, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(8, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan works correctly when wrapping with a
+// multiple-character push.
+TEST(UartBufferTest, PushSpanWrapMultiple) {
+ UartBuffer<4> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 2> data{{10, 20}};
+ ASSERT_EQ(2, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(10, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(20, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 4> data{{1, 2, 4, 8}};
+ ASSERT_EQ(4, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(1, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(2, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(4, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(8, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan works correctly when wrapping with a
+// single-character push.
+TEST(UartBufferTest, PushSpanWrapSingle) {
+ UartBuffer<3> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 3> data{{10, 20, 30}};
+ ASSERT_EQ(3, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(10, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+
+ {
+ ::std::array<char, 1> data{{1}};
+ ASSERT_EQ(1, buffer.PushSpan(data));
+ }
+
+ ASSERT_EQ(20, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(30, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(1, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan works correctly when overflowing with a
+// multiple-character push, where none of it fits.
+TEST(UartBufferTest, PushSpanOverflowAllMultiple) {
+ UartBuffer<2> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 2> data{{10, 20}};
+ ASSERT_EQ(2, buffer.PushSpan(data));
+ }
+
+ {
+ ::std::array<char, 4> data{{1, 2, 4, 8}};
+ ASSERT_EQ(0, buffer.PushSpan(data));
+ }
+
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(10, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(20, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan works correctly when overflowing with a
+// multiple-character push, where some of it fits.
+TEST(UartBufferTest, PushSpanOverflowSomeMultiple) {
+ UartBuffer<4> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 2> data{{10, 20}};
+ ASSERT_EQ(2, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+
+ {
+ ::std::array<char, 4> data{{1, 2, 4, 8}};
+ ASSERT_EQ(2, buffer.PushSpan(data));
+ }
+
+ ASSERT_EQ(10, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(20, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(1, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(2, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+// Tests that using PushSpan works correctly when overflowing with a
+// single-character push.
+TEST(UartBufferTest, PushSpanOverflowSingle) {
+ UartBuffer<3> buffer;
+ ASSERT_TRUE(buffer.empty());
+
+ {
+ ::std::array<char, 3> data{{10, 20, 30}};
+ ASSERT_EQ(3, buffer.PushSpan(data));
+ }
+ ASSERT_FALSE(buffer.empty());
+
+ {
+ ::std::array<char, 1> data{{1}};
+ ASSERT_EQ(0, buffer.PushSpan(data));
+ }
+
+ ASSERT_EQ(10, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(20, buffer.PopSingle());
+ ASSERT_FALSE(buffer.empty());
+ ASSERT_EQ(30, buffer.PopSingle());
+ ASSERT_TRUE(buffer.empty());
+}
+
+} // namespace testing
+} // namespace teensy
+} // namespace frc971