Merge "Factor out jpeg list image dataset loading for use in calibration."
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index c139195..3e24fab 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -1,5 +1,7 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "ring_buffer",
hdrs = [
@@ -41,6 +43,7 @@
hdrs = [
"sized_array.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/time/BUILD b/aos/time/BUILD
index b76cd99..6570490 100644
--- a/aos/time/BUILD
+++ b/aos/time/BUILD
@@ -1,4 +1,4 @@
-package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
cc_library(
name = "time",
@@ -8,11 +8,31 @@
hdrs = [
"time.h",
],
+ visibility = ["//visibility:public"],
deps = [
"//aos:macros",
- "//aos/mutex:mutex",
- "//aos/logging",
"//aos/ipc_lib:shared_mem",
+ "//aos/logging",
+ "//aos/mutex",
+ "//aos/type_traits",
+ ],
+)
+
+# TODO(Brian): Remove this hack once bazel chases deps through selects correctly.
+cc_library(
+ name = "time_mcu",
+ srcs = [
+ "time.cc",
+ ],
+ hdrs = [
+ "time.h",
+ ],
+ restricted_to = mcu_cpus,
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos:macros",
+ "//aos/type_traits",
+ "//motors/core",
],
)
@@ -24,7 +44,7 @@
deps = [
":time",
"//aos/logging",
- "//aos/util:death_test_log_implementation",
"//aos/testing:googletest",
+ "//aos/util:death_test_log_implementation",
],
)
diff --git a/aos/time/time.cc b/aos/time/time.cc
index 0d7295c..06f4b12 100644
--- a/aos/time/time.cc
+++ b/aos/time/time.cc
@@ -6,6 +6,8 @@
#include <atomic>
#include <chrono>
+#ifdef __linux__
+
// We only use global_core from here, which is weak, so we don't really have a
// dependency on it.
#include "aos/ipc_lib/shared_mem.h"
@@ -13,8 +15,19 @@
#include "aos/logging/logging.h"
#include "aos/mutex/mutex.h"
+#else // __linux__
+
+#include "motors/core/kinetis.h"
+
+// The systick interrupt increments this every 1ms.
+extern "C" volatile uint32_t systick_millis_count;
+
+#endif // __linux__
+
namespace chrono = ::std::chrono;
+#ifdef __linux__
+
namespace std {
namespace this_thread {
template <>
@@ -42,10 +55,13 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
namespace aos {
namespace time {
+#ifdef __linux__
+
// State required to enable and use mock time.
namespace {
// True if mock time is enabled.
@@ -98,6 +114,8 @@
chrono::duration_cast<chrono::nanoseconds>(offset).count();
}
+#endif // __linux__
+
struct timespec to_timespec(
const ::aos::monotonic_clock::duration duration) {
struct timespec time_timespec;
@@ -119,11 +137,11 @@
constexpr monotonic_clock::time_point monotonic_clock::min_time;
monotonic_clock::time_point monotonic_clock::now() noexcept {
- {
- if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
- MutexLocker time_mutex_locker(&time::time_mutex);
- return time::current_mock_time;
- }
+#ifdef __linux__
+
+ if (time::mock_time_enabled.load(::std::memory_order_relaxed)) {
+ MutexLocker time_mutex_locker(&time::time_mutex);
+ return time::current_mock_time;
}
struct timespec current_time;
@@ -139,7 +157,45 @@
return time_point(::std::chrono::seconds(current_time.tv_sec) +
::std::chrono::nanoseconds(current_time.tv_nsec)) + offset;
+
+#else // __linux__
+
+ __disable_irq();
+ const uint32_t current_counter = SYST_CVR;
+ uint32_t ms_count = systick_millis_count;
+ const uint32_t istatus = SCB_ICSR;
+ __enable_irq();
+ // If the interrupt is pending and the timer has already wrapped from 0 back
+ // up to its max, then add another ms.
+ if ((istatus & SCB_ICSR_PENDSTSET) && current_counter > 50) {
+ ++ms_count;
+ }
+
+ // It counts down, but everything we care about counts up.
+ const uint32_t counter_up = ((F_CPU / 1000) - 1) - current_counter;
+
+ // "3.2.1.2 System Tick Timer" in the TRM says "The System Tick Timer's clock
+ // source is always the core clock, FCLK".
+ using systick_duration =
+ std::chrono::duration<uint32_t, std::ratio<1, F_CPU>>;
+
+ return time_point(aos::time::round<std::chrono::nanoseconds>(
+ std::chrono::milliseconds(ms_count) + systick_duration(counter_up)));
+
+#endif // __linux__
}
+#ifdef __linux__
+realtime_clock::time_point realtime_clock::now() noexcept {
+ struct timespec current_time;
+ if (clock_gettime(CLOCK_REALTIME, ¤t_time) != 0) {
+ PLOG(FATAL, "clock_gettime(%jd, %p) failed",
+ static_cast<uintmax_t>(CLOCK_REALTIME), ¤t_time);
+ }
+
+ return time_point(::std::chrono::seconds(current_time.tv_sec) +
+ ::std::chrono::nanoseconds(current_time.tv_nsec));
+}
+#endif // __linux__
} // namespace aos
diff --git a/aos/time/time.h b/aos/time/time.h
index 9bef741..136629c 100644
--- a/aos/time/time.h
+++ b/aos/time/time.h
@@ -24,7 +24,32 @@
typedef ::std::chrono::time_point<monotonic_clock> time_point;
static monotonic_clock::time_point now() noexcept;
- static constexpr bool is_steady = true;
+ // This clock is still subject to rate adjustments based on adjtime, so it is
+ // not steady.
+ static constexpr bool is_steady = false;
+
+ // Returns the epoch (0).
+ static constexpr monotonic_clock::time_point epoch() {
+ return time_point(zero());
+ }
+
+ static constexpr monotonic_clock::duration zero() { return duration(0); }
+
+ static constexpr time_point min_time{
+ time_point(duration(::std::numeric_limits<duration::rep>::min()))};
+};
+
+class realtime_clock {
+ public:
+ typedef ::std::chrono::nanoseconds::rep rep;
+ typedef ::std::chrono::nanoseconds::period period;
+ typedef ::std::chrono::nanoseconds duration;
+ typedef ::std::chrono::time_point<monotonic_clock> time_point;
+
+#ifdef __linux__
+ static monotonic_clock::time_point now() noexcept;
+#endif // __linux__
+ static constexpr bool is_steady = false;
// Returns the epoch (0).
static constexpr monotonic_clock::time_point epoch() {
@@ -39,6 +64,8 @@
namespace time {
+#ifdef __linux__
+
// Enables returning the mock time value for Now instead of checking the system
// clock.
void EnableMockTime(monotonic_clock::time_point now);
@@ -78,6 +105,8 @@
DISALLOW_COPY_AND_ASSIGN(TimeFreezer);
};
+#endif // __linux__
+
// Converts a monotonic_clock::duration into a timespec object.
struct timespec to_timespec(::aos::monotonic_clock::duration duration);
@@ -85,9 +114,72 @@
// epoch.
struct timespec to_timespec(::aos::monotonic_clock::time_point time);
+namespace time_internal {
+
+template <class T>
+struct is_duration : std::false_type {};
+template <class Rep, class Period>
+struct is_duration<std::chrono::duration<Rep, Period>> : std::true_type {};
+
+} // namespace time_internal
+
+// Returns the greatest duration t representable in ToDuration that is less or
+// equal to d.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/floor.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<time_internal::is_duration<To>{}>>
+constexpr To floor(const std::chrono::duration<Rep, Period> &d) {
+ To t = std::chrono::duration_cast<To>(d);
+ if (t > d) return t - To{1};
+ return t;
+}
+
+// Returns the value t representable in ToDuration that is the closest to d. If
+// there are two such values, returns the even value (that is, the value t such
+// that t % 2 == 0).
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/duration/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Rep, class Period,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr To round(const std::chrono::duration<Rep, Period> &d) {
+ To t0 = aos::time::floor<To>(d);
+ To t1 = t0 + To{1};
+ auto diff0 = d - t0;
+ auto diff1 = t1 - d;
+ if (diff0 == diff1) {
+ if (t0.count() & 1) return t1;
+ return t0;
+ } else if (diff0 < diff1) {
+ return t0;
+ }
+ return t1;
+}
+
+// Returns the nearest time point to tp representable in ToDuration, rounding to
+// even in halfway cases, like std::chrono::round in C++17.
+// Implementation copied from
+// https://en.cppreference.com/w/cpp/chrono/time_point/round.
+// TODO(Brian): Remove once we have C++17 support.
+template <class To, class Clock, class FromDuration,
+ class = std::enable_if_t<
+ time_internal::is_duration<To>{} &&
+ !std::chrono::treat_as_floating_point<typename To::rep>{}>>
+constexpr std::chrono::time_point<Clock, To> round(
+ const std::chrono::time_point<Clock, FromDuration> &tp) {
+ return std::chrono::time_point<Clock, To>{
+ aos::time::round<To>(tp.time_since_epoch())};
+}
+
} // namespace time
} // namespace aos
+#ifdef __linux__
+
namespace std {
namespace this_thread {
// Template specialization for monotonic_clock, since we can use clock_nanosleep
@@ -98,5 +190,6 @@
} // namespace this_thread
} // namespace std
+#endif // __linux__
#endif // AOS_TIME_H_
diff --git a/aos/type_traits/BUILD b/aos/type_traits/BUILD
index 171bb8d..e5590b0 100644
--- a/aos/type_traits/BUILD
+++ b/aos/type_traits/BUILD
@@ -1,10 +1,13 @@
package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+
cc_library(
name = "type_traits",
hdrs = [
"type_traits.h",
],
+ compatible_with = mcu_cpus,
)
cc_test(
diff --git a/aos/type_traits/type_traits.h b/aos/type_traits/type_traits.h
index f0a2e72..437cb3e 100644
--- a/aos/type_traits/type_traits.h
+++ b/aos/type_traits/type_traits.h
@@ -1,8 +1,6 @@
#ifndef AOS_TYPE_TRAITS_
#define AOS_TYPE_TRAITS_
-#include <features.h>
-
#include <type_traits>
namespace aos {
diff --git a/motors/core/time.h b/motors/core/time.h
index 247de2c..d03cb10 100644
--- a/motors/core/time.h
+++ b/motors/core/time.h
@@ -3,6 +3,8 @@
#include <stdint.h>
+// This whole file is deprecated. Use //aos/time instead.
+
#ifdef __cplusplus
extern "C"
{
diff --git a/motors/peripheral/BUILD b/motors/peripheral/BUILD
index ef2e833..2c71973 100644
--- a/motors/peripheral/BUILD
+++ b/motors/peripheral/BUILD
@@ -71,6 +71,7 @@
visibility = ["//visibility:public"],
deps = [
":uart_buffer",
+ "//aos/containers:sized_array",
"//motors:util",
"//motors/core",
"//third_party/GSL",
diff --git a/motors/peripheral/uart.cc b/motors/peripheral/uart.cc
index 3f7ddaf..9cd4014 100644
--- a/motors/peripheral/uart.cc
+++ b/motors/peripheral/uart.cc
@@ -39,18 +39,24 @@
// 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;
+ c2_value_ = 0;
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 */;
+ c2_value_ = M_UART_TE | M_UART_RE;
+ module_->C2 = c2_value_;
// TODO(Brian): Adjust for DMA?
module_->TWFIFO = tx_fifo_size_ - 1;
- module_->RWFIFO = rx_fifo_size_ - 1;
+ module_->RWFIFO = 1;
}
void Uart::DoWrite(gsl::span<const char> data) {
+ // In theory, we could be more efficient about this by writing the number of
+ // bytes we know there's space for and only calling SpaceAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
for (int i = 0; i < data.size(); ++i) {
while (!SpaceAvailable()) {
}
@@ -58,38 +64,91 @@
}
}
-Uart::~Uart() { DisableTransmitInterrupt(); }
+aos::SizedArray<char, 4> Uart::DoRead() {
+ // In theory, we could be more efficient about this by reading the number of
+ // bytes we know to be accessible and only calling DataAvailable() (or
+ // otherwise reading S1) before the final one. In practice, the FIFOs are so
+ // short on this part it probably won't help anything.
+ aos::SizedArray<char, 4> result;
+ while (DataAvailable() && !result.full()) {
+ result.push_back(ReadCharacter());
+ }
+ return result;
+}
+
+Uart::~Uart() {
+ DoDisableTransmitInterrupt();
+ DoDisableReceiveInterrupt();
+}
+
+InterruptBufferedUart::~InterruptBufferedUart() {
+ uart_.DisableReceiveInterrupt(DisableInterrupts());
+}
void InterruptBufferedUart::Initialize(int baud_rate) {
uart_.Initialize(baud_rate);
+ {
+ DisableInterrupts disable_interrupts;
+ uart_.EnableReceiveInterrupt(disable_interrupts);
+ }
}
void InterruptBufferedUart::Write(gsl::span<const char> data) {
DisableInterrupts disable_interrupts;
- uart_.EnableTransmitInterrupt();
- static_assert(buffer_.size() >= 8,
- "Need enough space to not turn the interrupt off each time");
+ uart_.EnableTransmitInterrupt(disable_interrupts);
while (!data.empty()) {
- const int bytes_written = buffer_.PushSpan(data);
+ const int bytes_written = transmit_buffer_.PushSpan(data);
data = data.subspan(bytes_written);
WriteCharacters(data.empty(), disable_interrupts);
ReenableInterrupts{&disable_interrupts};
}
}
-void InterruptBufferedUart::WriteCharacters(bool disable_empty,
- const DisableInterrupts &) {
+gsl::span<char> InterruptBufferedUart::Read(gsl::span<char> buffer) {
+ size_t bytes_read = 0;
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size());
+ std::copy(read_data.begin(), read_data.end(), buffer.begin());
+ bytes_read += read_data.size();
+ }
+ {
+ DisableInterrupts disable_interrupts;
+ const gsl::span<const char> read_data =
+ receive_buffer_.PopSpan(buffer.size() - bytes_read);
+ std::copy(read_data.begin(), read_data.end(),
+ buffer.subspan(bytes_read).begin());
+ bytes_read += read_data.size();
+ }
+ return buffer.subspan(0, bytes_read);
+}
+
+void InterruptBufferedUart::WriteCharacters(
+ bool disable_empty, const DisableInterrupts &disable_interrupts) {
while (true) {
- if (buffer_.empty()) {
+ if (transmit_buffer_.empty()) {
if (disable_empty) {
- uart_.DisableTransmitInterrupt();
+ uart_.DisableTransmitInterrupt(disable_interrupts);
}
return;
}
if (!uart_.SpaceAvailable()) {
return;
}
- uart_.WriteCharacter(buffer_.PopSingle());
+ uart_.WriteCharacter(transmit_buffer_.PopSingle());
+ }
+}
+
+void InterruptBufferedUart::ReadCharacters(const DisableInterrupts &) {
+ while (true) {
+ if (receive_buffer_.full()) {
+ return;
+ }
+ if (!uart_.DataAvailable()) {
+ return;
+ }
+ receive_buffer_.PushSingle(uart_.ReadCharacter());
}
}
diff --git a/motors/peripheral/uart.h b/motors/peripheral/uart.h
index ca40ab7..ffb1529 100644
--- a/motors/peripheral/uart.h
+++ b/motors/peripheral/uart.h
@@ -1,6 +1,7 @@
#ifndef MOTORS_PERIPHERAL_UART_H_
#define MOTORS_PERIPHERAL_UART_H_
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "motors/peripheral/uart_buffer.h"
#include "motors/util.h"
@@ -25,22 +26,59 @@
DoWrite(data);
}
+ // Returns all the data which is currently available.
+ aos::SizedArray<char, 4> Read(const DisableInterrupts &) {
+ return DoRead();
+ }
+
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() {
+ bool DataAvailable() const { return module_->S1 & M_UART_RDRF; }
+ // Only call this if DataAvailable() has just returned true.
+ char ReadCharacter() { return module_->D; }
+
+ // TODO(Brian): These APIs for enabling/disabling interrupts aren't quite
+ // right. Redo them some time. Some issues:
+ // * They get called during initialization/destruction time, which means
+ // interrupts don't really need to be disabled because everything is
+ // singlethreaded.
+ // * Often, several C2 modifications are made in a single
+ // interrupts-disabled section. These could be batched to reduce
+ // peripheral writes. Sometimes, no modifications are made at all, in
+ // which case there doesn't even need to be a single write.
+
+ void EnableTransmitInterrupt(const DisableInterrupts &) {
c2_value_ |= M_UART_TIE;
module_->C2 = c2_value_;
}
- void DisableTransmitInterrupt() {
- c2_value_ &= ~M_UART_TIE;
+ void DisableTransmitInterrupt(const DisableInterrupts &) {
+ DoDisableTransmitInterrupt();
+ }
+
+ void EnableReceiveInterrupt(const DisableInterrupts &) {
+ c2_value_ |= M_UART_RIE;
module_->C2 = c2_value_;
}
+ void DisableReceiveInterrupt(const DisableInterrupts &) {
+ DoDisableReceiveInterrupt();
+ }
+
private:
+ void DoDisableTransmitInterrupt() {
+ c2_value_ &= ~M_UART_TIE;
+ module_->C2 = c2_value_;
+ }
+ void DoDisableReceiveInterrupt() {
+ c2_value_ &= ~M_UART_RIE;
+ module_->C2 = c2_value_;
+ }
+
void DoWrite(gsl::span<const char> data);
+ aos::SizedArray<char, 4> DoRead();
KINETISK_UART_t *const module_;
const int module_clock_frequency_;
@@ -51,27 +89,37 @@
};
// Interrupt-based buffered interface to a UART.
+// TODO(Brian): Move DisableInterrupts calls up to the caller of this.
class InterruptBufferedUart {
public:
InterruptBufferedUart(KINETISK_UART_t *module, int module_clock_frequency)
: uart_(module, module_clock_frequency) {}
+ ~InterruptBufferedUart();
void Initialize(int baud_rate);
+ // Queues up the given data for immediate writing. Blocks only if the queue
+ // fills up before all of data is enqueued.
void Write(gsl::span<const char> data);
+ // Reads currently available data.
+ // Returns all the data which is currently available (possibly none);
+ // buffer is where to store the result. The return value will be a subspan of
+ // this.
+ gsl::span<char> Read(gsl::span<char> buffer);
+
// Should be called as the body of the interrupt handler.
void HandleInterrupt(const DisableInterrupts &disable_interrupts) {
WriteCharacters(true, disable_interrupts);
+ ReadCharacters(disable_interrupts);
}
private:
void WriteCharacters(bool disable_empty, const DisableInterrupts &);
+ void ReadCharacters(const DisableInterrupts &);
Uart uart_;
- UartBuffer<1024> buffer_;
-
- bool interrupt_enabled_ = false;
+ UartBuffer<1024> transmit_buffer_, receive_buffer_;
};
} // namespace teensy
diff --git a/motors/peripheral/uart_buffer.h b/motors/peripheral/uart_buffer.h
index 63dd70d..879ab5c 100644
--- a/motors/peripheral/uart_buffer.h
+++ b/motors/peripheral/uart_buffer.h
@@ -15,15 +15,25 @@
// Returns the number of characters added.
__attribute__((warn_unused_result)) int PushSpan(gsl::span<const char> data);
+ // max is the maximum size the returned spans should be.
+ // The data in the result is only valid until another method is called.
+ // Note that this may not return all available data when doing so would
+ // require wrapping around, but it will always return a non-empty span if any
+ // data is available.
+ gsl::span<const char> PopSpan(int max);
+
bool empty() const { return size_ == 0; }
+ bool full() const { return size_ == kSize; }
// This may only be called when !empty().
char PopSingle();
+ // This may only be called when !full().
+ void PushSingle(char c);
static constexpr int size() { return kSize; }
private:
- // The index at which we will push the next character.
+ // The index at which we will pop the next character.
int start_ = 0;
// How many characters we currently have.
int size_ = 0;
@@ -31,7 +41,7 @@
::std::array<char, kSize> data_;
};
-template<int kSize>
+template <int kSize>
int UartBuffer<kSize>::PushSpan(gsl::span<const char> data) {
const int end_location = (start_ + size_) % kSize;
const int remaining_end = ::std::min(kSize - size_, kSize - end_location);
@@ -52,7 +62,16 @@
return on_end + on_start;
}
-template<int kSize>
+template <int kSize>
+gsl::span<const char> UartBuffer<kSize>::PopSpan(int max) {
+ const size_t result_size = std::min(max, std::min(kSize - start_, size_));
+ const auto result = gsl::span<const char>(data_).subspan(start_, result_size);
+ start_ = (start_ + result_size) % kSize;
+ size_ -= result_size;
+ return result;
+}
+
+template <int kSize>
char UartBuffer<kSize>::PopSingle() {
const char r = data_[start_];
--size_;
@@ -60,6 +79,13 @@
return r;
}
+template <int kSize>
+void UartBuffer<kSize>::PushSingle(char c) {
+ const int end_location = (start_ + size_) % kSize;
+ data_[end_location] = c;
+ ++size_;
+}
+
} // namespace teensy
} // namespace frc971
diff --git a/motors/peripheral/uart_buffer_test.cc b/motors/peripheral/uart_buffer_test.cc
index 5ab7c98..c464f8a 100644
--- a/motors/peripheral/uart_buffer_test.cc
+++ b/motors/peripheral/uart_buffer_test.cc
@@ -181,6 +181,106 @@
ASSERT_TRUE(buffer.empty());
}
+// Tests that using PopSpan with single characters works correctly.
+TEST(UartBufferTest, PopSpanSingle) {
+ UartBuffer<3> buffer;
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(1);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(2);
+ ASSERT_FALSE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(1u, result[0]);
+ }
+
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(3);
+ ASSERT_FALSE(buffer.full());
+ buffer.PushSingle(4);
+ ASSERT_TRUE(buffer.full());
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(2u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(3u, result[0]);
+ }
+
+ {
+ const auto result = buffer.PopSpan(1);
+ ASSERT_EQ(1u, result.size());
+ EXPECT_EQ(4u, result[0]);
+ }
+}
+
+// Tests that using PopSpan with multiple characters works correctly.
+TEST(UartBufferTest, PopSpanMultiple) {
+ UartBuffer<1024> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ ASSERT_TRUE(buffer.PopSpan(5).empty());
+ ASSERT_TRUE(buffer.PopSpan(3000).empty());
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+}
+
+// Tests that using PopSpan with multiple characters works correctly when
+// wrapping.
+TEST(UartBufferTest, PopSpanWrapMultiple) {
+ UartBuffer<10> buffer;
+ for (int i = 0; i < 10; ++i) {
+ buffer.PushSingle(i);
+ }
+ ASSERT_TRUE(buffer.PopSpan(0).empty());
+ {
+ const auto result = buffer.PopSpan(5);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i), result[i]);
+ }
+ }
+ for (int i = 0; i < 5; ++i) {
+ buffer.PushSingle(20 + i);
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 5), result[i]);
+ }
+ }
+ {
+ const auto result = buffer.PopSpan(10);
+ ASSERT_EQ(5u, result.size());
+ for (int i = 0; i < 5; ++i) {
+ EXPECT_EQ(static_cast<char>(i + 20), result[i]);
+ }
+ }
+}
+
} // namespace testing
} // namespace teensy
} // namespace frc971
diff --git a/third_party/google-glog/BUILD b/third_party/google-glog/BUILD
index 37fb27b..0c41228 100644
--- a/third_party/google-glog/BUILD
+++ b/third_party/google-glog/BUILD
@@ -1,5 +1,5 @@
-licenses(['notice'])
+licenses(["notice"])
-load(':bazel/glog.bzl', 'glog_library')
+load(":bazel/glog.bzl", "glog_library")
glog_library()
diff --git a/tools/ci/run-tests.sh b/tools/ci/run-tests.sh
index 91fbb2d..93e5d09 100755
--- a/tools/ci/run-tests.sh
+++ b/tools/ci/run-tests.sh
@@ -9,4 +9,4 @@
bazel test -c opt --config=eigen --curses=no --color=no ${TARGETS}
bazel build -c opt --curses=no --color=no ${TARGETS} --cpu=roborio
bazel build --curses=no --color=no ${TARGETS} --cpu=armhf-debian
-bazel build -c opt --curses=no --color=no //motors/... --cpu=cortex-m4f
+bazel build -c opt --curses=no --color=no //motors/... //y2019/jevois/... --cpu=cortex-m4f
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 99e3be8..fde8c12 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -1,4 +1,5 @@
-package(default_visibility = ["//visibility:public"])
+load("//tools:environments.bzl", "mcu_cpus")
+load("//motors:macros.bzl", "hex_from_elf")
spi_crc_args = [
"$(location //third_party/pycrc:pycrc_main)",
@@ -62,6 +63,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/containers:sized_array",
+ "//aos/time",
"//third_party/eigen",
],
)
@@ -135,9 +137,30 @@
cc_library(
name = "serial",
- hdrs = ["serial.h"],
srcs = ["serial.cc"],
+ hdrs = ["serial.h"],
deps = [
- "//aos/logging:logging",
+ "//aos/logging",
],
)
+
+cc_binary(
+ name = "teensy.elf",
+ srcs = [
+ "teensy.cc",
+ ],
+ restricted_to = ["//tools:cortex-m4f"],
+ deps = [
+ "//aos/time:time_mcu",
+ "//motors:util",
+ "//motors/core",
+ "//motors/peripheral:configuration",
+ "//motors/peripheral:uart",
+ "//motors/print:usb",
+ ],
+)
+
+hex_from_elf(
+ name = "teensy",
+ restricted_to = ["//tools:cortex-m4f"],
+)
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index c82a089..15889c4 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -10,6 +10,7 @@
#include "Eigen/Dense"
#include "aos/containers/sized_array.h"
+#include "aos/time/time.h"
namespace frc971 {
namespace jevois {
@@ -106,6 +107,15 @@
// This is all the information sent from the Teensy to each camera.
struct CameraCalibration {
+ enum class CameraCommand {
+ // Stay in normal mode.
+ kNormal,
+ // Go to camera passthrough mode.
+ kCameraPassthrough,
+ // Go to being a useful USB device.
+ kUsb,
+ };
+
bool operator==(const CameraCalibration &other) const {
if (other.calibration != calibration) {
return false;
@@ -120,6 +130,17 @@
//
// TODO(Parker): What are the details on how this is defined?
Eigen::Matrix<float, 3, 4> calibration;
+
+ // A local timestamp from the Teensy. This starts at 0 when the Teensy is
+ // powered on.
+ aos::monotonic_clock::time_point teensy_now;
+
+ // A realtime timestamp from the roboRIO. This will be min_time if the roboRIO
+ // has never sent anything.
+ aos::realtime_clock::time_point realtime_now;
+
+ // What mode the camera should transition into.
+ CameraCommand camera_command;
};
// This is all the information the Teensy sends to the RoboRIO.
@@ -160,6 +181,9 @@
// Whether the light ring for each camera should be on.
std::bitset<5> light_rings;
+
+ // The current time.
+ aos::realtime_clock::time_point realtime_now;
};
} // namespace jevois
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
new file mode 100644
index 0000000..2d3c020
--- /dev/null
+++ b/y2019/jevois/teensy.cc
@@ -0,0 +1,412 @@
+#include "aos/time/time.h"
+#include "motors/core/kinetis.h"
+#include "motors/core/time.h"
+#include "motors/peripheral/configuration.h"
+#include "motors/peripheral/uart.h"
+#include "motors/print/print.h"
+#include "motors/util.h"
+
+namespace frc971 {
+namespace jevois {
+namespace {
+
+struct Uarts {
+ Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = this;
+ }
+ ~Uarts() {
+ DisableInterrupts disable_interrupts;
+ instance = nullptr;
+ }
+
+ void Initialize(int baud_rate) {
+ cam0.Initialize(baud_rate);
+ cam1.Initialize(baud_rate);
+ cam2.Initialize(baud_rate);
+ cam3.Initialize(baud_rate);
+ cam4.Initialize(baud_rate);
+ }
+
+ frc971::teensy::InterruptBufferedUart cam0{&UART1, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam1{&UART0, F_CPU};
+ frc971::teensy::InterruptBufferedUart cam2{&UART2, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam3{&UART3, BUS_CLOCK_FREQUENCY};
+ frc971::teensy::InterruptBufferedUart cam4{&UART4, BUS_CLOCK_FREQUENCY};
+
+ static Uarts *instance;
+};
+
+Uarts *Uarts::instance = nullptr;
+
+extern "C" {
+
+void *__stack_chk_guard = (void *)0x67111971;
+void __stack_chk_fail(void) {
+ while (true) {
+ GPIOC_PSOR = (1 << 5);
+ printf("Stack corruption detected\n");
+ delay(1000);
+ GPIOC_PCOR = (1 << 5);
+ delay(1000);
+ }
+}
+
+extern char *__brkval;
+extern uint32_t __bss_ram_start__[];
+extern uint32_t __heap_start__[];
+extern uint32_t __stack_end__[];
+
+void uart0_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam1.HandleInterrupt(disable_interrupts);
+}
+
+void uart1_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam0.HandleInterrupt(disable_interrupts);
+}
+
+void uart2_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam2.HandleInterrupt(disable_interrupts);
+}
+
+void uart3_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam3.HandleInterrupt(disable_interrupts);
+}
+
+void uart4_status_isr(void) {
+ DisableInterrupts disable_interrupts;
+ Uarts::instance->cam4.HandleInterrupt(disable_interrupts);
+}
+
+} // extern "C"
+
+// A test program which echos characters back after adding a per-UART offset to
+// them (CAM0 adds 1, CAM1 adds 2, etc).
+__attribute__((unused)) void TestUarts() {
+ Uarts *const uarts = Uarts::instance;
+ while (true) {
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam0.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 1;
+ }
+ uarts->cam0.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam1.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 2;
+ }
+ uarts->cam1.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam2.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 3;
+ }
+ uarts->cam2.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam3.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 4;
+ }
+ uarts->cam3.Write(data);
+ }
+ {
+ std::array<char, 10> buffer;
+ const auto data = uarts->cam4.Read(buffer);
+ for (int i = 0; i < data.size(); ++i) {
+ data[i] += 5;
+ }
+ uarts->cam4.Write(data);
+ }
+ }
+}
+
+// Tests all the I/O pins. Cycles through each one for 1 second. While active,
+// each output is turned on, and each input has its value printed.
+__attribute__((unused)) void TestIo() {
+ // Set SPI0 pins to GPIO.
+ // SPI_OUT
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 6) = 1;
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_CS
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 0) = 0;
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_IN
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 7) = 0;
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // SPI_SCK
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 1) = 0;
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ // Set LED pins to GPIO.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 11) = 1;
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 10) = 1;
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 8) = 1;
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 9) = 1;
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 18) = 1;
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 2) = 1;
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 7) = 1;
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 1) = 1;
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 19) = 1;
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 5) = 1;
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ auto next = aos::monotonic_clock::now();
+ static constexpr auto kTick = std::chrono::seconds(1);
+ while (true) {
+ printf("SPI_MISO\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 6) = 0;
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CS %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 0));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_MOSI %d\n", (int)PERIPHERAL_BITBAND(GPIOC_PDIR, 7));
+ }
+ next += kTick;
+
+ while (aos::monotonic_clock::now() < next + kTick) {
+ printf("SPI_CLK %d\n", (int)PERIPHERAL_BITBAND(GPIOD_PDIR, 1));
+ }
+ next += kTick;
+
+ printf("CAM0\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = 0;
+ next += kTick;
+
+ printf("CAM1\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = 0;
+ next += kTick;
+
+ printf("CAM2\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = 0;
+ next += kTick;
+
+ printf("CAM3\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = 0;
+ next += kTick;
+
+ printf("CAM4\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = 0;
+ next += kTick;
+
+ printf("CAM5\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 2) = 0;
+ next += kTick;
+
+ printf("CAM6\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 7) = 0;
+ next += kTick;
+
+ printf("CAM7\n");
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 1) = 0;
+ next += kTick;
+
+ printf("CAM8\n");
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 19) = 0;
+ next += kTick;
+
+ printf("CAM9\n");
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 1;
+ while (aos::monotonic_clock::now() < next + kTick) {
+ }
+ PERIPHERAL_BITBAND(GPIOD_PDOR, 5) = 0;
+ next += kTick;
+ }
+}
+
+int Main() {
+ // for background about this startup delay, please see these conversations
+ // https://forum.pjrc.com/threads/36606-startup-time-(400ms)?p=113980&viewfull=1#post113980
+ // https://forum.pjrc.com/threads/31290-Teensey-3-2-Teensey-Loader-1-24-Issues?p=87273&viewfull=1#post87273
+ delay(400);
+
+ // Set all interrupts to the second-lowest priority to start with.
+ for (int i = 0; i < NVIC_NUM_INTERRUPTS; i++) NVIC_SET_SANE_PRIORITY(i, 0xD);
+
+ // Now set priorities for all the ones we care about. They only have meaning
+ // relative to each other, which means centralizing them here makes it a lot
+ // more manageable.
+ NVIC_SET_SANE_PRIORITY(IRQ_FTM0, 0x3);
+ NVIC_SET_SANE_PRIORITY(IRQ_UART0_STATUS, 0xE);
+
+ // Set the LED's pin to output mode.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 5) = 1;
+ PORTC_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+
+ frc971::motors::PrintingParameters printing_parameters;
+ printing_parameters.dedicated_usb = true;
+ const ::std::unique_ptr<frc971::motors::PrintingImplementation> printing =
+ CreatePrinting(printing_parameters);
+ printing->Initialize();
+
+ DMA.CR = M_DMA_EMLM;
+
+ SIM_SCGC4 |=
+ SIM_SCGC4_UART0 | SIM_SCGC4_UART1 | SIM_SCGC4_UART2 | SIM_SCGC4_UART3;
+ SIM_SCGC1 |= SIM_SCGC1_UART4;
+
+ // SPI0 goes to the roboRIO.
+ // SPI0_PCS0 is SPI_CS.
+ PORTD_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SOUT is SPI_MISO.
+ PORTC_PCR6 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SIN is SPI_MOSI.
+ PORTC_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+ // SPI0_SCK is SPI_CLK.
+ PORTD_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(2);
+
+ // FTM0_CH0 is LED0 (7 in silkscreen, a beacon channel).
+ PORTC_PCR1 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH1 is LED1 (5 in silkscreen, a beacon channel).
+ PORTC_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH7 is LED2 (6 in silkscreen, a beacon channel).
+ PORTD_PCR7 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+ // FTM0_CH5 is LED3 (9 in silkscreen, a vision camera).
+ PORTD_PCR5 = PORT_PCR_DSE | PORT_PCR_MUX(4);
+
+ // FTM2_CH1 is LED4 (8 in silkscreen, a vision camera).
+ PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM2_CH0 is LED5 (for CAM4).
+ PORTB_PCR18 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // FTM3_CH4 is LED6 (for CAM2).
+ PORTC_PCR8 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH5 is LED7 (for CAM3).
+ PORTC_PCR9 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH6 is LED8 (for CAM1).
+ PORTC_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // FTM3_CH7 is LED9 (for CAM0).
+ PORTC_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // This hardware has been deactivated, but keep this comment for now to
+ // document which pins it is on.
+#if 0
+ // This is ODROID_EN.
+ PERIPHERAL_BITBAND(GPIOC_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 0) = 0;
+ PORTC_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+ // This is CAM_EN.
+ PERIPHERAL_BITBAND(GPIOB_PDDR, 0) = 1;
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 0) = 0;
+ PORTB_PCR0 = PORT_PCR_DSE | PORT_PCR_MUX(1);
+#endif
+ // This is 5V_PGOOD.
+ PERIPHERAL_BITBAND(GPIOD_PDDR, 6) = 0;
+ PORTD_PCR6 = PORT_PCR_MUX(1);
+
+ // These go to CAM1.
+ // UART0_RX (peripheral) is UART1_RX (schematic).
+ PORTA_PCR15 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART0_TX (peripheral) is UART1_TX (schematic).
+ PORTA_PCR14 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM0.
+ // UART1_RX (peripheral) is UART0_RX (schematic).
+ PORTC_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART1_TX (peripheral) is UART0_TX (schematic).
+ PORTC_PCR4 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM2.
+ // UART2_RX
+ PORTD_PCR2 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART2_TX
+ PORTD_PCR3 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM3.
+ // UART3_RX
+ PORTB_PCR10 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART3_TX
+ PORTB_PCR11 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ // These go to CAM4.
+ // UART4_RX
+ PORTE_PCR25 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+ // UART4_TX
+ PORTE_PCR24 = PORT_PCR_DSE | PORT_PCR_MUX(3);
+
+ Uarts uarts;
+
+ // Give everything a chance to get going.
+ delay(100);
+
+ printf("Ram start: %p\n", __bss_ram_start__);
+ printf("Heap start: %p\n", __heap_start__);
+ printf("Heap end: %p\n", __brkval);
+ printf("Stack start: %p\n", __stack_end__);
+
+ uarts.Initialize(115200);
+ NVIC_ENABLE_IRQ(IRQ_UART0_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART1_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART2_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART3_STATUS);
+ NVIC_ENABLE_IRQ(IRQ_UART4_STATUS);
+
+ while (true) {
+ }
+}
+
+extern "C" {
+
+int main(void) {
+ return Main();
+}
+
+} // extern "C"
+
+} // namespace
+} // namespace jevois
+} // namespace frc971
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
new file mode 100644
index 0000000..6736c24
--- /dev/null
+++ b/y2019/vision/BUILD
@@ -0,0 +1,80 @@
+load("//aos/build:queues.bzl", "queue_library")
+load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
+load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+
+package(default_visibility = ["//visibility:public"])
+
+VISION_TARGETS = [ "//tools:k8", "//tools:armhf-debian"]
+
+cc_library(
+ name = "target_finder",
+ srcs = ["target_finder.cc", "target_geometry.cc"],
+ hdrs = ["target_finder.h", "target_types.h"],
+ deps = [
+ "@com_google_ceres_solver//:ceres",
+ "//aos/vision/blob:hierarchical_contour_merge",
+ "//aos/vision/blob:region_alloc",
+ "//aos/vision/blob:contour",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:overlay",
+ "//aos/vision/math:vector",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+gtk_dependent_cc_binary(
+ name = "debug_viewer",
+ srcs = ["debug_viewer.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/vision/blob:move_scale",
+ "//aos/vision/blob:threshold",
+ "//aos/vision/blob:transpose",
+ "//aos/vision/debug:debug_framework",
+ "//aos/vision/math:vector",
+ ],
+ copts = ["-Wno-unused-variable"],
+ restricted_to = VISION_TARGETS,
+)
+
+cc_binary(
+ name = "target_sender",
+ srcs = ["target_sender.cc"],
+ deps = [
+ ":target_finder",
+ "//y2019/jevois:serial",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+
+"""
+cc_binary(
+ name = "calibration",
+ srcs = ["calibration.cc"],
+ deps = [
+ ":target_finder",
+ "//aos/logging",
+ "//aos/logging:implementations",
+ "//aos/vision/blob:find_blob",
+ "//aos/vision/blob:codec",
+ "//aos/vision/events:epoll_events",
+ "//aos/vision/events:socket_types",
+ "//aos/vision/events:udp",
+ "//aos/vision/image:image_stream",
+ "//aos/vision/image:reader",
+ "@com_google_ceres_solver//:ceres",
+ ],
+ restricted_to = VISION_TARGETS,
+)
+"""
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
new file mode 100644
index 0000000..4f43c9a
--- /dev/null
+++ b/y2019/vision/debug_viewer.cc
@@ -0,0 +1,269 @@
+#include <Eigen/Dense>
+#include <iostream>
+
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/move_scale.h"
+#include "aos/vision/blob/stream_view.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/debug_framework.h"
+#include "aos/vision/math/vector.h"
+
+using aos::vision::ImageRange;
+using aos::vision::ImageFormat;
+using aos::vision::RangeImage;
+using aos::vision::AnalysisAllocator;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+using aos::vision::Segment;
+using aos::vision::PixelRef;
+
+namespace y2019 {
+namespace vision {
+
+std::vector<PixelRef> GetNColors(size_t num_colors) {
+ std::vector<PixelRef> colors;
+ for (size_t i = 0; i < num_colors; ++i) {
+ int quadrent = i * 6 / num_colors;
+ uint8_t alpha = (256 * 6 * i - quadrent * num_colors * 256) / num_colors;
+ uint8_t inv_alpha = 255 - alpha;
+ switch (quadrent) {
+ case 0:
+ colors.push_back(PixelRef{255, alpha, 0});
+ break;
+ case 1:
+ colors.push_back(PixelRef{inv_alpha, 255, 0});
+ break;
+ case 2:
+ colors.push_back(PixelRef{0, 255, alpha});
+ break;
+ case 3:
+ colors.push_back(PixelRef{0, inv_alpha, 255});
+ break;
+ case 4:
+ colors.push_back(PixelRef{alpha, 0, 255});
+ break;
+ case 5:
+ colors.push_back(PixelRef{255, 0, inv_alpha});
+ break;
+ }
+ }
+ return colors;
+}
+
+class FilterHarness : public aos::vision::FilterHarness {
+ public:
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image) override {
+ return finder_.Threshold(image);
+ }
+
+ void InstallViewer(aos::vision::BlobStreamViewer *viewer) override {
+ viewer_ = viewer;
+ viewer_->SetScale(0.75);
+ overlays_.push_back(&overlay_);
+ overlays_.push_back(finder_.GetOverlay());
+ viewer_->view()->SetOverlays(&overlays_);
+ }
+
+ void DrawBlob(const RangeImage &blob, PixelRef color) {
+ if (viewer_) {
+ BlobList list;
+ list.push_back(blob);
+ viewer_->DrawBlobList(list, color);
+ }
+ }
+
+ bool HandleBlobs(BlobList imgs, ImageFormat fmt) override {
+ imgs_last_ = imgs;
+ fmt_last_ = fmt;
+ // reset for next drawing cycle
+ for (auto &overlay : overlays_) {
+ overlay->Reset();
+ }
+
+ if (draw_select_blob_ || draw_raw_poly_ || draw_components_ ||
+ draw_raw_target_ || draw_raw_IR_ || draw_results_) {
+ printf("_____ New Image _____\n");
+ }
+
+ // Remove bad blobs.
+ finder_.PreFilter(&imgs);
+
+ // Find polygons from blobs.
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon =
+ finder_.FillPolygon(blob, draw_raw_poly_);
+ if (polygon.empty()) {
+ DrawBlob(blob, {255, 0, 0});
+ } else {
+ raw_polys.push_back(polygon);
+ if (draw_select_blob_) {
+ DrawBlob(blob, {0, 0, 255});
+ }
+ if (draw_raw_poly_) {
+ std::vector<PixelRef> colors = GetNColors(polygon.size());
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(polygon[i].Intersect(polygon[(i + 1) % 4]));
+ }
+
+ for (size_t i = 0; i < 4; ++i) {
+ overlay_.AddLine(corners[i], corners[(i + 1) % 4], colors[i]);
+ }
+ }
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+ if (draw_components_) {
+ for (const TargetComponent &comp : target_component_list) {
+ DrawComponent(comp, {0, 255, 255}, {0, 255, 255}, {255, 0, 0},
+ {0, 0, 255});
+ }
+ }
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list = finder_.FindTargetsFromComponents(
+ target_component_list, draw_raw_target_);
+ if (draw_raw_target_) {
+ for (const Target &target : target_list) {
+ DrawTarget(target);
+ }
+ }
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
+ }
+
+ // Check that our current results match possible solutions.
+ results = finder_.FilterResults(results);
+ if (draw_results_) {
+ for (const IntermediateResult &res : results) {
+ DrawResult(res, {0, 255, 0});
+ DrawTarget(res, {0, 255, 0});
+ }
+ }
+
+ // If the target list is not empty then we found a target.
+ return !results.empty();
+ }
+
+ std::function<void(uint32_t)> RegisterKeyPress() override {
+ return [this](uint32_t key) {
+ (void)key;
+ if (key == 'z') {
+ draw_results_ = !draw_results_;
+ } else if (key == 'x') {
+ draw_raw_IR_ = !draw_raw_IR_;
+ } else if (key == 'c') {
+ draw_raw_target_ = !draw_raw_target_;
+ } else if (key == 'v') {
+ draw_components_ = !draw_components_;
+ } else if (key == 'b') {
+ draw_raw_poly_ = !draw_raw_poly_;
+ } else if (key == 'n') {
+ draw_select_blob_ = !draw_select_blob_;
+ } else if (key == 'q') {
+ printf("User requested shutdown.\n");
+ exit(0);
+ }
+ HandleBlobs(imgs_last_, fmt_last_);
+ viewer_->Redraw();
+ };
+ }
+
+ void DrawComponent(const TargetComponent &comp, PixelRef top_color,
+ PixelRef bot_color, PixelRef in_color,
+ PixelRef out_color) {
+ overlay_.AddLine(comp.top, comp.inside, top_color);
+ overlay_.AddLine(comp.bottom, comp.outside, bot_color);
+
+ overlay_.AddLine(comp.bottom, comp.inside, in_color);
+ overlay_.AddLine(comp.top, comp.outside, out_color);
+ }
+
+ void DrawTarget(const Target &target) {
+ Vector<2> leftTop = (target.left.top + target.left.inside) * 0.5;
+ Vector<2> rightTop = (target.right.top + target.right.inside) * 0.5;
+ overlay_.AddLine(leftTop, rightTop, {255, 215, 0});
+
+ Vector<2> leftBot = (target.left.bottom + target.left.outside) * 0.5;
+ Vector<2> rightBot = (target.right.bottom + target.right.outside) * 0.5;
+ overlay_.AddLine(leftBot, rightBot, {255, 215, 0});
+
+ overlay_.AddLine(leftTop, leftBot, {255, 215, 0});
+ overlay_.AddLine(rightTop, rightBot, {255, 215, 0});
+ }
+
+ void DrawResult(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ DrawComponent(target.left, color, color, color, color);
+ DrawComponent(target.right, color, color, color, color);
+ }
+
+ void DrawTarget(const IntermediateResult &result, PixelRef color) {
+ Target target =
+ Project(finder_.GetTemplateTarget(), intrinsics(), result.extrinsics);
+ Segment<2> leftAx((target.left.top + target.left.inside) * 0.5,
+ (target.left.bottom + target.left.outside) * 0.5);
+ leftAx.Set(leftAx.A() * 0.9 + leftAx.B() * 0.1,
+ leftAx.B() * 0.9 + leftAx.A() * 0.1);
+ overlay_.AddLine(leftAx, color);
+
+ Segment<2> rightAx((target.right.top + target.right.inside) * 0.5,
+ (target.right.bottom + target.right.outside) * 0.5);
+ rightAx.Set(rightAx.A() * 0.9 + rightAx.B() * 0.1,
+ rightAx.B() * 0.9 + rightAx.A() * 0.1);
+ overlay_.AddLine(rightAx, color);
+
+ overlay_.AddLine(leftAx.A(), rightAx.A(), color);
+ overlay_.AddLine(leftAx.B(), rightAx.B(), color);
+ Vector<3> p1(0.0, 0.0, 100.0);
+
+ Vector<3> p2 =
+ Rotate(intrinsics().mount_angle, result.extrinsics.r1, 0.0, p1);
+ Vector<2> p3(p2.x(), p2.y());
+ overlay_.AddLine(leftAx.A(), p3 + leftAx.A(), {0, 255, 0});
+ overlay_.AddLine(leftAx.B(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(rightAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(rightAx.B(), p3 + rightAx.B(), {0, 255, 0});
+
+ overlay_.AddLine(p3 + leftAx.A(), p3 + leftAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.A(), p3 + rightAx.A(), {0, 255, 0});
+ overlay_.AddLine(p3 + rightAx.A(), p3 + rightAx.B(), {0, 255, 0});
+ overlay_.AddLine(p3 + leftAx.B(), p3 + rightAx.B(), {0, 255, 0});
+ }
+
+ const IntrinsicParams &intrinsics() const { return finder_.intrinsics(); }
+
+ private:
+ // implementation of the filter pipeline.
+ TargetFinder finder_;
+ aos::vision::BlobStreamViewer *viewer_ = nullptr;
+ aos::vision::PixelLinesOverlay overlay_;
+ std::vector<aos::vision::OverlayBase *> overlays_;
+ BlobList imgs_last_;
+ ImageFormat fmt_last_;
+ bool draw_select_blob_ = false;
+ bool draw_raw_poly_ = false;
+ bool draw_components_ = false;
+ bool draw_raw_target_ = false;
+ bool draw_raw_IR_ = false;
+ bool draw_results_ = true;
+};
+
+} // namespace vision
+} // namespace y2017
+
+int main(int argc, char **argv) {
+ y2019::vision::FilterHarness filter_harness;
+ aos::vision::DebugFrameworkMain(argc, argv, &filter_harness,
+ aos::vision::CameraParams());
+}
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
new file mode 100644
index 0000000..f69e987
--- /dev/null
+++ b/y2019/vision/target_finder.cc
@@ -0,0 +1,379 @@
+#include "y2019/vision/target_finder.h"
+
+#include "aos/vision/blob/hierarchical_contour_merge.h"
+
+using namespace aos::vision;
+
+namespace y2019 {
+namespace vision {
+
+TargetFinder::TargetFinder() { target_template_ = Target::MakeTemplate(); }
+
+aos::vision::RangeImage TargetFinder::Threshold(aos::vision::ImagePtr image) {
+ const uint8_t threshold_value = GetThresholdValue();
+ return aos::vision::DoThreshold(image, [&](aos::vision::PixelRef &px) {
+ if (px.g > threshold_value && px.b > threshold_value &&
+ px.r > threshold_value) {
+ return true;
+ }
+ return false;
+ });
+}
+
+// Filter blobs on size.
+void TargetFinder::PreFilter(BlobList *imgs) {
+ imgs->erase(
+ std::remove_if(imgs->begin(), imgs->end(),
+ [](RangeImage &img) {
+ // We can drop images with a small number of
+ // pixels, but images
+ // must be over 20px or the math will have issues.
+ return (img.npixels() < 100 || img.height() < 25);
+ }),
+ imgs->end());
+}
+
+// TODO: Try hierarchical merge for this.
+// Convert blobs into polygons.
+std::vector<aos::vision::Segment<2>> TargetFinder::FillPolygon(
+ const RangeImage &blob, bool verbose) {
+ if (verbose) printf("Process Polygon.\n");
+ alloc_.reset();
+ auto *st = RangeImgToContour(blob, &alloc_);
+
+ struct Pt {
+ float x;
+ float y;
+ };
+ std::vector<Pt> pts;
+
+ // Collect all slopes from the contour.
+ auto opt = st->pt;
+ for (auto *node = st; node->next != st;) {
+ node = node->next;
+
+ auto npt = node->pt;
+
+ pts.push_back(
+ {static_cast<float>(npt.x - opt.x), static_cast<float>(npt.y - opt.y)});
+
+ opt = npt;
+ }
+
+ const int n = pts.size();
+ auto get_pt = [&](int i) { return pts[(i + n * 2) % n]; };
+
+ std::vector<Pt> pts_new = pts;
+ auto run_box_filter = [&](int window_size) {
+ for (size_t i = 0; i < pts.size(); ++i) {
+ Pt a{0.0, 0.0};
+ for (int j = -window_size; j <= window_size; ++j) {
+ Pt p = get_pt(j + i);
+ a.x += p.x;
+ a.y += p.y;
+ }
+ a.x /= (window_size * 2 + 1);
+ a.y /= (window_size * 2 + 1);
+
+ float scale = 1.0 + (i / float(pts.size() * 10));
+ a.x *= scale;
+ a.y *= scale;
+ pts_new[i] = a;
+ }
+ pts = pts_new;
+ };
+ // Three box filter makith a guassian?
+ // Run gaussian filter over the slopes.
+ run_box_filter(2);
+ run_box_filter(2);
+ run_box_filter(2);
+
+ // Heuristic which says if a particular slope is part of a corner.
+ auto is_corner = [&](size_t i) {
+ Pt a = get_pt(i - 3);
+ Pt b = get_pt(i + 3);
+ double dx = (a.x - b.x);
+ double dy = (a.y - b.y);
+ return dx * dx + dy * dy > 0.25;
+ };
+
+ bool prev_v = is_corner(-1);
+
+ // Find all centers of corners.
+ // Because they round, multiple points may be a corner.
+ std::vector<size_t> edges;
+ size_t kBad = pts.size() + 10;
+ size_t prev_up = kBad;
+ size_t wrapped_n = prev_up;
+
+ for (size_t i = 0; i < pts.size(); ++i) {
+ bool v = is_corner(i);
+ if (prev_v && !v) {
+ if (prev_up == kBad) {
+ wrapped_n = i;
+ } else {
+ edges.push_back((prev_up + i - 1) / 2);
+ }
+ }
+ if (v && !prev_v) {
+ prev_up = i;
+ }
+ prev_v = v;
+ }
+
+ if (wrapped_n != kBad) {
+ edges.push_back(((prev_up + pts.size() + wrapped_n - 1) / 2) % pts.size());
+ }
+
+ if (verbose) printf("Edge Count (%zu).\n", edges.size());
+
+ // Get all CountourNodes from the contour.
+ using aos::vision::PixelRef;
+ std::vector<ContourNode *> segments;
+ {
+ std::vector<ContourNode *> segments_all;
+
+ for (ContourNode *node = st; node->next != st;) {
+ node = node->next;
+ segments_all.push_back(node);
+ }
+ for (size_t i : edges) {
+ segments.push_back(segments_all[i]);
+ }
+ }
+ if (verbose) printf("Segment Count (%zu).\n", segments.size());
+
+ // Run best-fits over each line segment.
+ std::vector<Segment<2>> seg_list;
+ if (segments.size() == 4) {
+ for (size_t i = 0; i < segments.size(); ++i) {
+ auto *ed = segments[(i + 1) % segments.size()];
+ auto *st = segments[i];
+ float mx = 0.0;
+ float my = 0.0;
+ int n = 0;
+ for (auto *node = st; node != ed; node = node->next) {
+ mx += node->pt.x;
+ my += node->pt.y;
+ ++n;
+ // (x - [x] / N) ** 2 = [x * x] - 2 * [x] * [x] / N + [x] * [x] / N / N;
+ }
+ mx /= n;
+ my /= n;
+
+ float xx = 0.0;
+ float xy = 0.0;
+ float yy = 0.0;
+ for (auto *node = st; node != ed; node = node->next) {
+ float x = node->pt.x - mx;
+ float y = node->pt.y - my;
+ xx += x * x;
+ xy += x * y;
+ yy += y * y;
+ }
+
+ // TODO: Extract common to hierarchical merge.
+ float neg_b_over_2 = (xx + yy) / 2.0;
+ float c = (xx * yy - xy * xy);
+
+ float sqr = sqrt(neg_b_over_2 * neg_b_over_2 - c);
+
+ {
+ float lam = neg_b_over_2 + sqr;
+ float x = xy;
+ float y = lam - xx;
+
+ float norm = sqrt(x * x + y * y);
+ x /= norm;
+ y /= norm;
+
+ seg_list.push_back(
+ Segment<2>(Vector<2>(mx, my), Vector<2>(mx + x, my + y)));
+ }
+
+ /* Characteristic polynomial
+ 1 lam^2 - (xx + yy) lam + (xx * yy - xy * xy) = 0
+
+ [a b]
+ [c d]
+
+ // covariance matrix.
+ [xx xy] [nx]
+ [xy yy] [ny]
+ */
+ }
+ }
+ if (verbose) printf("Poly Count (%zu).\n", seg_list.size());
+ return seg_list;
+}
+
+// Convert segments into target components (left or right)
+std::vector<TargetComponent> TargetFinder::FillTargetComponentList(
+ const std::vector<std::vector<Segment<2>>> &seg_list) {
+ std::vector<TargetComponent> list;
+ TargetComponent new_target;
+ for (const auto &poly : seg_list) {
+ // Reject missized pollygons for now. Maybe rectify them here in the future;
+ if (poly.size() != 4) continue;
+ std::vector<Vector<2>> corners;
+ for (size_t i = 0; i < 4; ++i) {
+ corners.push_back(poly[i].Intersect(poly[(i + 1) % 4]));
+ }
+
+ // Select the closest two points. Short side of the rectangle.
+ double min_dist = -1;
+ std::pair<size_t, size_t> closest;
+ for (size_t i = 0; i < 4; ++i) {
+ size_t next = (i + 1) % 4;
+ double nd = corners[i].SquaredDistanceTo(corners[next]);
+ if (min_dist == -1 || nd < min_dist) {
+ min_dist = nd;
+ closest.first = i;
+ closest.second = next;
+ }
+ }
+
+ // Verify our top is above the bottom.
+ size_t bot_index = closest.first;
+ size_t top_index = (closest.first + 2) % 4;
+ if (corners[top_index].y() < corners[bot_index].y()) {
+ closest.first = top_index;
+ closest.second = (top_index + 1) % 4;
+ }
+
+ // Find the major axis.
+ size_t far_first = (closest.first + 2) % 4;
+ size_t far_second = (closest.second + 2) % 4;
+ Segment<2> major_axis(
+ (corners[closest.first] + corners[closest.second]) * 0.5,
+ (corners[far_first] + corners[far_second]) * 0.5);
+ if (major_axis.AsVector().AngleToZero() > M_PI / 180.0 * 120.0 ||
+ major_axis.AsVector().AngleToZero() < M_PI / 180.0 * 60.0) {
+ // Target is angled way too much, drop it.
+ continue;
+ }
+
+ // organize the top points.
+ Vector<2> topA = corners[closest.first] - major_axis.B();
+ new_target.major_axis = major_axis;
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a left target since we are leaning positive.
+ new_target.is_right = false;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ }
+ } else {
+ // We have a right target since we are leaning negative.
+ new_target.is_right = true;
+ if (topA.AngleTo(major_axis.AsVector()) > 0.0) {
+ // And our A point is left of the major axis.
+ new_target.inside = corners[closest.first];
+ new_target.top = corners[closest.second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.inside = corners[closest.second];
+ new_target.top = corners[closest.first];
+ }
+ }
+
+ // organize the top points.
+ Vector<2> botA = corners[far_first] - major_axis.A();
+ if (major_axis.AsVector().AngleToZero() > M_PI / 2.0) {
+ // We have a right target since we are leaning positive.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ }
+ } else {
+ // We have a left target since we are leaning negative.
+ if (botA.AngleTo(major_axis.AsVector()) < M_PI) {
+ // And our A point is left of the major axis.
+ new_target.outside = corners[far_first];
+ new_target.bottom = corners[far_second];
+ } else {
+ // our A point is to the right of the major axis.
+ new_target.outside = corners[far_second];
+ new_target.bottom = corners[far_first];
+ }
+ }
+
+ // This piece of the target should be ready now.
+ list.emplace_back(new_target);
+ }
+
+ return list;
+}
+
+// Match components into targets.
+std::vector<Target> TargetFinder::FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose) {
+ std::vector<Target> target_list;
+ using namespace aos::vision;
+ if (component_list.size() < 2) {
+ // We don't enough parts for a target.
+ return target_list;
+ }
+
+ for (size_t i = 0; i < component_list.size(); i++) {
+ const TargetComponent &a = component_list[i];
+ for (size_t j = 0; j < i; j++) {
+ bool target_valid = false;
+ Target new_target;
+ const TargetComponent &b = component_list[j];
+
+ // Reject targets that are too far off vertically.
+ Vector<2> a_center = a.major_axis.Center();
+ if (a_center.y() > b.bottom.y() || a_center.y() < b.top.y()) {
+ continue;
+ }
+ Vector<2> b_center = b.major_axis.Center();
+ if (b_center.y() > a.bottom.y() || b_center.y() < a.top.y()) {
+ continue;
+ }
+
+ if (a.is_right && !b.is_right) {
+ if (a.top.x() > b.top.x()) {
+ new_target.right = a;
+ new_target.left = b;
+ target_valid = true;
+ }
+ } else if (!a.is_right && b.is_right) {
+ if (b.top.x() > a.top.x()) {
+ new_target.right = b;
+ new_target.left = a;
+ target_valid = true;
+ }
+ }
+ if (target_valid) {
+ target_list.emplace_back(new_target);
+ }
+ }
+ }
+ if (verbose) printf("Possible Target: %zu.\n", target_list.size());
+ return target_list;
+}
+
+std::vector<IntermediateResult> TargetFinder::FilterResults(
+ const std::vector<IntermediateResult> &results) {
+ std::vector<IntermediateResult> filtered;
+ for (const IntermediateResult &res : results) {
+ if (res.solver_error < 75.0) {
+ filtered.emplace_back(res);
+ }
+ }
+ return filtered;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
new file mode 100644
index 0000000..633733a
--- /dev/null
+++ b/y2019/vision/target_finder.h
@@ -0,0 +1,80 @@
+#ifndef _Y2019_VISION_TARGET_FINDER_H_
+#define _Y2019_VISION_TARGET_FINDER_H_
+
+#include "aos/vision/blob/region_alloc.h"
+#include "aos/vision/blob/threshold.h"
+#include "aos/vision/blob/transpose.h"
+#include "aos/vision/debug/overlay.h"
+#include "aos/vision/math/vector.h"
+#include "y2019/vision/target_types.h"
+
+namespace y2019 {
+namespace vision {
+
+using aos::vision::ImageRange;
+using aos::vision::RangeImage;
+using aos::vision::BlobList;
+using aos::vision::Vector;
+
+class TargetFinder {
+ public:
+ TargetFinder();
+ // Turn a raw image into blob range image.
+ aos::vision::RangeImage Threshold(aos::vision::ImagePtr image);
+
+ // Value against which we threshold.
+ uint8_t GetThresholdValue() { return 120; }
+
+ // filter out obvious or durranged blobs.
+ void PreFilter(BlobList *imgs);
+
+ // Turn a blob into a polgygon.
+ std::vector<aos::vision::Segment<2>> FillPolygon(const RangeImage &blob,
+ bool verbose);
+
+ // Turn a bloblist into components of a target.
+ std::vector<TargetComponent> FillTargetComponentList(
+ const std::vector<std::vector<aos::vision::Segment<2>>> &seg_list);
+
+ // Piece the compenents together into a target.
+ std::vector<Target> FindTargetsFromComponents(
+ const std::vector<TargetComponent> component_list, bool verbose);
+
+ // Given a target solve for the transformation of the template target.
+ IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
+
+ std::vector<IntermediateResult> FilterResults(
+ const std::vector<IntermediateResult> &results);
+
+ // Get the local overlay for debug if we are doing that.
+ aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
+
+ // Convert target location into meters and radians.
+ void GetAngleDist(const aos::vision::Vector<2> &target, double down_angle,
+ double *dist, double *angle);
+
+ // Return the template target in a normalized space.
+ const Target &GetTemplateTarget() { return target_template_; }
+
+ const IntrinsicParams &intrinsics() const { return intrinsics_; }
+
+ private:
+ // Find a loosly connected target.
+ double DetectConnectedTarget(const RangeImage &img);
+
+ aos::vision::PixelLinesOverlay overlay_;
+
+ aos::vision::AnalysisAllocator alloc_;
+
+ // The template for the default target in the standard space.
+ Target target_template_;
+
+ IntrinsicParams intrinsics_;
+
+ ExtrinsicParams default_extrinsics_;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif
diff --git a/y2019/vision/target_geometry.cc b/y2019/vision/target_geometry.cc
new file mode 100644
index 0000000..77adc83
--- /dev/null
+++ b/y2019/vision/target_geometry.cc
@@ -0,0 +1,186 @@
+#include "y2019/vision/target_finder.h"
+
+#include "ceres/ceres.h"
+
+#include <math.h>
+
+using ceres::NumericDiffCostFunction;
+using ceres::CENTRAL;
+using ceres::CostFunction;
+using ceres::Problem;
+using ceres::Solver;
+using ceres::Solve;
+
+namespace y2019 {
+namespace vision {
+
+static constexpr double kInchesToMeters = 0.0254;
+
+using namespace aos::vision;
+using aos::vision::Vector;
+
+Target Target::MakeTemplate() {
+ Target out;
+ // This is how off-vertical the tape is.
+ const double theta = 14.5 * M_PI / 180.0;
+
+ const double tape_offset = 4 * kInchesToMeters;
+ const double tape_width = 2 * kInchesToMeters;
+ const double tape_length = 5.5 * kInchesToMeters;
+
+ const double s = sin(theta);
+ const double c = cos(theta);
+ out.right.top = Vector<2>(tape_offset, 0.0);
+ out.right.inside = Vector<2>(tape_offset + tape_width * c, tape_width * s);
+ out.right.bottom = Vector<2>(tape_offset + tape_width * c + tape_length * s,
+ tape_width * s - tape_length * c);
+ out.right.outside =
+ Vector<2>(tape_offset + tape_length * s, -tape_length * c);
+
+ out.right.is_right = true;
+ out.left.top = Vector<2>(-out.right.top.x(), out.right.top.y());
+ out.left.inside = Vector<2>(-out.right.inside.x(), out.right.inside.y());
+ out.left.bottom = Vector<2>(-out.right.bottom.x(), out.right.bottom.y());
+ out.left.outside = Vector<2>(-out.right.outside.x(), out.right.outside.y());
+ return out;
+}
+
+std::array<Vector<2>, 8> Target::toPointList() const {
+ return std::array<Vector<2>, 8>{{right.top, right.inside, right.bottom,
+ right.outside, left.top, left.inside,
+ left.bottom, left.outside}};
+}
+
+Vector<2> Project(Vector<2> pt, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ double y = extrinsics.y;
+ double z = extrinsics.z;
+ double r1 = extrinsics.r1;
+ double r2 = extrinsics.r2;
+ double rup = intrinsics.mount_angle;
+ double fl = intrinsics.focal_length;
+
+ ::Eigen::Matrix<double, 1, 3> pts{pt.x(), pt.y() + y, 0.0};
+
+ {
+ double theta = r1;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ pts(2) += z;
+
+ {
+ double theta = r2;
+ double s = sin(theta);
+ double c = cos(theta);
+ pts = (::Eigen::Matrix<double, 3, 3>() << c, 0, -s, 0, 1, 0, s, 0,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Apply 15 degree downward rotation.
+ {
+ double theta = rup;
+ double s = sin(theta);
+ double c = cos(theta);
+
+ pts = (::Eigen::Matrix<double, 3, 3>() << 1, 0, 0, 0, c, -s, 0, s,
+ c).finished() *
+ pts.transpose();
+ }
+
+ // TODO: Final image projection.
+ ::Eigen::Matrix<double, 1, 3> res = pts;
+
+ float scale = fl / res.z();
+ return Vector<2>(res.x() * scale + 320.0, 240.0 - res.y() * scale);
+}
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics) {
+ auto project = [&](Vector<2> pt) {
+ return Project(pt, intrinsics, extrinsics);
+ };
+ Target new_targ;
+ new_targ.right.is_right = true;
+ new_targ.right.top = project(target.right.top);
+ new_targ.right.inside = project(target.right.inside);
+ new_targ.right.bottom = project(target.right.bottom);
+ new_targ.right.outside = project(target.right.outside);
+
+ new_targ.left.top = project(target.left.top);
+ new_targ.left.inside = project(target.left.inside);
+ new_targ.left.bottom = project(target.left.bottom);
+ new_targ.left.outside = project(target.left.outside);
+
+ return new_targ;
+}
+
+// Used at runtime on a single image given camera parameters.
+struct RuntimeCostFunctor {
+ RuntimeCostFunctor(Vector<2> result, Vector<2> template_pt,
+ IntrinsicParams intrinsics)
+ : result(result), template_pt(template_pt), intrinsics(intrinsics) {}
+
+ bool operator()(const double *const x, double *residual) const {
+ auto extrinsics = ExtrinsicParams::get(x);
+ auto pt = result - Project(template_pt, intrinsics, extrinsics);
+ residual[0] = pt.x();
+ residual[1] = pt.y();
+ return true;
+ }
+
+ Vector<2> result;
+ Vector<2> template_pt;
+ IntrinsicParams intrinsics;
+};
+
+IntermediateResult TargetFinder::ProcessTargetToResult(const Target &target,
+ bool verbose) {
+ // Memory for the ceres solver.
+ double params[ExtrinsicParams::kNumParams];
+ default_extrinsics_.set(¶ms[0]);
+
+ Problem problem;
+
+ auto target_value = target.toPointList();
+ auto template_value = target_template_.toPointList();
+
+ for (size_t i = 0; i < 8; ++i) {
+ auto a = template_value[i];
+ auto b = target_value[i];
+
+ problem.AddResidualBlock(
+ new NumericDiffCostFunction<RuntimeCostFunctor, CENTRAL, 2, 4>(
+ new RuntimeCostFunctor(b, a, intrinsics_)),
+ NULL, ¶ms[0]);
+ }
+
+ Solver::Options options;
+ options.minimizer_progress_to_stdout = false;
+ Solver::Summary summary;
+ Solve(options, &problem, &summary);
+
+ IntermediateResult IR;
+ IR.extrinsics = ExtrinsicParams::get(¶ms[0]);
+ IR.solver_error = summary.final_cost;
+
+ if (verbose) {
+ std::cout << summary.BriefReport() << "\n";
+ std::cout << "y = " << IR.extrinsics.y / kInchesToMeters << ";\n";
+ std::cout << "z = " << IR.extrinsics.z / kInchesToMeters << ";\n";
+ std::cout << "r1 = " << IR.extrinsics.r1 * 180 / M_PI << ";\n";
+ std::cout << "r2 = " << IR.extrinsics.r2 * 180 / M_PI << ";\n";
+ std::cout << "rup = " << intrinsics_.mount_angle * 180 / M_PI << ";\n";
+ std::cout << "fl = " << intrinsics_.focal_length << ";\n";
+ std::cout << "error = " << summary.final_cost << ";\n";
+ }
+ return IR;
+}
+
+} // namespace vision
+} // namespace y2019
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
new file mode 100644
index 0000000..0d53e2c
--- /dev/null
+++ b/y2019/vision/target_sender.cc
@@ -0,0 +1,136 @@
+#include <fstream>
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "aos/vision/blob/codec.h"
+#include "aos/vision/blob/find_blob.h"
+#include "aos/vision/events/socket_types.h"
+#include "aos/vision/events/udp.h"
+#include "aos/vision/image/image_stream.h"
+#include "aos/vision/image/reader.h"
+
+#include "y2019/jevois/serial.h"
+#include "y2019/vision/target_finder.h"
+
+using ::aos::events::DataSocket;
+using ::aos::events::RXUdpSocket;
+using ::aos::events::TCPServer;
+using ::aos::vision::DataRef;
+using ::aos::vision::Int32Codec;
+using ::aos::monotonic_clock;
+using ::y2019::jevois::open_via_terminos;
+using aos::vision::Segment;
+
+class CameraStream : public ::aos::vision::ImageStreamEvent {
+ public:
+ CameraStream(::aos::vision::CameraParams params, const ::std::string &fname)
+ : ImageStreamEvent(fname, params) {}
+
+ void ProcessImage(DataRef data, monotonic_clock::time_point monotonic_now) {
+ LOG(INFO, "got frame: %d\n", (int)data.size());
+
+ static unsigned i = 0;
+
+ /*
+ std::ofstream ofs(std::string("/jevois/data/debug_viewer_jpeg_") +
+ std::to_string(i) + ".yuyv",
+ std::ofstream::out);
+ ofs << data;
+ ofs.close();
+ */
+ if (on_frame) on_frame(data, monotonic_now);
+ ++i;
+
+ if (i == 200) exit(-1);
+ }
+
+ std::function<void(DataRef, monotonic_clock::time_point)> on_frame;
+};
+
+int open_terminos(const char *tty_name) { return open_via_terminos(tty_name); }
+
+std::string GetFileContents(const std::string &filename) {
+ std::ifstream in(filename, std::ios::in | std::ios::binary);
+ if (in) {
+ std::string contents;
+ in.seekg(0, std::ios::end);
+ contents.resize(in.tellg());
+ in.seekg(0, std::ios::beg);
+ in.read(&contents[0], contents.size());
+ in.close();
+ return (contents);
+ }
+ fprintf(stderr, "Could not read file: %s\n", filename.c_str());
+ exit(-1);
+}
+
+int main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int itsDev = open_terminos("/dev/ttyS0");
+ dup2(itsDev, 1);
+ dup2(itsDev, 2);
+
+ TargetFinder finder_;
+
+ // Check that our current results match possible solutions.
+ aos::vision::CameraParams params0;
+ params0.set_exposure(0);
+ params0.set_brightness(40);
+ params0.set_width(640);
+ // params0.set_fps(10);
+ params0.set_height(480);
+
+ ::std::unique_ptr<CameraStream> camera0(
+ new CameraStream(params0, "/dev/video0"));
+ camera0->on_frame = [&](DataRef data,
+ monotonic_clock::time_point /*monotonic_now*/) {
+ aos::vision::ImageFormat fmt{640, 480};
+ aos::vision::BlobList imgs = aos::vision::FindBlobs(
+ aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
+ finder_.PreFilter(&imgs);
+
+ bool verbose = false;
+ std::vector<std::vector<Segment<2>>> raw_polys;
+ for (const RangeImage &blob : imgs) {
+ std::vector<Segment<2>> polygon = finder_.FillPolygon(blob, verbose);
+ if (polygon.empty()) {
+ } else {
+ raw_polys.push_back(polygon);
+ }
+ }
+
+ // Calculate each component side of a possible target.
+ std::vector<TargetComponent> target_component_list =
+ finder_.FillTargetComponentList(raw_polys);
+
+ // Put the compenents together into targets.
+ std::vector<Target> target_list =
+ finder_.FindTargetsFromComponents(target_component_list, verbose);
+
+ // Use the solver to generate an intermediate version of our results.
+ std::vector<IntermediateResult> results;
+ for (const Target &target : target_list) {
+ results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
+ }
+
+ results = finder_.FilterResults(results);
+ };
+
+ aos::events::EpollLoop loop;
+
+ for (int i = 0; i < 100; ++i) {
+ std::this_thread::sleep_for(std::chrono::milliseconds(20));
+ camera0->ReadEvent();
+ }
+
+ // TODO: Fix event loop on jevois:
+ // loop.Add(camera0.get());
+ // loop.Run();
+}
diff --git a/y2019/vision/target_types.h b/y2019/vision/target_types.h
new file mode 100644
index 0000000..557b04a
--- /dev/null
+++ b/y2019/vision/target_types.h
@@ -0,0 +1,128 @@
+#ifndef _Y2019_VISION_TARGET_TYPES_H_
+#define _Y2019_VISION_TARGET_TYPES_H_
+
+#include "aos/vision/math/segment.h"
+#include "aos/vision/math/vector.h"
+
+namespace y2019 {
+namespace vision {
+
+// This polynomial exists in transpose space.
+struct TargetComponent {
+ aos::vision::Vector<2> GetByIndex(size_t i) const {
+ switch (i) {
+ case 0:
+ return top;
+ case 1:
+ return inside;
+ case 2:
+ return bottom;
+ case 3:
+ return outside;
+ default:
+ return aos::vision::Vector<2>();
+ }
+ }
+ bool is_right;
+ aos::vision::Vector<2> top;
+ aos::vision::Vector<2> inside;
+ aos::vision::Vector<2> outside;
+ aos::vision::Vector<2> bottom;
+
+ aos::vision::Segment<2> major_axis;
+};
+
+// Convert back to screen space for final result.
+struct Target {
+ TargetComponent left;
+ TargetComponent right;
+
+ static Target MakeTemplate();
+ // Get the points in some order (will match against the template).
+ std::array<aos::vision::Vector<2>, 8> toPointList() const;
+};
+
+struct IntrinsicParams {
+ static constexpr size_t kNumParams = 2;
+
+ double mount_angle = 10.0481 / 180.0 * M_PI;
+ double focal_length = 729.445;
+
+ void set(double *data) {
+ data[0] = mount_angle;
+ data[1] = focal_length;
+ }
+ static IntrinsicParams get(const double *data) {
+ IntrinsicParams out;
+ out.mount_angle = data[0];
+ out.focal_length = data[1];
+ return out;
+ }
+};
+
+struct ExtrinsicParams {
+ static constexpr size_t kNumParams = 4;
+
+ double y = 18.0 * 0.0254;
+ double z = 23.0 * 0.0254;
+ double r1 = 1.0 / 180 * M_PI;
+ double r2 = -1.0 / 180 * M_PI;
+
+ void set(double *data) {
+ data[0] = y;
+ data[1] = z;
+ data[2] = r1;
+ data[3] = r2;
+ }
+ static ExtrinsicParams get(const double *data) {
+ ExtrinsicParams out;
+ out.y = data[0];
+ out.z = data[1];
+ out.r1 = data[2];
+ out.r2 = data[3];
+ return out;
+ }
+};
+// Projects a point from idealized template space to camera space.
+aos::vision::Vector<2> Project(aos::vision::Vector<2> pt,
+ const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+Target Project(const Target &target, const IntrinsicParams &intrinsics,
+ const ExtrinsicParams &extrinsics);
+
+// An intermediate for of the results.
+// These are actual awnsers, but in a form from the solver that is not ready for
+// the wire.
+struct IntermediateResult {
+ ExtrinsicParams extrinsics;
+
+ // Error from solver calulations.
+ double solver_error;
+};
+
+// Final foramtting ready for output on the wire.
+struct TargetResult {
+ // Distance to the target in meters. Specifically, the distance from the
+ // center of the camera's image plane to the center of the target.
+ float distance;
+ // Height of the target in meters. Specifically, the distance from the floor
+ // to the center of the target.
+ float height;
+
+ // Heading of the center of the target in radians. Zero is straight out
+ // perpendicular to the camera's image plane. Images to the left (looking at a
+ // camera image) are at a positive angle.
+ float heading;
+
+ // The angle between the target and the camera's image plane. This is
+ // projected so both are assumed to be perpendicular to the floor. Parallel
+ // targets have a skew of zero. Targets rotated such that their left edge
+ // (looking at a camera image) is closer are at a positive angle.
+ float skew;
+};
+
+} // namespace vision
+} // namespace y2019
+
+#endif