Merge "Add code for a practice defense bot"
diff --git a/WORKSPACE b/WORKSPACE
index 35715d2..d76d359 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -623,13 +623,13 @@
)
# Downloaded from
-# https://developer.arm.com/-/media/Files/downloads/gnu-rm/7-2018q2/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2?revision=bc2c96c0-14b5-4bb4-9f18-bceb4050fee7?product=GNU%20Arm%20Embedded%20Toolchain,64-bit,,Linux,7-2018-q2-update
+# From https://developer.arm.com/downloads/-/arm-gnu-toolchain-downloads
http_archive(
name = "gcc_arm_none_eabi",
build_file = "@//:compilers/gcc_arm_none_eabi.BUILD",
- sha256 = "bb17109f0ee697254a5d4ae6e5e01440e3ea8f0277f2e8169bf95d07c7d5fe69",
- strip_prefix = "gcc-arm-none-eabi-7-2018-q2-update/",
- url = "https://software.frc971.org/Build-Dependencies/gcc-arm-none-eabi-7-2018-q2-update-linux.tar.bz2",
+ sha256 = "6cd1bbc1d9ae57312bcd169ae283153a9572bd6a8e4eeae2fedfbc33b115fdbb",
+ strip_prefix = "arm-gnu-toolchain-13.2.Rel1-x86_64-arm-none-eabi",
+ url = "https://developer.arm.com/-/media/Files/downloads/gnu/13.2.rel1/binrel/arm-gnu-toolchain-13.2.rel1-x86_64-arm-none-eabi.tar.xz",
)
# Java11 JDK.
diff --git a/aos/configuration.cc b/aos/configuration.cc
index 64804e5..6cae0d4 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -949,10 +949,10 @@
VLOG(1) << "No match for { \"name\": \"" << name << "\", \"type\": \""
<< type << "\" }";
if (original_name != name && !quiet) {
- LOG(WARNING) << "Remapped from {\"name\": \"" << original_name
- << "\", \"type\": \"" << type << "\"}, to {\"name\": \""
- << name << "\", \"type\": \"" << type
- << "\"}, but no channel by that name exists.";
+ VLOG(1) << "Remapped from {\"name\": \"" << original_name
+ << "\", \"type\": \"" << type << "\"}, to {\"name\": \"" << name
+ << "\", \"type\": \"" << type
+ << "\"}, but no channel by that name exists.";
}
return nullptr;
}
diff --git a/aos/containers/BUILD b/aos/containers/BUILD
index ee1bf98..c0b365c 100644
--- a/aos/containers/BUILD
+++ b/aos/containers/BUILD
@@ -42,6 +42,29 @@
)
cc_library(
+ name = "inlined_vector",
+ hdrs = [
+ "inlined_vector.h",
+ ],
+ deps = [
+ "@com_google_absl//absl/container:inlined_vector",
+ ],
+)
+
+cc_test(
+ name = "inlined_vector_test",
+ srcs = [
+ "inlined_vector_test.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":inlined_vector",
+ "//aos:realtime",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "sized_array",
hdrs = [
"sized_array.h",
diff --git a/aos/containers/inlined_vector.h b/aos/containers/inlined_vector.h
new file mode 100644
index 0000000..abd375d
--- /dev/null
+++ b/aos/containers/inlined_vector.h
@@ -0,0 +1,18 @@
+#ifndef AOS_CONTAINERS_INLINED_VECTOR_H_
+#define AOS_CONTAINERS_INLINED_VECTOR_H_
+
+#include <vector>
+
+#include "absl/container/inlined_vector.h"
+
+namespace aos {
+
+template <typename T, size_t N>
+struct InlinedVector : public absl::InlinedVector<T, N> {};
+
+// Specialized for the N == 0 case because absl::InlinedVector doesn't support
+// it for some reason.
+template <typename T>
+struct InlinedVector<T, 0> : public std::vector<T> {};
+} // namespace aos
+#endif // AOS_CONTAINERS_INLINED_VECTOR_H_
diff --git a/aos/containers/inlined_vector_test.cc b/aos/containers/inlined_vector_test.cc
new file mode 100644
index 0000000..7368844
--- /dev/null
+++ b/aos/containers/inlined_vector_test.cc
@@ -0,0 +1,49 @@
+#include "aos/containers/inlined_vector.h"
+
+#include "gtest/gtest.h"
+
+#include "aos/realtime.h"
+
+DECLARE_bool(die_on_malloc);
+
+namespace aos {
+namespace testing {
+
+// Checks that we don't malloc until/unless we need to increase the size of the
+// vector.
+TEST(SizedArrayTest, NoUnnecessaryMalloc) {
+ gflags::FlagSaver flag_saver;
+ FLAGS_die_on_malloc = true;
+ RegisterMallocHook();
+ InlinedVector<int, 5> a;
+ {
+ aos::ScopedRealtime realtime;
+ a.push_back(9);
+ a.push_back(7);
+ a.push_back(1);
+ a.push_back(2);
+ a.push_back(3);
+
+ // And double-check that we can actually construct a new object at realtime.
+ InlinedVector<int, 5> b;
+ }
+// Malloc hooks don't work with asan/msan.
+#if !__has_feature(address_sanitizer) && !__has_feature(memory_sanitizer)
+ EXPECT_DEATH(
+ {
+ aos::ScopedRealtime realtime;
+ a.push_back(4);
+ },
+ "Malloced");
+#endif
+}
+
+// Tests that we can create/define a vector with zero statically allocated
+// elements (the absl::InlinedVector does not allow this for some reason).
+TEST(SizedArrayTest, ZeroLengthVector) {
+ InlinedVector<int, 0> zero;
+ zero.push_back(1);
+ ASSERT_EQ(1, zero[0]);
+}
+} // namespace testing
+} // namespace aos
diff --git a/aos/events/epoll.cc b/aos/events/epoll.cc
index 9a11197..340d0cd 100644
--- a/aos/events/epoll.cc
+++ b/aos/events/epoll.cc
@@ -2,10 +2,13 @@
#include <fcntl.h>
#include <sys/epoll.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
#include <sys/timerfd.h>
#include <unistd.h>
#include <atomic>
+#include <cstring>
#include <vector>
#include "glog/logging.h"
@@ -203,17 +206,47 @@
LOG(FATAL) << "fd " << fd << " not found";
}
+namespace {
+bool IsSocket(int fd) {
+ struct stat st;
+ if (fstat(fd, &st) == -1) {
+ return false;
+ }
+ return static_cast<bool>(S_ISSOCK(st.st_mode));
+}
+
+::std::string GetSocketErrorStr(int fd) {
+ ::std::string error_str;
+ if (IsSocket(fd)) {
+ int error = 0;
+ socklen_t errlen = sizeof(error);
+ if (getsockopt(fd, SOL_SOCKET, SO_ERROR, (void *)&error, &errlen) == 0) {
+ if (error) {
+ error_str = "Socket error: " + ::std::string(strerror(error));
+ }
+ }
+ }
+ return error_str;
+}
+} // namespace
+
void EPoll::InOutEventData::DoCallbacks(uint32_t events) {
if (events & kInEvents) {
- CHECK(in_fn) << ": No handler registered for input events on " << fd;
+ CHECK(in_fn) << ": No handler registered for input events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec;
in_fn();
}
if (events & kOutEvents) {
- CHECK(out_fn) << ": No handler registered for output events on " << fd;
+ CHECK(out_fn) << ": No handler registered for output events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec;
out_fn();
}
if (events & kErrorEvents) {
- CHECK(err_fn) << ": No handler registered for error events on " << fd;
+ CHECK(err_fn) << ": No handler registered for error events on descriptor "
+ << fd << ". Received events = 0x" << std::hex << events
+ << std::dec << ". " << GetSocketErrorStr(fd);
err_fn();
}
}
diff --git a/aos/events/event_loop.h b/aos/events/event_loop.h
index 827dd46..f57053f 100644
--- a/aos/events/event_loop.h
+++ b/aos/events/event_loop.h
@@ -818,6 +818,10 @@
// provided string_view.
void SetVersionString(std::string_view version);
+ std::optional<std::string_view> VersionString() const {
+ return version_string_;
+ }
+
protected:
// Sets the name of the event loop. This is the application name.
virtual void set_name(const std::string_view name) = 0;
diff --git a/aos/events/logging/multinode_logger_test_lib.h b/aos/events/logging/multinode_logger_test_lib.h
index d6d3f60..f5fb4cf 100644
--- a/aos/events/logging/multinode_logger_test_lib.h
+++ b/aos/events/logging/multinode_logger_test_lib.h
@@ -78,13 +78,13 @@
};
constexpr std::string_view kCombinedConfigSha1() {
- return "8f8a0fd505b5cd8ffc553b2af2796232450a2db88c6db974c5e3406b38eecb93";
+ return "32514f3a686e5f8936cc4651e7c81350112f7be8d80dcb8d4afaa29d233c5619";
}
constexpr std::string_view kSplitConfigSha1() {
- return "0a2443f8fe49815de16723c48881f3a78c4e6399273d15da2d77d35588c8cae8";
+ return "416da222c09d83325c6f453591d34c7ef12c12c2dd129ddeea657c4bec61b7fd";
}
constexpr std::string_view kReloggedSplitConfigSha1() {
- return "5e4f4784b890c1fdca7aeeadad8bd0354325f3ed8f11dbc9a81112df87407931";
+ return "3fe428684a38298d3323ef087f44517574da3f07dd84b3740829156d6d870108";
}
LoggerState MakeLoggerState(NodeEventLoopFactory *node,
diff --git a/aos/starter/starter_demo.py b/aos/starter/starter_demo.py
index 89d06a0..12d8775 100755
--- a/aos/starter/starter_demo.py
+++ b/aos/starter/starter_demo.py
@@ -48,5 +48,5 @@
os.makedirs(os.path.dirname(destination), exist_ok=True)
shutil.copy(config, destination)
shutil.copy(config, f"{tmpdir}/aos_config.{suffix}")
- args = [tmpdir + "/starterd"]
+ args = [tmpdir + "/starterd", "--version_string=starter_version"]
subprocess.run(args, check=True, cwd=tmpdir)
diff --git a/aos/starter/starterd.cc b/aos/starter/starterd.cc
index d6caf3f..1ac4514 100644
--- a/aos/starter/starterd.cc
+++ b/aos/starter/starterd.cc
@@ -10,6 +10,8 @@
DEFINE_string(config, "aos_config.json", "File path of aos configuration");
DEFINE_string(user, "",
"Starter runs as though this user ran a SUID binary if set.");
+DEFINE_string(version_string, "",
+ "Version to report for starterd and subprocesses.");
DECLARE_string(shm_base);
DEFINE_bool(purge_shm_base, false,
@@ -58,6 +60,9 @@
const aos::Configuration *config_msg = &config.message();
aos::starter::Starter starter(config_msg);
+ if (!FLAGS_version_string.empty()) {
+ starter.event_loop()->SetVersionString(FLAGS_version_string);
+ }
starter.Run();
diff --git a/aos/starter/subprocess.cc b/aos/starter/subprocess.cc
index b6a20f0..9606adc 100644
--- a/aos/starter/subprocess.cc
+++ b/aos/starter/subprocess.cc
@@ -725,17 +725,22 @@
stop_reason_ = static_cast<aos::starter::LastStopReason>(*read_result);
}
+ const std::string starter_version_string =
+ absl::StrCat("starter version '",
+ event_loop_->VersionString().value_or("unknown"), "'");
switch (status_) {
case aos::starter::State::STARTING: {
if (exit_code_.value() == 0) {
LOG_IF(INFO, quiet_flag_ == QuietLogging::kNo)
<< "Application '" << name_ << "' pid " << pid_
- << " exited with status " << exit_code_.value();
+ << " exited with status " << exit_code_.value() << " and "
+ << starter_version_string;
} else {
LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo ||
quiet_flag_ == QuietLogging::kNotForDebugging)
<< "Failed to start '" << name_ << "' on pid " << pid_
- << " : Exited with status " << exit_code_.value();
+ << " : Exited with status " << exit_code_.value() << " and "
+ << starter_version_string;
}
if (autorestart()) {
QueueStart();
@@ -753,13 +758,13 @@
} else {
if (quiet_flag_ == QuietLogging::kNo ||
quiet_flag_ == QuietLogging::kNotForDebugging) {
- std::string version_string =
+ const std::string version_string =
latest_timing_report_version_.has_value()
- ? absl::StrCat("'", latest_timing_report_version_.value(),
- "'")
- : "unknown";
+ ? absl::StrCat("version '",
+ latest_timing_report_version_.value(), "'")
+ : starter_version_string;
LOG_IF(WARNING, quiet_flag_ == QuietLogging::kNo)
- << "Application '" << name_ << "' pid " << pid_ << " version "
+ << "Application '" << name_ << "' pid " << pid_ << " "
<< version_string << " exited unexpectedly with status "
<< exit_code_.value();
}
diff --git a/aos/starter/subprocess_test.cc b/aos/starter/subprocess_test.cc
index 12f7f6d..fcc1128 100644
--- a/aos/starter/subprocess_test.cc
+++ b/aos/starter/subprocess_test.cc
@@ -105,6 +105,89 @@
ASSERT_EQ(aos::starter::State::STOPPED, echo_stderr.status());
}
+// Checks that when a child application crashing results in the starter printing
+// out its own version by default.
+TEST_F(SubprocessTest, PrintNoTimingReportVersionString) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+
+ ::testing::internal::CaptureStderr();
+
+ // Set up application without quiet flag active
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SetVersionString("version_string");
+ bool observed_stopped = false;
+ Application error_out(
+ "false", "bash", &event_loop,
+ [&observed_stopped, &error_out]() {
+ if (error_out.status() == aos::starter::State::STOPPED) {
+ observed_stopped = true;
+ }
+ },
+ Application::QuietLogging::kNo);
+ error_out.set_args({"-c", "sleep 3; false"});
+
+ error_out.Start();
+ aos::TimerHandler *exit_timer =
+ event_loop.AddTimer([&event_loop]() { event_loop.Exit(); });
+ event_loop.OnRun([&event_loop, exit_timer]() {
+ exit_timer->Schedule(event_loop.monotonic_now() +
+ std::chrono::milliseconds(5000));
+ });
+
+ event_loop.Run();
+
+ // Ensure presence of logs without quiet flag
+ std::string output = ::testing::internal::GetCapturedStderr();
+ std::string expected = "starter version 'version_string'";
+
+ ASSERT_TRUE(output.find(expected) != std::string::npos) << output;
+ EXPECT_TRUE(observed_stopped);
+ EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
+}
+
+TEST_F(SubprocessTest, PrintFailedToStartVersionString) {
+ const std::string config_file =
+ ::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
+
+ ::testing::internal::CaptureStderr();
+
+ // Set up application without quiet flag active
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(config_file);
+ aos::ShmEventLoop event_loop(&config.message());
+ event_loop.SetVersionString("version_string");
+ bool observed_stopped = false;
+ Application error_out(
+ "false", "false", &event_loop,
+ [&observed_stopped, &error_out]() {
+ if (error_out.status() == aos::starter::State::STOPPED) {
+ observed_stopped = true;
+ }
+ },
+ Application::QuietLogging::kNo);
+
+ error_out.Start();
+ aos::TimerHandler *exit_timer =
+ event_loop.AddTimer([&event_loop]() { event_loop.Exit(); });
+ event_loop.OnRun([&event_loop, exit_timer]() {
+ exit_timer->Schedule(event_loop.monotonic_now() +
+ std::chrono::milliseconds(1500));
+ });
+
+ event_loop.Run();
+
+ // Ensure presence of logs without quiet flag
+ std::string output = ::testing::internal::GetCapturedStderr();
+ std::string expected = "starter version 'version_string'";
+
+ ASSERT_TRUE(output.find(expected) != std::string::npos) << output;
+ EXPECT_TRUE(observed_stopped);
+ EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
+}
+
TEST_F(SubprocessTest, UnactiveQuietFlag) {
const std::string config_file =
::aos::testing::ArtifactPath("aos/events/pingpong_config.json");
@@ -142,7 +225,8 @@
std::string expectedRun = "exited unexpectedly with status";
ASSERT_TRUE(output.find(expectedStart) != std::string::npos ||
- output.find(expectedRun) != std::string::npos);
+ output.find(expectedRun) != std::string::npos)
+ << output;
EXPECT_TRUE(observed_stopped);
EXPECT_EQ(aos::starter::State::STOPPED, error_out.status());
}
diff --git a/build_tests/BUILD b/build_tests/BUILD
index c097726..f0ff94d 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -213,6 +213,7 @@
srcs = [
"upstream_python_test.py",
],
+ target_compatible_with = ["@platforms//os:linux"],
deps = [
"@pip//matplotlib",
"@pip//numpy",
diff --git a/compilers/gcc_arm_none_eabi.BUILD b/compilers/gcc_arm_none_eabi.BUILD
index db2cc77..ba2462a 100644
--- a/compilers/gcc_arm_none_eabi.BUILD
+++ b/compilers/gcc_arm_none_eabi.BUILD
@@ -4,6 +4,7 @@
name = "gcc",
srcs = [
"bin/arm-none-eabi-gcc",
+ ":compiler_pieces",
],
)
@@ -61,6 +62,7 @@
srcs = glob([
"arm-none-eabi/**",
"lib/gcc/arm-none-eabi/**",
+ "libexec/gcc/arm-none-eabi/**",
"include/**",
]),
)
diff --git a/frc971/amd64/build_rootfs.py b/frc971/amd64/build_rootfs.py
index cc0138e..49735c8 100755
--- a/frc971/amd64/build_rootfs.py
+++ b/frc971/amd64/build_rootfs.py
@@ -165,6 +165,10 @@
"--exclude=./usr/bin/X11",
"--exclude=./usr/lib/systemd/system/system-systemd*cryptsetup.slice",
"--exclude=./dev",
+ "--exclude=./usr/include/cub",
+ "--exclude=./usr/include/nv",
+ "--exclude=./usr/include/thrust",
+ "--exclude=./usr/include/cuda",
"-cf",
tarball,
".",
diff --git a/frc971/control_loops/BUILD b/frc971/control_loops/BUILD
index 31b245b..c03a964 100644
--- a/frc971/control_loops/BUILD
+++ b/frc971/control_loops/BUILD
@@ -199,6 +199,13 @@
],
)
+flatbuffer_ts_library(
+ name = "can_falcon_ts_fbs",
+ srcs = [
+ "can_falcon.fbs",
+ ],
+)
+
flatbuffer_cc_library(
name = "can_falcon_fbs",
srcs = [
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index fdbc1dc..e631799 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -71,6 +71,19 @@
deps = ["//frc971/control_loops:control_loops_ts_fbs"],
)
+flatbuffer_ts_library(
+ name = "drivetrain_position_ts_fbs",
+ srcs = ["drivetrain_position.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+flatbuffer_ts_library(
+ name = "drivetrain_can_position_ts_fbs",
+ srcs = ["drivetrain_can_position.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = ["//frc971/control_loops:can_falcon_ts_fbs"],
+)
+
genrule(
name = "drivetrain_goal_float_fbs_generated",
srcs = ["drivetrain_goal.fbs"],
diff --git a/frc971/control_loops/hybrid_state_feedback_loop.h b/frc971/control_loops/hybrid_state_feedback_loop.h
index 79e7da7..3732266 100644
--- a/frc971/control_loops/hybrid_state_feedback_loop.h
+++ b/frc971/control_loops/hybrid_state_feedback_loop.h
@@ -86,12 +86,12 @@
StateFeedbackHybridPlant(StateFeedbackHybridPlant &&other)
: index_(other.index_) {
- ::std::swap(coefficients_, other.coefficients_);
- X_.swap(other.X_);
- Y_.swap(other.Y_);
- A_.swap(other.A_);
- B_.swap(other.B_);
- DelayedU_.swap(other.DelayedU_);
+ coefficients_ = ::std::move(other.coefficients_);
+ X_ = ::std::move(other.X_);
+ Y_ = ::std::move(other.Y_);
+ A_ = ::std::move(other.A_);
+ B_ = ::std::move(other.B_);
+ DelayedU_ = ::std::move(other.DelayedU_);
}
virtual ~StateFeedbackHybridPlant() {}
@@ -290,12 +290,11 @@
}
HybridKalman(HybridKalman &&other) : index_(other.index_) {
- ::std::swap(coefficients_, other.coefficients_);
-
- X_hat_.swap(other.X_hat_);
- P_.swap(other.P_);
- Q_.swap(other.Q_);
- R_.swap(other.R_);
+ coefficients_ = ::std::move(other.coefficients_);
+ X_hat_ = ::std::move(other.X_hat_);
+ P_ = ::std::move(other.P_);
+ Q_ = ::std::move(other.Q_);
+ R_ = ::std::move(other.R_);
}
// Getters for Q
diff --git a/frc971/orin/BUILD b/frc971/orin/BUILD
index a81abf1..61d9b8e 100644
--- a/frc971/orin/BUILD
+++ b/frc971/orin/BUILD
@@ -8,6 +8,14 @@
visibility = ["//visibility:public"],
)
+halide_library(
+ name = "ycbcr422",
+ src = "crcv_generator.cc",
+ args = "rows=1088 cols=1456 ystride=1536 cbcrstride=1536",
+ function = "ycbcr422",
+ visibility = ["//visibility:public"],
+)
+
cc_library(
name = "cuda",
srcs = [
@@ -24,6 +32,7 @@
"labeling_allegretti_2019_BKE.h",
"points.h",
"threshold.h",
+ "transform_output_iterator.h",
],
copts = [
"-Wno-pass-failed",
@@ -65,3 +74,51 @@
"//y2023/vision:ToGreyscaleAndDecimateHalide",
],
)
+
+cc_test(
+ name = "output_iterator_test",
+ srcs = [
+ "output_iterator_test.cc",
+ ],
+ features = ["cuda"],
+ deps = [
+ ":cuda",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//third_party:cudart",
+ ],
+)
+
+cc_test(
+ name = "points_test",
+ srcs = [
+ "points_test.cc",
+ ],
+ features = ["cuda"],
+ deps = [
+ ":cuda",
+ "//aos/testing:googletest",
+ "//aos/testing:random_seed",
+ "//third_party:cudart",
+ ],
+)
+
+cc_binary(
+ name = "argus_camera",
+ srcs = [
+ "argus_camera.cc",
+ ],
+ target_compatible_with = ["@platforms//cpu:arm64"],
+ deps = [
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//aos/util:file",
+ "//frc971/orin:ycbcr",
+ "//frc971/orin:ycbcr422",
+ "//frc971/vision:vision_fbs",
+ "@arm64_debian_sysroot//:argus",
+ "@arm64_debian_sysroot//:egl",
+ "@arm64_debian_sysroot//:eglstream",
+ "@arm64_debian_sysroot//:nvbufsurface",
+ ],
+)
diff --git a/frc971/orin/README.md b/frc971/orin/README.md
new file mode 100644
index 0000000..1b16ba3
--- /dev/null
+++ b/frc971/orin/README.md
@@ -0,0 +1,26 @@
+# Building the rootfs for the Orin NX 8g
+
+Our code targets Debian Bookworm. Debian support for the Orin is poor, but
+yocto support is pretty good. So, rather than try to recreate what Yocto has,
+we instead install the .deb's that Yocto generates for the NVIDIA libraries
+and install them on a bookworm sysroot. To do this:
+
+Check out and follow the instructions on https://github.com/frc971/meta-frc971
+to create a working image for the Orin. You will need this to install.
+
+Then, build a Bookworm image.
+
+```
+./build_rootfs.py
+```
+
+You'll likely have to point that to your yocto build unless your username is
+austin.
+
+From there, copy the resulting image to `./frc971-image-orin-nx-8g.tegraflash.tar.gz`
+
+Then, hold the bootloader switch, reboot the Orin, and then run the install
+script to install it on your orin.
+```
+doflash_frc971.sh
+```
diff --git a/frc971/orin/apriltag.cc b/frc971/orin/apriltag.cc
index 381ffae..ff8ccec 100644
--- a/frc971/orin/apriltag.cc
+++ b/frc971/orin/apriltag.cc
@@ -19,6 +19,7 @@
#include "aos/time/time.h"
#include "frc971/orin/labeling_allegretti_2019_BKE.h"
#include "frc971/orin/threshold.h"
+#include "frc971/orin/transform_output_iterator.h"
namespace frc971 {
namespace apriltag {
@@ -315,23 +316,32 @@
__host__ __device__ __forceinline__ IndexPoint
operator()(cub::KeyValuePair<long, QuadBoundaryPoint> pt) const {
size_t index = blob_finder_.FindBlobIndex(pt.key);
- MinMaxExtents extents = blob_finder_.Get(index);
IndexPoint result(index, pt.value.point_bits());
-
- float theta =
- (atan2f(pt.value.y() - extents.cy(), pt.value.x() - extents.cx()) +
- M_PI) *
- 8e6;
- long long int theta_int = llrintf(theta);
-
- result.set_theta(std::max<long long int>(0, theta_int));
-
return result;
}
BlobExtentsIndexFinder blob_finder_;
};
+// Calculates Theta for a given IndexPoint
+class AddThetaToIndexPoint {
+ public:
+ AddThetaToIndexPoint(MinMaxExtents *extents_device, size_t num_extents)
+ : blob_finder_(extents_device, num_extents) {}
+ __host__ __device__ __forceinline__ IndexPoint operator()(IndexPoint a) {
+ MinMaxExtents extents = blob_finder_.Get(a.blob_index());
+ float theta =
+ (atan2f(a.y() - extents.cy(), a.x() - extents.cx()) + M_PI) * 8e6;
+ long long int theta_int = llrintf(theta);
+
+ a.set_theta(std::max<long long int>(0, theta_int));
+ return a;
+ }
+
+ private:
+ BlobExtentsIndexFinder blob_finder_;
+};
+
// TODO(austin): Make something which rewrites points on the way back out to
// memory and adds the slope.
@@ -756,10 +766,16 @@
cub::ArgIndexInputIterator<QuadBoundaryPoint *> value_index_input_iterator(
sorted_union_marker_pair_device_.get());
RewriteToIndexPoint rewrite(extents_device_.get(), num_quads_host);
+
cub::TransformInputIterator<IndexPoint, RewriteToIndexPoint,
cub::ArgIndexInputIterator<QuadBoundaryPoint *>>
input_iterator(value_index_input_iterator, rewrite);
+ AddThetaToIndexPoint add_theta(extents_device_.get(), num_quads_host);
+
+ TransformOutputIterator<IndexPoint, IndexPoint, AddThetaToIndexPoint>
+ output_iterator(selected_blobs_device_.get(), add_theta);
+
SelectBlobs select_blobs(
extents_device_.get(), reduced_dot_blobs_pair_device_.get(),
min_tag_width_, reversed_border_, normal_border_,
@@ -767,12 +783,12 @@
size_t temp_storage_bytes =
temp_storage_compressed_filtered_blobs_device_.size();
+
CHECK_CUDA(cub::DeviceSelect::If(
temp_storage_compressed_filtered_blobs_device_.get(),
- temp_storage_bytes, input_iterator, selected_blobs_device_.get(),
+ temp_storage_bytes, input_iterator, output_iterator,
num_selected_blobs_device_.get(), num_compressed_union_marker_pair_host,
select_blobs, stream_.get()));
-
MaybeCheckAndSynchronize();
num_selected_blobs_device_.MemcpyTo(&num_selected_blobs_host);
diff --git a/frc971/orin/argus_camera.cc b/frc971/orin/argus_camera.cc
new file mode 100644
index 0000000..ca3d34a
--- /dev/null
+++ b/frc971/orin/argus_camera.cc
@@ -0,0 +1,625 @@
+#include <chrono>
+#include <filesystem>
+#include <thread>
+
+#include "glog/logging.h"
+
+#include "Argus/Argus.h"
+#include "Argus/EGLStream.h"
+#include "Argus/Types.h"
+#include "Argus/utils/Error.h"
+#include "EGLStream/FrameConsumer.h"
+#include "EGLStream/Image.h"
+#include "EGLStream/NV/ImageNativeBuffer.h"
+#include "HalideBuffer.h"
+#include "HalideRuntime.h"
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/time/time.h"
+#include "aos/util/file.h"
+#include "frc971/orin/ycbcr.h"
+#include "frc971/orin/ycbcr422.h"
+#include "frc971/vision/vision_generated.h"
+#include "nvbufsurface.h"
+
+DEFINE_string(config, "aos_config.json", "Path to the config file to use.");
+
+DEFINE_int32(colorformat, NVBUF_COLOR_FORMAT_NV16,
+ "Mode to use. Don't change unless you know what you are doing.");
+DEFINE_int32(camera, 0, "Camera number");
+DEFINE_int32(mode, 0, "Mode number to use.");
+DEFINE_int32(exposure, 200000, "Exposure number to use.");
+DEFINE_int32(gain, 5, "gain number to use.");
+DEFINE_int32(width, 1456, "Image width");
+DEFINE_int32(height, 1088, "Image height");
+DEFINE_double(rgain, 1.0, "R gain");
+DEFINE_double(g1gain, 1.0, "G gain");
+DEFINE_double(g2gain, 1.0, "G gain");
+DEFINE_double(bgain, 1.0, "B gain");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+
+namespace frc971 {
+
+namespace chrono = std::chrono;
+
+// Converts a multiplanar 422 image into a single plane 422 image at the
+// provided memory location sutable for putting in a flatbuffer.
+void YCbCr422(NvBufSurface *nvbuf_surf, uint8_t *data_pointer) {
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.width[0],
+ nvbuf_surf->surfaceList->planeParams.width[1] * 2);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.height[0],
+ nvbuf_surf->surfaceList->planeParams.height[1]);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.pitch[0], 0x600u);
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.pitch[1], 0x600u);
+ std::array<halide_dimension_t, 2> y_dimensions{{
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0]),
+ /*.stride =*/1,
+ /*.flags =*/0,
+ },
+ {
+ /*.min = */ 0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[0]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.pitch[0]),
+ /*.flags =*/0,
+ },
+ }};
+
+ Halide::Runtime::Buffer<uint8_t, 2> y(
+ reinterpret_cast<uint8_t *>(nvbuf_surf->surfaceList->mappedAddr.addr[0]),
+ y_dimensions.size(), y_dimensions.data());
+
+ std::array<halide_dimension_t, 3> cbcr_dimensions{
+ {{
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[1]),
+ /*.stride =*/2,
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[1]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.pitch[1]),
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/2,
+ /*.stride =*/1,
+ /*.flags =*/0,
+ }}};
+
+ Halide::Runtime::Buffer<uint8_t, 3> cbcr(
+ reinterpret_cast<uint8_t *>(nvbuf_surf->surfaceList->mappedAddr.addr[1]),
+ cbcr_dimensions.size(), cbcr_dimensions.data());
+
+ std::array<halide_dimension_t, 3> output_dimensions{
+ {{
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0]),
+ /*.stride =*/2,
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.height[0]),
+ /*.stride =*/
+ static_cast<int32_t>(nvbuf_surf->surfaceList->planeParams.width[0] *
+ 2),
+ /*.flags =*/0,
+ },
+ {
+ /*.min =*/0,
+ /*.extent =*/2,
+ /*.stride =*/1,
+ /*.flags =*/0,
+ }}};
+
+ Halide::Runtime::Buffer<uint8_t, 3> output(
+ data_pointer, output_dimensions.size(), output_dimensions.data());
+ ycbcr422(y, cbcr, output);
+}
+
+// Helper class to tie a NvBufSurface to an Argus::Buffer.
+class DmaBuffer {
+ public:
+ // Creates a DmaBuffer. This is a static method so we can make sure it ends
+ // up as a unique_ptr so the pointer value doesn't change and break all the
+ // links.
+ static std::unique_ptr<DmaBuffer> Create(
+ const Argus::Size2D<uint32_t> &size, NvBufSurfaceColorFormat color_format,
+ NvBufSurfaceLayout layout = NVBUF_LAYOUT_PITCH) {
+ std::unique_ptr<DmaBuffer> buffer(new DmaBuffer());
+
+ NvBufSurfaceAllocateParams params;
+
+ params.memtag = NvBufSurfaceTag_CAMERA;
+ params.params.width = size.width();
+ params.params.height = size.height();
+ params.params.colorFormat = color_format;
+ params.params.layout = layout;
+ params.params.isContiguous = true;
+ params.disablePitchPadding = true;
+ params.params.memType = NVBUF_MEM_SURFACE_ARRAY;
+
+ NvBufSurface *nvbuf_surf = 0;
+ CHECK_EQ(NvBufSurfaceAllocate(&nvbuf_surf, 1, ¶ms), 0);
+ buffer->fd_ = nvbuf_surf->surfaceList[0].bufferDesc;
+
+ return buffer;
+ }
+
+ // Extracts the DmaBuffer from the Argus::Buffer.
+ static DmaBuffer *FromArgusBuffer(Argus::Buffer *buffer) {
+ Argus::IBuffer *i_buffer = Argus::interface_cast<Argus::IBuffer>(buffer);
+ const DmaBuffer *dmabuf =
+ static_cast<const DmaBuffer *>(i_buffer->getClientData());
+
+ return const_cast<DmaBuffer *>(dmabuf);
+ }
+
+ // Returns the DMA buffer handle.
+ int fd() const { return fd_; }
+
+ // Sets and gets the Argus::Buffer pointer.
+ void set_argus_buffer(Argus::Buffer *buffer) { buffer_ = buffer; }
+ Argus::Buffer *get_argus_buffer() const { return buffer_; }
+
+ virtual ~DmaBuffer() {
+ if (fd_ >= 0) {
+ NvBufSurface *nvbuf_surf = 0;
+ NvBufSurfaceFromFd(fd_, (void **)(&nvbuf_surf));
+ if (nvbuf_surf != NULL) {
+ NvBufSurfaceDestroy(nvbuf_surf);
+ }
+ }
+ }
+
+ private:
+ // Private to force people to use Create() above.
+ DmaBuffer() {}
+
+ int fd_ = -1;
+ Argus::Buffer *buffer_ = nullptr;
+};
+
+int Main() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ event_loop.SetRuntimeRealtimePriority(55);
+
+ aos::Sender<frc971::vision::CameraImage> sender =
+ event_loop.MakeSender<frc971::vision::CameraImage>(FLAGS_channel);
+
+ LOG(INFO) << "Started";
+ // Initialize the Argus camera provider.
+ Argus::UniqueObj<Argus::CameraProvider> camera_provider;
+ camera_provider =
+ Argus::UniqueObj<Argus::CameraProvider>(Argus::CameraProvider::create());
+
+ // Get the ICameraProvider interface from the global CameraProvider
+ Argus::ICameraProvider *i_camera_provider =
+ Argus::interface_cast<Argus::ICameraProvider>(camera_provider);
+ if (!i_camera_provider) {
+ ORIGINATE_ERROR("Failed to get ICameraProvider interface");
+ }
+
+ // Get the camera devices.
+ std::vector<Argus::CameraDevice *> camera_devices;
+ i_camera_provider->getCameraDevices(&camera_devices);
+ if (camera_devices.size() == 0) {
+ ORIGINATE_ERROR("there are %d cameras", (unsigned)camera_devices.size());
+ }
+
+ LOG(INFO) << "Found " << camera_devices.size() << " cameras";
+ for (Argus::CameraDevice *camera : camera_devices) {
+ Argus::ICameraProperties *iCameraProperties =
+ Argus::interface_cast<Argus::ICameraProperties>(camera);
+ LOG(INFO) << "Camera " << iCameraProperties->getModelName();
+ }
+
+ std::vector<Argus::SensorMode *> sensor_modes;
+ Argus::ICameraProperties *iCameraProperties =
+ Argus::interface_cast<Argus::ICameraProperties>(
+ camera_devices[FLAGS_camera]);
+ if (!iCameraProperties)
+ ORIGINATE_ERROR("Failed to get ICameraProperties Interface");
+ // Get available Sensor Modes
+ iCameraProperties->getAllSensorModes(&sensor_modes);
+ LOG(INFO) << "Found " << sensor_modes.size() << " modes";
+
+ for (Argus::SensorMode *mode : sensor_modes) {
+ Argus::ISensorMode *imode = Argus::interface_cast<Argus::ISensorMode>(mode);
+ LOG(INFO) << imode->getResolution().width() << " x "
+ << imode->getResolution().height();
+ LOG(INFO) << "type " << imode->getSensorModeType().getName();
+ LOG(INFO) << "exposure min " << imode->getExposureTimeRange().min();
+ LOG(INFO) << "exposure max " << imode->getExposureTimeRange().max();
+ }
+ if (sensor_modes.size() <= 0) {
+ ORIGINATE_ERROR("Preview Sensor Mode %d not available", 0);
+ }
+
+ Argus::ISensorMode *i_sensor_mode =
+ Argus::interface_cast<Argus::ISensorMode>(sensor_modes[FLAGS_mode]);
+ if (!i_sensor_mode) {
+ ORIGINATE_ERROR("Failed to get SensorMode interface");
+ }
+
+ {
+ auto range = i_sensor_mode->getFrameDurationRange();
+ LOG(INFO) << "Min: " << range.min() << ", " << range.max();
+ LOG(INFO) << "type " << i_sensor_mode->getSensorModeType().getName();
+ }
+
+ // Create the capture session using the first device and get the core
+ // interface.
+ Argus::UniqueObj<Argus::CaptureSession> capture_session;
+ capture_session.reset(
+ i_camera_provider->createCaptureSession(camera_devices[FLAGS_camera]));
+ Argus::ICaptureSession *i_capture_session =
+ Argus::interface_cast<Argus::ICaptureSession>(capture_session);
+ if (!i_capture_session) {
+ ORIGINATE_ERROR("Failed to create CaptureSession");
+ }
+
+ EGLDisplay egl_display = eglGetDisplay(EGL_DEFAULT_DISPLAY);
+ CHECK_NE(egl_display, EGL_NO_DISPLAY) << ": Failed to open display";
+
+ // Create the OutputStream.
+ Argus::UniqueObj<Argus::OutputStreamSettings> stream_settings(
+ i_capture_session->createOutputStreamSettings(Argus::STREAM_TYPE_BUFFER));
+
+ Argus::IBufferOutputStreamSettings *i_buffer_output_stream_settings =
+ Argus::interface_cast<Argus::IBufferOutputStreamSettings>(
+ stream_settings);
+ CHECK(i_buffer_output_stream_settings != nullptr);
+ i_buffer_output_stream_settings->setBufferType(Argus::BUFFER_TYPE_EGL_IMAGE);
+ i_buffer_output_stream_settings->setMetadataEnable(true);
+ LOG(INFO) << "Type: "
+ << i_buffer_output_stream_settings->getBufferType().getName();
+
+ Argus::UniqueObj<Argus::OutputStream> output_stream(
+ i_capture_session->createOutputStream(stream_settings.get()));
+ LOG(INFO) << "Got sream";
+
+ Argus::IBufferOutputStream *i_buffer_output_stream =
+ Argus::interface_cast<Argus::IBufferOutputStream>(output_stream);
+ CHECK(i_buffer_output_stream != nullptr);
+
+ // Build the DmaBuffers
+ std::array<std::unique_ptr<DmaBuffer>, 10> native_buffers;
+ for (size_t i = 0; i < native_buffers.size(); ++i) {
+ native_buffers[i] = DmaBuffer::Create(
+ i_sensor_mode->getResolution(),
+ static_cast<NvBufSurfaceColorFormat>(FLAGS_colorformat),
+ NVBUF_LAYOUT_PITCH);
+ }
+
+ std::array<NvBufSurface *, 10> surf;
+
+ // Create EGLImages from the native buffers
+ std::array<EGLImageKHR, 10> egl_images;
+ for (size_t i = 0; i < egl_images.size(); i++) {
+ int ret = 0;
+
+ ret = NvBufSurfaceFromFd(native_buffers[i]->fd(), (void **)(&surf[i]));
+ CHECK(ret == 0) << ": NvBufSurfaceFromFd failed";
+
+ ret = NvBufSurfaceMapEglImage(surf[i], 0);
+ CHECK(ret == 0) << ": NvBufSurfaceMapEglImage failed";
+
+ egl_images[i] = surf[i]->surfaceList[0].mappedAddr.eglImage;
+ CHECK(egl_images[i] != EGL_NO_IMAGE_KHR) << ": Failed to create EGLImage";
+ }
+
+ // Create the BufferSettings object to configure Buffer creation.
+ Argus::UniqueObj<Argus::BufferSettings> buffer_settings(
+ i_buffer_output_stream->createBufferSettings());
+ Argus::IEGLImageBufferSettings *i_buffer_settings =
+ Argus::interface_cast<Argus::IEGLImageBufferSettings>(buffer_settings);
+ if (!i_buffer_settings) ORIGINATE_ERROR("Failed to create BufferSettings");
+
+ // Create the Buffers for each EGLImage (and release to the stream for initial
+ // capture use)
+ std::array<Argus::UniqueObj<Argus::Buffer>, 10> buffers;
+ for (size_t i = 0; i < buffers.size(); i++) {
+ i_buffer_settings->setEGLImage(egl_images[i]);
+ i_buffer_settings->setEGLDisplay(egl_display);
+ buffers[i].reset(
+ i_buffer_output_stream->createBuffer(buffer_settings.get()));
+ Argus::IBuffer *i_buffer =
+ Argus::interface_cast<Argus::IBuffer>(buffers[i]);
+
+ // Ties Argus::Buffer and DmaBuffer together.
+ i_buffer->setClientData(native_buffers[i].get());
+ native_buffers[i]->set_argus_buffer(buffers[i].get());
+
+ CHECK(Argus::interface_cast<Argus::IEGLImageBuffer>(buffers[i]) != nullptr)
+ << ": Failed to create Buffer";
+
+ if (i_buffer_output_stream->releaseBuffer(buffers[i].get()) !=
+ Argus::STATUS_OK)
+ ORIGINATE_ERROR("Failed to release Buffer for capture use");
+ }
+
+ Argus::UniqueObj<Argus::Request> request(i_capture_session->createRequest());
+ Argus::IRequest *i_request = Argus::interface_cast<Argus::IRequest>(request);
+ CHECK(i_request);
+
+ Argus::IAutoControlSettings *i_auto_control_settings =
+ Argus::interface_cast<Argus::IAutoControlSettings>(
+ i_request->getAutoControlSettings());
+ CHECK(i_auto_control_settings != nullptr);
+ i_auto_control_settings->setAwbMode(Argus::AWB_MODE_OFF);
+
+ i_auto_control_settings->setAeLock(false);
+ Argus::Range<float> isp_digital_gain_range;
+ isp_digital_gain_range.min() = 1;
+ isp_digital_gain_range.max() = 1;
+ i_auto_control_settings->setIspDigitalGainRange(isp_digital_gain_range);
+
+ Argus::IEdgeEnhanceSettings *i_ee_settings =
+ Argus::interface_cast<Argus::IEdgeEnhanceSettings>(request);
+ CHECK(i_ee_settings != nullptr);
+
+ i_ee_settings->setEdgeEnhanceStrength(0);
+
+ i_request->enableOutputStream(output_stream.get());
+
+ Argus::ISourceSettings *i_source_settings =
+ Argus::interface_cast<Argus::ISourceSettings>(
+ i_request->getSourceSettings());
+ CHECK(i_source_settings != nullptr);
+
+ i_source_settings->setFrameDurationRange(
+ i_sensor_mode->getFrameDurationRange().min());
+ i_source_settings->setSensorMode(sensor_modes[FLAGS_mode]);
+
+ Argus::Range<float> sensor_mode_analog_gain_range;
+ sensor_mode_analog_gain_range.min() = FLAGS_gain;
+ sensor_mode_analog_gain_range.max() = FLAGS_gain;
+ i_source_settings->setGainRange(sensor_mode_analog_gain_range);
+
+ Argus::Range<uint64_t> limit_exposure_time_range;
+ limit_exposure_time_range.min() = FLAGS_exposure;
+ limit_exposure_time_range.max() = FLAGS_exposure;
+ i_source_settings->setExposureTimeRange(limit_exposure_time_range);
+
+ if (i_capture_session->repeat(request.get()) != Argus::STATUS_OK) {
+ LOG(ERROR) << "Failed to submit repeat";
+ }
+
+ LOG(INFO) << "Session submitted";
+
+ // Run.
+ //
+ // TODO(austin): Use the event loop a bit better... That'll let us set
+ // priority + get stats. Timer which always repeats "now" ?
+ aos::monotonic_clock::time_point last_time = aos::monotonic_clock::epoch();
+ while (true) {
+ VLOG(1) << "Going for frame";
+ Argus::Status status;
+
+ Argus::Buffer *buffer = i_buffer_output_stream->acquireBuffer(
+ std::chrono::nanoseconds(std::chrono::seconds(5)).count(), &status);
+
+ if (status == Argus::STATUS_END_OF_STREAM) {
+ break;
+ }
+
+ const aos::monotonic_clock::time_point now = aos::monotonic_clock::now();
+
+ DmaBuffer *dmabuf = DmaBuffer::FromArgusBuffer(buffer);
+ int dmabuf_fd = dmabuf->fd();
+
+ Argus::IBuffer *ibuffer = Argus::interface_cast<Argus::IBuffer>(buffer);
+ CHECK(ibuffer != nullptr);
+
+ const Argus::CaptureMetadata *metadata = ibuffer->getMetadata();
+ const Argus::ICaptureMetadata *imetadata =
+ Argus::interface_cast<const Argus::ICaptureMetadata>(metadata);
+
+ NvBufSurface *nvbuf_surf = 0;
+ CHECK_EQ(NvBufSurfaceFromFd(dmabuf_fd, (void **)(&nvbuf_surf)), 0);
+
+ CHECK_EQ(NvBufSurfaceMap(nvbuf_surf, -1, -1, NVBUF_MAP_READ), 0);
+ VLOG(1) << "Mapped";
+ NvBufSurfaceSyncForCpu(nvbuf_surf, -1, -1);
+
+ VLOG(1) << "Planes " << nvbuf_surf->surfaceList->planeParams.num_planes
+ << " colorFormat " << nvbuf_surf->surfaceList->colorFormat;
+ for (size_t i = 0; i < nvbuf_surf->surfaceList->planeParams.num_planes;
+ ++i) {
+ VLOG(1) << "Address "
+ << static_cast<void *>(
+ nvbuf_surf->surfaceList->mappedAddr.addr[i])
+ << ", pitch " << nvbuf_surf->surfaceList->planeParams.pitch[i]
+ << " height " << nvbuf_surf->surfaceList->planeParams.height[i]
+ << " width " << nvbuf_surf->surfaceList->planeParams.width[i]
+ << " bytes per pixel "
+ << nvbuf_surf->surfaceList->planeParams.bytesPerPix[i];
+ }
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.width[0],
+ static_cast<size_t>(FLAGS_width));
+ CHECK_EQ(nvbuf_surf->surfaceList->planeParams.height[0],
+ static_cast<size_t>(FLAGS_height));
+
+ aos::Sender<frc971::vision::CameraImage>::Builder builder =
+ sender.MakeBuilder();
+
+ uint8_t *data_pointer = nullptr;
+ builder.fbb()->StartIndeterminateVector(FLAGS_width * FLAGS_height * 2, 1,
+ 64, &data_pointer);
+
+ YCbCr422(nvbuf_surf, data_pointer);
+ flatbuffers::Offset<flatbuffers::Vector<uint8_t>> data_offset =
+ builder.fbb()->EndIndeterminateVector(FLAGS_width * FLAGS_height * 2,
+ 1);
+
+ auto image_builder = builder.MakeBuilder<frc971::vision::CameraImage>();
+ image_builder.add_data(data_offset);
+ image_builder.add_rows(FLAGS_height);
+ image_builder.add_cols(FLAGS_width);
+ image_builder.add_monotonic_timestamp_ns(imetadata->getFrameReadoutTime());
+ builder.CheckOk(builder.Send(image_builder.Finish()));
+
+ const aos::monotonic_clock::time_point after_send =
+ aos::monotonic_clock::now();
+
+ CHECK_EQ(NvBufSurfaceUnMap(nvbuf_surf, -1, -1), 0);
+
+ CHECK(imetadata);
+ VLOG(1) << "Got " << imetadata->getCaptureId() << " delay "
+ << chrono::duration<double>(
+ chrono::nanoseconds((now.time_since_epoch().count() -
+ (imetadata->getSensorTimestamp() +
+ imetadata->getFrameReadoutTime()))))
+ .count()
+ << " mmap " << chrono::duration<double>(after_send - now).count()
+ << "sec dt " << chrono::duration<double>(now - last_time).count()
+ << "sec " << dmabuf << " exposure "
+ << imetadata->getSensorExposureTime();
+ i_buffer_output_stream->releaseBuffer(buffer);
+
+ last_time = now;
+ }
+
+ i_capture_session->stopRepeat();
+ i_buffer_output_stream->endOfStream();
+ i_capture_session->waitForIdle();
+
+ output_stream.reset();
+
+ for (uint32_t i = 0; i < surf.size(); i++) {
+ NvBufSurfaceUnMapEglImage(surf[i], 0);
+ }
+
+ eglTerminate(egl_display);
+ return 0;
+}
+
+}; // namespace frc971
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ return frc971::Main();
+}
+
+// I tried every different format option. Here's what worked and didn't work.
+//
+// NVBUF_COLOR_FORMAT_RGB,
+
+// NVBUF_COLOR_FORMAT_YUYV, // Failed
+// NVBUF_COLOR_FORMAT_NV24, // Works
+// NVBUF_COLOR_FORMAT_UYVY, // Failed
+// NVBUF_COLOR_FORMAT_YUV420, // Failed with error.
+
+// NVBUF_COLOR_FORMAT_GRAY8, // unsupported
+// NVBUF_COLOR_FORMAT_YUV420, // unsupported
+// NVBUF_COLOR_FORMAT_YVU420, // unsupported
+
+// NVBUF_COLOR_FORMAT_YUV420_ER, // unsupported
+// NVBUF_COLOR_FORMAT_YVU420_ER, // unsupported
+//
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV12, // Works! pitch 2048 height 1080 width
+// 1920 colorFormat 6 planes 2 bytes per pixel 1 delay 0.00203304
+// mmap 0.000340288sec dt 0.0166379sec
+//
+///** Specifies BT.601 colorspace - Y/CbCr ER 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV12_ER, // Works! pitch 2048 height 1080
+// width 1920 colorFormat 7 planes 2 bytes per pixel 1
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV21, // Works! pitch 2048 height 1080 width
+// 1920 colorFormat 8 planes 2 bytes per pixel 1
+///** Specifies BT.601 colorspace - Y/CbCr ER 4:2:0 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV21_ER, // Works! pitch 2048 height 1080
+// width 1920 colorFormat 9 planes 2 bytes per pixel 1
+//
+//
+// NVBUF_COLOR_FORMAT_UYVY, // works with an error?!?
+// NVBUF_COLOR_FORMAT_UYVY_ER, // unsupported 11
+// NVBUF_COLOR_FORMAT_VYUY, // unsupported 12
+// NVBUF_COLOR_FORMAT_VYUY_ER, // unsupported 13
+// NVBUF_COLOR_FORMAT_YUYV, // unsupported 14
+// NVBUF_COLOR_FORMAT_YUYV_ER, // unsupported 15
+// NVBUF_COLOR_FORMAT_YVYU, // unsupported 16
+// NVBUF_COLOR_FORMAT_YVYU_ER, // unsupported 17
+// NVBUF_COLOR_FORMAT_YUV444, // unsupported 18
+// NVBUF_COLOR_FORMAT_RGBA, // unsupported 19
+// NVBUF_COLOR_FORMAT_BGRA, // unsupported 20
+// NVBUF_COLOR_FORMAT_ARGB, // unsupported 21
+// NVBUF_COLOR_FORMAT_ABGR, // unsupported 22
+// NVBUF_COLOR_FORMAT_RGBx, // unsupported 23
+// NVBUF_COLOR_FORMAT_BGRx, // unsupported 24
+// NVBUF_COLOR_FORMAT_xRGB, // unsupported 25
+// NVBUF_COLOR_FORMAT_xBGR, // unsupported 26
+// NVBUF_COLOR_FORMAT_RGB, // unsupported 27
+// NVBUF_COLOR_FORMAT_BGR, // unsupported 28
+// NVBUF_COLOR_FORMAT_NV12_10LE, // unsupported 29
+// NVBUF_COLOR_FORMAT_NV12_12LE, // unsupported 30
+// NVBUF_COLOR_FORMAT_YUV420_709, // unsupported 31
+// NVBUF_COLOR_FORMAT_YUV420_709_ER, // unsupported 32
+// NVBUF_COLOR_FORMAT_NV12_709, // works pitch 2048 height 1080
+// width 1920 colorFormat 33 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV12_709_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 34 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_YUV420_2020, // unsupported 35
+// NVBUF_COLOR_FORMAT_NV12_2020, // unsupported 36
+// NVBUF_COLOR_FORMAT_NV12_10LE_ER, // unsupported 37
+// NVBUF_COLOR_FORMAT_NV12_10LE_709, // unsupported 38
+// NVBUF_COLOR_FORMAT_NV12_10LE_709_ER, // unsupported 39
+// NVBUF_COLOR_FORMAT_NV12_10LE_2020, // unsupported 40
+// NVBUF_COLOR_FORMAT_SIGNED_R16G16, // unsupported 41
+// NVBUF_COLOR_FORMAT_R8_G8_B8, // unsupported 42
+// NVBUF_COLOR_FORMAT_B8_G8_R8, // unsupported 43
+// NVBUF_COLOR_FORMAT_R32F_G32F_B32F, // unsupported 44
+// NVBUF_COLOR_FORMAT_B32F_G32F_R32F, // unsupported 45
+// NVBUF_COLOR_FORMAT_YUV422, // unsupported 46
+// NVBUF_COLOR_FORMAT_NV21_10LE, // unsupported 47
+// NVBUF_COLOR_FORMAT_NV21_12LE, // unsupported 48
+// NVBUF_COLOR_FORMAT_NV12_12LE_2020, // unsupported 49
+///** Specifies BT.601 colorspace - Y/CbCr 4:2:2 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV16, // works pitch 2048 height 1080 width
+// 1920 colorFormat 50 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV16_10LE, // unsupported 51
+///** Specifies BT.601 colorspace - Y/CbCr 4:4:4 multi-planar. */
+// NVBUF_COLOR_FORMAT_NV24, // works pitch 2048 height 1080
+// width 1920 colorFormat 52 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV24_10LE, // unsupported 53
+//
+// NVBUF_COLOR_FORMAT_NV16_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 54 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV24_ER, // works pitch 2048 height 1080
+// width 1920 colorFormat 55 planes 2 bytes per pixel 1
+// NVBUF_COLOR_FORMAT_NV16_709, // unsupported 56
+// NVBUF_COLOR_FORMAT_NV24_709, // unsupported 57
+// NVBUF_COLOR_FORMAT_NV16_709_ER, // unsupported 58
+// NVBUF_COLOR_FORMAT_NV24_709_ER, // unsupported 59
+// NVBUF_COLOR_FORMAT_NV24_10LE_709, // unsupported 60
+// NVBUF_COLOR_FORMAT_NV24_10LE_709_ER, // unsupported 61
+// NVBUF_COLOR_FORMAT_NV24_10LE_2020, // unsupported 62
+// NVBUF_COLOR_FORMAT_NV24_12LE_2020, // unsupported 63
+// NVBUF_COLOR_FORMAT_RGBA_10_10_10_2_709, // unsupported 64
+// NVBUF_COLOR_FORMAT_RGBA_10_10_10_2_2020, // unsupported 65
+// NVBUF_COLOR_FORMAT_BGRA_10_10_10_2_709, // unsupported 66
+// NVBUF_COLOR_FORMAT_BGRA_10_10_10_2_2020, // unsupported 67
+// NVBUF_COLOR_FORMAT_A32, // unsupported 68
+// NVBUF_COLOR_FORMAT_UYVP, // unsupported 69
+// NVBUF_COLOR_FORMAT_UYVP_ER // unsupported 70
+
+// NVBUF_COLOR_FORMAT_ABGR,
+// NVBUF_COLOR_FORMAT_ARGB,
diff --git a/frc971/orin/build_rootfs.py b/frc971/orin/build_rootfs.py
index 3575772..c378226 100755
--- a/frc971/orin/build_rootfs.py
+++ b/frc971/orin/build_rootfs.py
@@ -765,7 +765,7 @@
resolved_deps.sort()
rule_name = obj[1:].replace('/', '_')
rule_deps = ''.join([
- ' ":%s",\n'.format(d[1:].replace('/', '_'))
+ ' ":{}",\n'.format(d[1:].replace('/', '_'))
for d in resolved_deps if d not in skip_set
])
rules.append(
@@ -835,10 +835,6 @@
],
exclude = [
"usr/share/**",
- "usr/local/cuda-11.8/include/thrust/**",
- "usr/local/cuda-11.8/include/nv/**",
- "usr/local/cuda-11.8/include/cuda/**",
- "usr/local/cuda-11.8/include/cub/**",
],
)""",
"RULES": '\n\n'.join(rules),
@@ -847,6 +843,8 @@
with open("../../compilers/orin_debian_rootfs.BUILD", "w") as file:
file.write(template.render(substitutions))
+ subprocess.run(['buildifier', "../../compilers/orin_debian_rootfs.BUILD"])
+
def do_package(partition):
tarball = datetime.date.today().strftime(
@@ -875,6 +873,10 @@
"--exclude=./usr/local/cuda-11.8/include/nv",
"--exclude=./usr/local/cuda-11.8/include/cuda",
"--exclude=./usr/local/cuda-11.8/include/cub",
+ "--exclude=./usr/include/cub",
+ "--exclude=./usr/include/nv",
+ "--exclude=./usr/include/thrust",
+ "--exclude=./usr/include/cuda",
"--exclude=./usr/share",
"-cf",
tarball,
@@ -1112,8 +1114,11 @@
target_mkdir("root:root", "755", "etc/systemd/network")
copyfile("root:root", "644", "etc/systemd/network/eth0.network")
- copyfile("root:root", "644", "etc/systemd/network/80-can.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-cana.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-canb.network")
+ copyfile("root:root", "644", "etc/systemd/network/80-canc.network")
copyfile("root:root", "644", "etc/udev/rules.d/nvidia.rules")
+ copyfile("root:root", "644", "etc/udev/rules.d/can.rules")
target(["/root/bin/change_hostname.sh", "pi-971-1"])
target(["systemctl", "enable", "systemd-networkd"])
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-cana.network
similarity index 64%
copy from frc971/orin/contents/etc/systemd/network/80-can.network
copy to frc971/orin/contents/etc/systemd/network/80-cana.network
index e75db33..0e4ad11 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-cana.network
@@ -1,9 +1,7 @@
[Match]
-Name=can0
+Name=cana
[CAN]
BitRate=1M
-DataBitRate=8M
RestartSec=1000ms
BusErrorReporting=yes
-FDMode=yes
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-canb.network
similarity index 90%
rename from frc971/orin/contents/etc/systemd/network/80-can.network
rename to frc971/orin/contents/etc/systemd/network/80-canb.network
index e75db33..1bef0ee 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-canb.network
@@ -1,5 +1,5 @@
[Match]
-Name=can0
+Name=canb
[CAN]
BitRate=1M
diff --git a/frc971/orin/contents/etc/systemd/network/80-can.network b/frc971/orin/contents/etc/systemd/network/80-canc.network
similarity index 90%
copy from frc971/orin/contents/etc/systemd/network/80-can.network
copy to frc971/orin/contents/etc/systemd/network/80-canc.network
index e75db33..cdcbe81 100644
--- a/frc971/orin/contents/etc/systemd/network/80-can.network
+++ b/frc971/orin/contents/etc/systemd/network/80-canc.network
@@ -1,5 +1,5 @@
[Match]
-Name=can0
+Name=canc
[CAN]
BitRate=1M
diff --git a/frc971/orin/contents/etc/udev/rules.d/can.rules b/frc971/orin/contents/etc/udev/rules.d/can.rules
new file mode 100644
index 0000000..83aa68e
--- /dev/null
+++ b/frc971/orin/contents/etc/udev/rules.d/can.rules
@@ -0,0 +1,3 @@
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x0", ENV{ID_PATH_TAG}=="platform-c310000_mttcan", NAME="cana"
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x0", ENV{ID_PATH_TAG}=="platform-14100000_pcie-pci-0001_01_00_0", NAME="canb"
+SUBSYSTEM=="net", ACTION=="add", ATTR{dev_id}=="0x1", ENV{ID_PATH_TAG}=="platform-14100000_pcie-pci-0001_01_00_0", NAME="canc"
diff --git a/frc971/orin/crcv_generator.cc b/frc971/orin/crcv_generator.cc
index 99318e6..5c72385 100644
--- a/frc971/orin/crcv_generator.cc
+++ b/frc971/orin/crcv_generator.cc
@@ -20,17 +20,17 @@
namespace {
template <typename T>
-void SetRowMajor(T *buffer_parameter, int cols, int rows) {
- buffer_parameter->dim(0).set_stride(3);
+void SetRowMajor(T *buffer_parameter, int cols, int rows, int channels) {
+ buffer_parameter->dim(0).set_stride(channels);
buffer_parameter->dim(0).set_extent(cols);
buffer_parameter->dim(0).set_min(0);
- buffer_parameter->dim(1).set_stride(cols * 3);
+ buffer_parameter->dim(1).set_stride(cols * channels);
buffer_parameter->dim(1).set_extent(rows);
buffer_parameter->dim(1).set_min(0);
buffer_parameter->dim(2).set_stride(1);
- buffer_parameter->dim(2).set_extent(3);
+ buffer_parameter->dim(2).set_extent(channels);
buffer_parameter->dim(2).set_min(0);
}
} // namespace
@@ -86,7 +86,59 @@
output.vectorize(col, 8);
output.unroll(col, 4);
- SetRowMajor(&output, cols, rows);
+ SetRowMajor(&output, cols, rows, 3);
+ }
+};
+
+class YCbCr422 : public Halide::Generator<YCbCr422> {
+ public:
+ GeneratorParam<int> cols{"cols", 0};
+ GeneratorParam<int> rows{"rows", 0};
+ GeneratorParam<int> ystride{"ystride", 0};
+ GeneratorParam<int> cbcrstride{"cbcrstride", 0};
+
+ Input<Buffer<uint8_t, 2>> input_y{"y"};
+ Input<Buffer<uint8_t, 3>> input_cbcr{"cbcr"};
+ Output<Buffer<uint8_t, 3>> output{"output"};
+
+ Var col{"col"}, row{"row"}, channel{"channel"};
+
+ // Everything is indexed as col, row, channel.
+ void generate() {
+ CHECK(cols > 0, "Must specify a cols");
+ CHECK((cols % 2) == 0, "Must specify a cols with an even number of cols");
+ CHECK(rows > 0, "Must specify a rows");
+
+ input_y.dim(0).set_stride(1);
+ input_y.dim(0).set_extent(cols);
+ input_y.dim(0).set_min(0);
+
+ input_y.dim(1).set_stride(ystride);
+ input_y.dim(1).set_extent(rows);
+ input_y.dim(1).set_min(0);
+
+ input_cbcr.dim(0).set_stride(2);
+ input_cbcr.dim(0).set_extent(cols / 2);
+ input_cbcr.dim(0).set_min(0);
+
+ input_cbcr.dim(1).set_stride(cbcrstride);
+ input_cbcr.dim(1).set_extent(rows);
+ input_cbcr.dim(1).set_min(0);
+
+ input_cbcr.dim(2).set_stride(1);
+ input_cbcr.dim(2).set_extent(2);
+ input_cbcr.dim(2).set_min(0);
+
+ output(col, row, channel) = Halide::select(
+ channel == 0, input_y(col, row), input_cbcr(col / 2, row, col % 2));
+
+ output.reorder(channel, col, row);
+ output.unroll(channel);
+
+ output.vectorize(col, 8);
+ output.unroll(col, 4);
+
+ SetRowMajor(&output, cols, rows, 2);
}
};
@@ -94,3 +146,4 @@
} // namespace frc971
HALIDE_REGISTER_GENERATOR(frc971::orin::YCbCr, ycbcr)
+HALIDE_REGISTER_GENERATOR(frc971::orin::YCbCr422, ycbcr422)
diff --git a/frc971/orin/cuda_april_tag_test.cc b/frc971/orin/cuda_april_tag_test.cc
index 1d5aa9b..776caf5 100644
--- a/frc971/orin/cuda_april_tag_test.cc
+++ b/frc971/orin/cuda_april_tag_test.cc
@@ -533,7 +533,7 @@
};
// The theta algorithm used by cuda.
- auto ComputeTheta = [&blob_centers](QuadBoundaryPoint pair) -> float {
+ auto ComputeTheta = [&blob_centers](QuadBoundaryPoint pair) -> long double {
const int32_t x = pair.x();
const int32_t y = pair.y();
@@ -546,7 +546,8 @@
float dx = x - cx;
float dy = y - cy;
- return atan2f(dy, dx);
+ // make atan2 more accurate than cuda to correctly sort
+ return atan2f64(dy, dx);
};
for (size_t i = 0; i < points.size();) {
@@ -1083,7 +1084,7 @@
slope_sorted_expected_grouped_points[i];
CHECK_EQ(cuda_grouped_blob.size(), slope_sorted_points.size());
- if (VLOG_IS_ON(1) && cuda_grouped_blob[0].blob_index() == 73) {
+ if (VLOG_IS_ON(1) && cuda_grouped_blob[0].blob_index() == 160) {
for (size_t j = 0; j < cuda_grouped_points[i].size(); ++j) {
LOG(INFO) << "For blob " << cuda_grouped_blob[0].blob_index()
<< ", got " << cuda_grouped_blob[j] << " ("
@@ -1094,20 +1095,86 @@
<< slope_sorted_points[j].y() << ")";
}
}
+
size_t missmatched_runs = 0;
+
+ // recalculates the theta to be used in the following check
+ std::map<uint64_t, std::pair<float, float>> blob_centers;
+ auto ComputeTheta =
+ [&blob_centers](QuadBoundaryPoint pair) -> long double {
+ const int32_t x = pair.x();
+ const int32_t y = pair.y();
+
+ auto blob_center = blob_centers.find(pair.rep01());
+ CHECK(blob_center != blob_centers.end());
+
+ const float cx = blob_center->second.first;
+ const float cy = blob_center->second.second;
+
+ float dx = x - cx;
+ float dy = y - cy;
+
+ return atan2f64(dy, dx);
+ };
+
+ // refinds blob centers
+ for (size_t i = 0; i < slope_sorted_points.size();) {
+ uint64_t first_rep01 = slope_sorted_points[i].rep01();
+
+ int min_x, min_y, max_x, max_y;
+ min_x = max_x = slope_sorted_points[i].x();
+ min_y = max_y = slope_sorted_points[i].y();
+
+ size_t j = i;
+ for (; j < slope_sorted_points.size() &&
+ slope_sorted_points[j].rep01() == first_rep01;
+ ++j) {
+ QuadBoundaryPoint pt = slope_sorted_points[j];
+
+ int x = pt.x();
+ int y = pt.y();
+ min_x = std::min(min_x, x);
+ max_x = std::max(max_x, x);
+ min_y = std::min(min_y, y);
+ max_y = std::max(max_y, y);
+ }
+
+ const float cx = (max_x + min_x) * 0.5 + 0.05118;
+ const float cy = (max_y + min_y) * 0.5 + -0.028581;
+
+ blob_centers[first_rep01] = std::make_pair(cx, cy);
+ i = j;
+ }
+
for (size_t j = 0; j < cuda_grouped_points[i].size(); ++j) {
if (cuda_grouped_blob[j].x() != slope_sorted_points[j].x() ||
cuda_grouped_blob[j].y() != slope_sorted_points[j].y()) {
+ size_t allowable_swapped_indices = 1;
+ long double max_allowed_imprecision = 3e-7;
+ // search through indixes j - a to j + a to see if they're swapped
+ // also only doesn't count if the precision needed to differentiate is
+ // less than max_allowed_imprecision
+ for (size_t k = j - allowable_swapped_indices;
+ k <= j + allowable_swapped_indices; k++) {
+ if (cuda_grouped_blob[j].x() == slope_sorted_points[k].x() &&
+ cuda_grouped_blob[j].y() == slope_sorted_points[k].y() &&
+ abs(ComputeTheta(slope_sorted_points[k]) -
+ ComputeTheta(slope_sorted_points[j])) <
+ max_allowed_imprecision) {
+ continue;
+ }
+ }
++missmatched_points;
++missmatched_runs;
// We shouldn't see a lot of points in a row which don't match.
- CHECK_LE(missmatched_runs, 4u);
VLOG(1) << "Missmatched point in blob "
<< cuda_grouped_blob[0].blob_index() << ", point " << j
<< " (" << cuda_grouped_blob[j].x() << ", "
<< cuda_grouped_blob[j].y() << ") vs ("
<< slope_sorted_points[j].x() << ", "
- << slope_sorted_points[j].y() << ")";
+ << slope_sorted_points[j].y() << ")"
+ << " in size " << cuda_grouped_points[i].size();
+ CHECK_LE(missmatched_runs, 4u);
} else {
missmatched_runs = 0;
}
@@ -1456,108 +1523,15 @@
INSTANTIATE_TEST_SUITE_P(
CapturedImages, SingleAprilDetectionTest,
::testing::Values("bfbs-capture-2013-01-18_08-54-16.869057537.bfbs",
- "bfbs-capture-2013-01-18_08-54-09.501047728.bfbs"
- // TODO(austin): Figure out why these fail...
- //"bfbs-capture-2013-01-18_08-51-24.861065764.bfbs",
- //"bfbs-capture-2013-01-18_08-52-01.846912552.bfbs",
- //"bfbs-capture-2013-01-18_08-52-33.462848049.bfbs",
- //"bfbs-capture-2013-01-18_08-54-24.931661979.bfbs",
- //"bfbs-capture-2013-01-18_09-29-16.806073486.bfbs",
- //"bfbs-capture-2013-01-18_09-33-00.993756514.bfbs",
- //"bfbs-capture-2013-01-18_08-57-00.171120695.bfbs"
- //"bfbs-capture-2013-01-18_08-57-17.858752817.bfbs",
- //"bfbs-capture-2013-01-18_08-57-08.096597542.bfbs"
- ));
-
-// Tests that QuadBoundaryPoint doesn't corrupt data.
-TEST(QuadBoundaryPoint, MasksWork) {
- std::mt19937 generator(aos::testing::RandomSeed());
- std::uniform_int_distribution<uint32_t> random_rep_scalar(0, 0xfffff);
- std::uniform_int_distribution<uint32_t> random_point_scalar(0, 0x3ff);
- std::uniform_int_distribution<uint32_t> random_dxy_scalar(0, 3);
- std::uniform_int_distribution<uint32_t> random_bool(0, 1);
-
- QuadBoundaryPoint point;
-
- EXPECT_EQ(point.key, 0);
-
- for (int i = 0; i < 25; ++i) {
- const uint32_t rep0 = random_rep_scalar(generator);
- for (int j = 0; j < 25; ++j) {
- const uint32_t rep1 = random_rep_scalar(generator);
- for (int k = 0; k < 25; ++k) {
- const uint32_t x = random_point_scalar(generator);
- const uint32_t y = random_point_scalar(generator);
- for (int k = 0; k < 25; ++k) {
- const uint32_t dxy = random_dxy_scalar(generator);
- for (int m = 0; m < 2; ++m) {
- const bool black_to_white = random_bool(generator) == 1;
- if (point.rep0() != rep0) {
- point.set_rep0(rep0);
- }
- if (point.rep1() != rep1) {
- point.set_rep1(rep1);
- }
- if (point.base_x() != x || point.base_y() != y) {
- point.set_base_xy(x, y);
- }
- switch (dxy) {
- case 0:
- if (point.dx() != 1 || point.dy() != 0) {
- point.set_dxy(dxy);
- }
- break;
- case 1:
- if (point.dx() != 1 || point.dy() != 1) {
- point.set_dxy(dxy);
- }
- break;
- case 2:
- if (point.dx() != 0 || point.dy() != 1) {
- point.set_dxy(dxy);
- }
- break;
- case 3:
- if (point.dx() != -1 || point.dy() != 1) {
- point.set_dxy(dxy);
- }
- break;
- }
- if (black_to_white != point.black_to_white()) {
- point.set_black_to_white(black_to_white);
- }
-
- ASSERT_EQ(point.rep0(), rep0);
- ASSERT_EQ(point.rep1(), rep1);
- ASSERT_EQ(point.base_x(), x);
- ASSERT_EQ(point.base_y(), y);
- switch (dxy) {
- case 0:
- ASSERT_EQ(point.dx(), 1);
- ASSERT_EQ(point.dy(), 0);
- break;
- case 1:
- ASSERT_EQ(point.dx(), 1);
- ASSERT_EQ(point.dy(), 1);
- break;
- case 2:
- ASSERT_EQ(point.dx(), 0);
- ASSERT_EQ(point.dy(), 1);
- break;
- case 3:
- ASSERT_EQ(point.dx(), -1);
- ASSERT_EQ(point.dy(), 1);
- break;
- }
- ASSERT_EQ(point.x(), x * 2 + point.dx());
- ASSERT_EQ(point.y(), y * 2 + point.dy());
-
- ASSERT_EQ(point.black_to_white(), black_to_white);
- }
- }
- }
- }
- }
-}
+ "bfbs-capture-2013-01-18_08-54-09.501047728.bfbs",
+ "bfbs-capture-2013-01-18_08-51-24.861065764.bfbs",
+ "bfbs-capture-2013-01-18_08-52-01.846912552.bfbs",
+ "bfbs-capture-2013-01-18_08-52-33.462848049.bfbs",
+ "bfbs-capture-2013-01-18_08-54-24.931661979.bfbs",
+ "bfbs-capture-2013-01-18_09-29-16.806073486.bfbs",
+ "bfbs-capture-2013-01-18_09-33-00.993756514.bfbs",
+ "bfbs-capture-2013-01-18_08-57-00.171120695.bfbs",
+ "bfbs-capture-2013-01-18_08-57-17.858752817.bfbs",
+ "bfbs-capture-2013-01-18_08-57-08.096597542.bfbs"));
} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/output_iterator_test.cc b/frc971/orin/output_iterator_test.cc
new file mode 100644
index 0000000..d4087d0
--- /dev/null
+++ b/frc971/orin/output_iterator_test.cc
@@ -0,0 +1,72 @@
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/random_seed.h"
+#include "frc971/orin/transform_output_iterator.h"
+
+namespace frc971::apriltag::testing {
+
+struct Mul2 {
+ uint64_t operator()(const uint32_t num) const {
+ return static_cast<uint64_t>(num) * 2;
+ }
+};
+
+// Tests that the transform output iterator both transforms and otherwise acts
+// like a normal pointer
+TEST(TransformOutputIteratorTest, IntArr) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_int_distribution<uint32_t> random_uint32(0, UINT32_MAX);
+
+ uint32_t *nums_in = (uint32_t *)malloc(UINT32_WIDTH * 20);
+ uint64_t *nums_out = (uint64_t *)malloc(UINT64_WIDTH * 20);
+ uint64_t *expected_out = (uint64_t *)malloc(UINT64_WIDTH * 20);
+
+ for (size_t i = 0; i < 20; i++) {
+ nums_in[i] = random_uint32(generator);
+ expected_out[i] = 2 * static_cast<uint64_t>(nums_in[i]);
+ }
+
+ Mul2 convert_op;
+ TransformOutputIterator<uint32_t, uint64_t, Mul2> itr(nums_out, convert_op);
+
+ // check indirection, array index, increments, and decrements
+ EXPECT_EQ(itr == itr, true);
+ EXPECT_EQ(itr != itr, false);
+ *itr = *nums_in; // [0]
+ *(++itr) = *(++nums_in); // [1]
+ auto temp = itr;
+ auto temp2 = itr++;
+ EXPECT_EQ(temp, temp2); // [2]
+ EXPECT_NE(temp, itr);
+ EXPECT_NE(temp2, itr);
+ nums_in++; // [2]
+ *itr = *nums_in; // [2]
+ auto temp3 = ++itr;
+ auto temp4 = itr;
+ EXPECT_EQ(temp3, temp4); // [3]
+ EXPECT_EQ(temp3, itr);
+ itr--; // [2]
+ auto temp5 = --itr;
+ auto temp6 = itr;
+ EXPECT_EQ(temp5, temp6); // [1]
+ EXPECT_EQ(temp5, itr);
+ nums_in--; // [1]
+ auto temp7 = itr;
+ auto temp8 = itr--;
+ EXPECT_EQ(temp7, temp8); // [0]
+ EXPECT_NE(temp7, itr);
+ EXPECT_NE(temp8, itr);
+ nums_in--; // [0]
+
+ for (size_t i = 3; i < 20; i++) {
+ itr[i] = nums_in[i]; // [3] -> [19]
+ }
+
+ // check expected out and converted out
+ for (size_t i = 0; i < 20; i++) {
+ EXPECT_EQ(expected_out[i], nums_out[i]);
+ }
+}
+} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/points.h b/frc971/orin/points.h
index 312d90a..d530a17 100644
--- a/frc971/orin/points.h
+++ b/frc971/orin/points.h
@@ -203,9 +203,17 @@
}
// See QuadBoundaryPoint for a description of the rest of these.
+ // Sets the 10 bit x and y.
+ __forceinline__ __host__ __device__ void set_base_xy(uint32_t x, uint32_t y) {
+ key = (key & 0xffffffffff00000full) |
+ (static_cast<uint64_t>(x & 0x3ff) << 14) |
+ (static_cast<uint64_t>(y & 0x3ff) << 4);
+ }
+
__forceinline__ __host__ __device__ uint32_t base_x() const {
return ((key >> 14) & 0x3ff);
}
+
__forceinline__ __host__ __device__ uint32_t base_y() const {
return ((key >> 4) & 0x3ff);
}
diff --git a/frc971/orin/points_test.cc b/frc971/orin/points_test.cc
new file mode 100644
index 0000000..852a1e4
--- /dev/null
+++ b/frc971/orin/points_test.cc
@@ -0,0 +1,204 @@
+#include "frc971/orin/points.h"
+
+#include <random>
+
+#include "gtest/gtest.h"
+
+#include "aos/testing/random_seed.h"
+
+namespace frc971::apriltag::testing {
+
+// Tests that QuadBoundaryPoint doesn't corrupt data.
+TEST(QuadBoundaryPoint, MasksWork) {
+ std::mt19937 generator(aos::testing::RandomSeed());
+ std::uniform_int_distribution<uint32_t> random_rep_scalar(0, 0xfffff);
+ std::uniform_int_distribution<uint32_t> random_point_scalar(0, 0x3ff);
+ std::uniform_int_distribution<uint32_t> random_dxy_scalar(0, 3);
+ std::uniform_int_distribution<uint32_t> random_bool(0, 1);
+
+ QuadBoundaryPoint point;
+
+ EXPECT_EQ(point.key, 0);
+
+ for (int i = 0; i < 25; ++i) {
+ const uint32_t rep0 = random_rep_scalar(generator);
+ for (int j = 0; j < 25; ++j) {
+ const uint32_t rep1 = random_rep_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t x = random_point_scalar(generator);
+ const uint32_t y = random_point_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t dxy = random_dxy_scalar(generator);
+ for (int m = 0; m < 2; ++m) {
+ const bool black_to_white = random_bool(generator) == 1;
+
+ if (point.rep0() != rep0) {
+ point.set_rep0(rep0);
+ }
+
+ if (point.rep1() != rep1) {
+ point.set_rep1(rep1);
+ }
+
+ if (point.base_x() != x || point.base_y() != y) {
+ point.set_base_xy(x, y);
+ }
+
+ switch (dxy) {
+ case 0:
+ if (point.dx() != 1 || point.dy() != 0) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 1:
+ if (point.dx() != 1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 2:
+ if (point.dx() != 0 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 3:
+ if (point.dx() != -1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ }
+
+ if (black_to_white != point.black_to_white()) {
+ point.set_black_to_white(black_to_white);
+ }
+
+ EXPECT_EQ(point.rep0(), rep0);
+ EXPECT_EQ(point.rep1(), rep1);
+ EXPECT_EQ(point.base_x(), x);
+ EXPECT_EQ(point.base_y(), y);
+ switch (dxy) {
+ case 0:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 0);
+ break;
+ case 1:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 2:
+ EXPECT_EQ(point.dx(), 0);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 3:
+ EXPECT_EQ(point.dx(), -1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ }
+
+ EXPECT_EQ(point.x(), x * 2 + point.dx());
+ EXPECT_EQ(point.y(), y * 2 + point.dy());
+
+ EXPECT_EQ(point.black_to_white(), black_to_white);
+ }
+ }
+ }
+ }
+ }
+}
+
+// Tests that IndexPoint doesn't corrupt anything
+TEST(IndexPoint, MasksWork) {
+ std::mt19937 generator(
+ aos::testing::RandomSeed()); // random_uint32(generator)
+ std::uniform_int_distribution<uint32_t> random_blob_index(0, 0xfff);
+ std::uniform_int_distribution<uint32_t> random_theta(0, 0xfffffff);
+ std::uniform_int_distribution<uint32_t> random_point_scalar(0, 0x3ff);
+ std::uniform_int_distribution<uint32_t> random_dxy_scalar(0, 3);
+ std::uniform_int_distribution<uint32_t> random_bool(0, 1);
+
+ IndexPoint point;
+
+ for (int i = 0; i < 25; i++) {
+ const uint32_t blob_index = random_blob_index(generator);
+ for (int j = 0; j < 25; j++) {
+ const uint32_t theta = random_theta(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t x = random_point_scalar(generator);
+ const uint32_t y = random_point_scalar(generator);
+ for (int k = 0; k < 25; ++k) {
+ const uint32_t dxy = random_dxy_scalar(generator);
+ for (int m = 0; m < 2; ++m) {
+ const bool black_to_white = random_bool(generator) == 1;
+
+ if (point.blob_index() != blob_index) {
+ point.set_blob_index(blob_index);
+ }
+
+ if (point.theta() != theta) {
+ point.set_theta(theta);
+ }
+
+ if (point.base_x() != x || point.base_y() != y) {
+ point.set_base_xy(x, y);
+ }
+
+ switch (dxy) {
+ case 0:
+ if (point.dx() != 1 || point.dy() != 0) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 1:
+ if (point.dx() != 1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 2:
+ if (point.dx() != 0 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ case 3:
+ if (point.dx() != -1 || point.dy() != 1) {
+ point.set_dxy(dxy);
+ }
+ break;
+ }
+
+ if (black_to_white != point.black_to_white()) {
+ point.set_black_to_white(black_to_white);
+ }
+
+ EXPECT_EQ(point.blob_index(), blob_index);
+ EXPECT_EQ(point.theta(), theta);
+ EXPECT_EQ(point.base_x(), x);
+ EXPECT_EQ(point.base_y(), y);
+
+ switch (dxy) {
+ case 0:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 0);
+ break;
+ case 1:
+ EXPECT_EQ(point.dx(), 1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 2:
+ EXPECT_EQ(point.dx(), 0);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ case 3:
+ EXPECT_EQ(point.dx(), -1);
+ EXPECT_EQ(point.dy(), 1);
+ break;
+ }
+ EXPECT_EQ(point.x(), x * 2 + point.dx());
+ EXPECT_EQ(point.y(), y * 2 + point.dy());
+
+ EXPECT_EQ(point.black_to_white(), black_to_white);
+ }
+ }
+ }
+ }
+ }
+}
+} // namespace frc971::apriltag::testing
diff --git a/frc971/orin/transform_output_iterator.h b/frc971/orin/transform_output_iterator.h
new file mode 100644
index 0000000..051baea
--- /dev/null
+++ b/frc971/orin/transform_output_iterator.h
@@ -0,0 +1,92 @@
+#ifndef FRC971_TRANSFORM_OUTPUT_ITERATOR_
+#define FRC971_TRANSFORM_OUTPUT_ITERATOR_
+
+namespace frc971 {
+namespace apriltag {
+
+// template class that allows conversions at the output of a cub algorithm
+template <typename InputType, typename OutputType, typename ConversionOp,
+ typename OffsetT = ptrdiff_t>
+class TransformOutputIterator {
+ private:
+ // proxy object to be able to convert when assigning value
+ struct Reference {
+ OutputType *ptr;
+ ConversionOp convert_op;
+ __host__ __device__ Reference(OutputType *ptr, ConversionOp convert_op)
+ : ptr(ptr), convert_op(convert_op) {}
+ __host__ __device__ Reference operator=(InputType val) {
+ *ptr = convert_op(val);
+ return *this;
+ }
+ };
+
+ public:
+ // typedefs may not be neeeded for iterator to work but is here to maintain
+ // similarity to cub's CacheModifiedOutputIterator
+ typedef TransformOutputIterator self_type;
+ typedef OffsetT difference_type;
+ typedef void value_type;
+ typedef void *pointer;
+ typedef Reference reference;
+
+ TransformOutputIterator(OutputType *ptr, const ConversionOp convert_op)
+ : convert_op(convert_op), ptr(ptr) {}
+
+ // postfix addition
+ __host__ __device__ __forceinline__ self_type operator++(int) {
+ self_type retval = *this;
+ ptr++;
+ return retval;
+ }
+
+ // prefix addition
+ __host__ __device__ __forceinline__ self_type operator++() {
+ ptr++;
+ return *this;
+ }
+
+ // postfix subtraction
+ __host__ __device__ __forceinline__ self_type operator--(int) {
+ self_type retval = *this;
+ ptr--;
+ return retval;
+ }
+
+ // prefix subtraction
+ __host__ __device__ __forceinline__ self_type operator--() {
+ ptr--;
+ return *this;
+ }
+
+ // indirection
+ __host__ __device__ __forceinline__ reference operator*() const {
+ return Reference(ptr, convert_op);
+ }
+
+ // array index
+ __host__ __device__ __forceinline__ reference operator[](int num) const {
+ return Reference(ptr + num, convert_op);
+ }
+
+ // equal to
+ __host__ __device__ __forceinline__ bool operator==(
+ const TransformOutputIterator &rhs) const {
+ return ptr == rhs.ptr;
+ }
+
+ // not equal to
+ __host__ __device__ __forceinline__ bool operator!=(
+ const TransformOutputIterator &rhs) const {
+ return ptr != rhs.ptr;
+ }
+
+ private:
+ const ConversionOp convert_op;
+ OutputType *ptr;
+};
+
+} // namespace apriltag
+} // namespace frc971
+
+#endif // FRC971_TRANSFORM_OUTPUT_ITERATOR_
diff --git a/motors/button_board.cc b/motors/button_board.cc
index fb17328..1d20fdb 100644
--- a/motors/button_board.cc
+++ b/motors/button_board.cc
@@ -289,7 +289,11 @@
PORTB_PCR19 = PORT_PCR_DSE | PORT_PCR_MUX(2);
// .1ms filter time.
- PORTA_DFWR = PORTB_DFWR = PORTC_DFWR = PORTD_DFWR = PORTE_DFWR = 6000;
+ PORTA_DFWR = 6000;
+ PORTB_DFWR = 6000;
+ PORTC_DFWR = 6000;
+ PORTD_DFWR = 6000;
+ PORTE_DFWR = 6000;
// Set up the buttons. The LEDs pull them up to 5V, so the Teensy needs to not
// be set to pull up.
diff --git a/motors/motor.cc b/motors/motor.cc
index a343301..ed1296c 100644
--- a/motors/motor.cc
+++ b/motors/motor.cc
@@ -40,8 +40,10 @@
"Need to prescale");
void Motor::Init() {
- pwm_ftm_->CNTIN = encoder_ftm_->CNTIN = 0;
- pwm_ftm_->CNT = encoder_ftm_->CNT = 0;
+ pwm_ftm_->CNTIN = 0;
+ encoder_ftm_->CNTIN = 0;
+ pwm_ftm_->CNT = 0;
+ encoder_ftm_->CNT = 0;
pwm_ftm_->MOD = counts_per_cycle() - 1;
encoder_ftm_->MOD = controls_->mechanical_counts_per_revolution() - 1;
diff --git a/motors/peripheral/adc_dma.cc b/motors/peripheral/adc_dma.cc
index d2dc6c9..15d50eb 100644
--- a/motors/peripheral/adc_dma.cc
+++ b/motors/peripheral/adc_dma.cc
@@ -147,17 +147,16 @@
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 constexpr long kResultABOffset =
+ static_cast<long>(offsetof(KINETIS_ADC_t, RB)) -
+ static_cast<long>(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 constexpr long kResultOffsetBits = ConstexprLog2(kResultABOffset * 2);
+ static constexpr long kSC1ABOffset =
+ static_cast<long>(offsetof(KINETIS_ADC_t, SC1B)) -
+ static_cast<long>(offsetof(KINETIS_ADC_t, SC1A));
static_assert(kSC1ABOffset > 0, "Offset is backwards");
- static constexpr ssize_t kSC1OffsetBits = ConstexprLog2(kSC1ABOffset * 2);
+ static constexpr long 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);
@@ -204,16 +203,16 @@
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 */;
+ result_dma(adc)->CITER = link | 4 /* 4 minor iterations */;
+ 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 */;
+ reconfigure_dma(adc)->CITER = link | 4 /* 4 minor iterations */;
+ reconfigure_dma(adc)->BITER = link | 4 /* 4 minor iterations */;
}
result_dma(adc)->DLASTSGA = -(kNumberAdcSamples * sizeof(uint16_t));
reconfigure_dma(adc)->DLASTSGA = 0;
diff --git a/motors/pistol_grip/controller.cc b/motors/pistol_grip/controller.cc
index 5a0d5a2..a88b9a0 100644
--- a/motors/pistol_grip/controller.cc
+++ b/motors/pistol_grip/controller.cc
@@ -792,7 +792,8 @@
uint32_t motor0_sum = 0, motor1_sum = 0, wheel_sum = 0;
// First clear both encoders.
- MOTOR0_ENCODER_FTM->CNT = MOTOR1_ENCODER_FTM->CNT = 0;
+ MOTOR0_ENCODER_FTM->CNT = 0;
+ MOTOR1_ENCODER_FTM->CNT = 0;
for (int i = 0; i < kNumberSamples; ++i) {
delay(1);
@@ -847,7 +848,10 @@
PORTA_PCR13 = PORT_PCR_DSE | PORT_PCR_MUX(2);
// .1ms filter time.
- PORTA_DFWR = PORTC_DFWR = PORTD_DFWR = PORTE_DFWR = 6000;
+ PORTA_DFWR = 6000;
+ PORTC_DFWR = 6000;
+ PORTD_DFWR = 6000;
+ PORTE_DFWR = 6000;
// BTN0
PORTC_PCR7 = PORT_PCR_PE | PORT_PCR_PS | PORT_PCR_MUX(1);
@@ -946,8 +950,8 @@
PIT_TCTRL3 = PIT_TCTRL_TIE | PIT_TCTRL_TEN;
// Have them both wait for the GTB signal.
- FTM0->CONF = FTM3->CONF =
- FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+ FTM0->CONF = FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
+ FTM3->CONF = FTM_CONF_GTBEEN | FTM_CONF_NUMTOF(kSwitchingDivisor - 1);
// Make FTM3's period half of what it should be so we can get it a half-cycle
// out of phase.
const uint32_t original_mod = FTM3->MOD;
diff --git a/third_party/apriltag/apriltag_quad_thresh.c b/third_party/apriltag/apriltag_quad_thresh.c
index e4c8b4e..1ce73ad 100644
--- a/third_party/apriltag/apriltag_quad_thresh.c
+++ b/third_party/apriltag/apriltag_quad_thresh.c
@@ -1014,7 +1014,9 @@
int x = 0;
if (v != 127) {
DO_UNIONFIND2(0, -1);
- DO_UNIONFIND2(1, -1);
+ if (v == 255) {
+ DO_UNIONFIND2(1, -1);
+ }
}
}
diff --git a/third_party/flatbuffers/build_defs.bzl b/third_party/flatbuffers/build_defs.bzl
index c1e568f..5f6d71b 100644
--- a/third_party/flatbuffers/build_defs.bzl
+++ b/third_party/flatbuffers/build_defs.bzl
@@ -21,6 +21,7 @@
"--gen-object-api",
"--gen-compare",
"--keep-prefix",
+ "--bfbs-builtins",
"--cpp-std",
"c++17",
"--require-explicit-ids",
diff --git a/tools/cpp/cortex_m4f_crosstool.pb b/tools/cpp/cortex_m4f_crosstool.pb
deleted file mode 100644
index 68114e6..0000000
--- a/tools/cpp/cortex_m4f_crosstool.pb
+++ /dev/null
@@ -1,240 +0,0 @@
-toolchain {
- abi_version: "%NAME%"
- abi_libc_version: "%NAME%"
- builtin_sysroot: ""
- compiler: "gcc"
- host_system_name: "local"
- needsPic: false
- supports_gold_linker: false
- supports_incremental_linker: false
- supports_fission: false
- supports_interface_shared_objects: false
- supports_normalizing_ar: false
- supports_start_end_lib: false
- supports_thin_archives: false
- target_libc: "%NAME%"
- target_cpu: "%NAME%"
- target_system_name: "%NAME%"
- toolchain_identifier: "%NAME%"
-
- tool_path { name: "ar" path: "gcc_arm_none_eabi/arm-none-eabi-ar" }
- tool_path { name: "compat-ld" path: "gcc_arm_none_eabi/arm-none-eabi-ld" }
- tool_path { name: "cpp" path: "gcc_arm_none_eabi/arm-none-eabi-cpp" }
- tool_path { name: "dwp" path: "gcc_arm_none_eabi/arm-none-eabi-dwp" }
- tool_path { name: "gcc" path: "gcc_arm_none_eabi/arm-none-eabi-gcc" }
- tool_path { name: "gcov" path: "gcc_arm_none_eabi/arm-none-eabi-gcov" }
- # C(++) compiles invoke the compiler (as that is the one knowing where
- # to find libraries), but we provide LD so other rules can invoke the linker.
- tool_path { name: "ld" path: "gcc_arm_none_eabi/arm-none-eabi-ld" }
- tool_path { name: "nm" path: "gcc_arm_none_eabi/arm-none-eabi-nm" }
- tool_path { name: "objcopy" path: "gcc_arm_none_eabi/arm-none-eabi-objcopy" }
- objcopy_embed_flag: "-I"
- objcopy_embed_flag: "binary"
- tool_path { name: "objdump" path: "gcc_arm_none_eabi/arm-none-eabi-objdump" }
- tool_path { name: "strip" path: "gcc_arm_none_eabi/arm-none-eabi-strip" }
- linking_mode_flags { mode: FULLY_STATIC }
-
- # TODO(bazel-team): In theory, the path here ought to exactly match the path
- # used by gcc. That works because bazel currently doesn't track files at
- # absolute locations and has no remote execution, yet. However, this will need
- # to be fixed, maybe with auto-detection?
- cxx_builtin_include_directory: '/usr/lib/gcc/arm-none-eabi/4.8/include'
- cxx_builtin_include_directory: '/usr/lib/gcc/arm-none-eabi/4.8/include-fixed'
- cxx_builtin_include_directory: '/usr/lib/arm-none-eabi/include'
- cxx_builtin_include_directory: '/usr/include/newlib',
-
- feature {
- name: "dbg"
- implies: "all_modes"
- flag_set {
- action: "preprocess-assemble"
- action: "c-compile"
- action: "c++-compile"
- action: "c++-header-parsing"
- action: "c++-header-preprocessing"
- action: "c++-module-compile"
- flag_group {
- flag: "-fno-omit-frame-pointer"
- }
- }
- }
-
- feature {
- name: "opt"
- implies: "all_modes"
- }
- feature {
- name: "fastbuild"
- implies: "all_modes"
- }
-
- feature {
- name: "all_modes"
- flag_set {
- action: "preprocess-assemble"
- action: "assemble"
- action: "c-compile"
- flag_group {
- flag: "--std=gnu99"
- }
- }
- flag_set {
- action: "c++-compile"
- action: "c++-header-parsing"
- action: "c++-header-preprocessing"
- action: "c++-module-compile"
- flag_group {
- flag: "--std=gnu++1z"
- flag: "-fno-exceptions"
- flag: "-fno-rtti"
- }
- }
- }
-
- # Anticipated future default.
- # This makes GCC and Clang do what we want when called through symlinks.
- unfiltered_cxx_flag: "-no-canonical-prefixes"
- linker_flag: "-no-canonical-prefixes"
-
- # Things that the code wants defined.
- compiler_flag: "-D__STDC_FORMAT_MACROS"
- compiler_flag: "-D__STDC_CONSTANT_MACROS"
- compiler_flag: "-D__STDC_LIMIT_MACROS"
-
- # Some identifiers for what MCU we're using.
- compiler_flag: "-D%CPU%"
- compiler_flag: "-DF_CPU=%F_CPU%"
-
- compiler_flag: "-Wl,--gc-sections"
-
- # Newlib's stdint.h does this, but GCC's freestanding stdint.h doesn't use
- # newlib's so we have to do it manually...
- compiler_flag: "-D__have_long32"
-
- # Make C++ compilation deterministic. Use linkstamping instead of these
- # compiler symbols.
- unfiltered_cxx_flag: "-Wno-builtin-macro-redefined"
- unfiltered_cxx_flag: "-D__DATE__=\"redacted\""
- unfiltered_cxx_flag: "-D__TIMESTAMP__=\"redacted\""
- unfiltered_cxx_flag: "-D__TIME__=\"redacted\""
-
- # Security hardening on by default.
- # Conservative choice; -D_FORTIFY_SOURCE=2 may be unsafe in some cases.
- # We need to undef it before redefining it as some distributions now have
- # it enabled by default.
- compiler_flag: "-fstack-protector"
- compiler_flag: "-mcpu=cortex-m4"
- compiler_flag: "-mfpu=fpv4-sp-d16"
- compiler_flag: "-mthumb"
- compiler_flag: "-mfloat-abi=hard"
- compiler_flag: "-fno-strict-aliasing"
- linker_flag: "-mcpu=cortex-m4"
- linker_flag: "-mfpu=fpv4-sp-d16"
- linker_flag: "-mthumb"
- linker_flag: "-mfloat-abi=hard"
- linker_flag: "-fno-strict-aliasing"
- linker_flag: "--specs=nano.specs"
-
- # Pretty much everything needs this, including parts of the glibc STL...
- linker_flag: "-lgcc"
- linker_flag: "-lstdc++_nano"
- linker_flag: "-lm"
- linker_flag: "-lc_nano"
- linker_flag: "-T%LINKER_SCRIPT%"
- linker_flag: "-Tmotors/core/kinetis_sections.ld"
-
- compiler_flag: "-fmessage-length=80"
- compiler_flag: "-fmax-errors=20"
-
- compiler_flag: "-Wall"
- compiler_flag: "-Wextra"
- compiler_flag: "-Wpointer-arith"
- compiler_flag: "-Wcast-qual"
- compiler_flag: "-Wwrite-strings"
- compiler_flag: "-Wtype-limits"
- compiler_flag: "-Wsign-compare"
- compiler_flag: "-Wformat=2"
- compiler_flag: "-Werror"
- compiler_flag: "-Wstrict-aliasing=2"
-
- # TODO(Brian): Drop these once we upgrade Eigen.
- compiler_flag: "-Wno-misleading-indentation"
- compiler_flag: "-Wno-int-in-bool-context"
-
- # Be annoying about using doubles places we probably didn't mean to, because
- # the FPU only does single-precision.
- compiler_flag: "-Wdouble-promotion"
-
- # Don't use temp files while compiling.
- compiler_flag: "-pipe"
-
- # Stamp the binary with a unique identifier.
- # TODO(austin): Put these back in.
- #linker_flag: "-Wl,--build-id=md5"
- #linker_flag: "-Wl,--hash-style=gnu"
-
- # Enable debug symbols.
- compiler_flag: "-g"
-
- # Common symbols are weird and not what we want, so just give multiple
- # declaration errors instead.
- compiler_flag: "-fno-common"
-
- # We're not a hosted environment (no file IO, main is called from our code,
- # etc).
- compiler_flag: "-ffreestanding"
- # However, we still want to optimize things like memcpy.
- compiler_flag: "-fbuiltin"
-
- compilation_mode_flags {
- mode: OPT
-
- # Freescale recommends this combination for reducing cycle count.
- # http://www.nxp.com/assets/documents/data/en/application-notes/AN4808.pdf
- compiler_flag: "-O2"
- compiler_flag: "-finline-functions"
-
- # This is definitely worth it for us. It makes the FPU a lot more useful,
- # especially with complex arithmetic, which matters a lot.
- compiler_flag: "-ffast-math"
-
- # It seems like this is a good idea, at least for the number crunching code.
- # Might want to look into moving it to copts on specific rules if the code
- # size increase becomes a problem.
- compiler_flag: "-funroll-loops"
-
- # Disable assertions
- compiler_flag: "-DNDEBUG"
-
- # Removal of unused code and data at link time (can this increase binary size in some cases?).
- compiler_flag: "-ffunction-sections"
- #compiler_flag: "-fdata-sections"
- linker_flag: "-Wl,--gc-sections"
- }
-
- feature {
- name: 'include_paths'
- flag_set {
- action: 'preprocess-assemble'
- action: 'c-compile'
- action: 'c++-compile'
- action: 'c++-header-parsing'
- action: 'c++-header-preprocessing'
- action: 'c++-module-compile'
- flag_group {
- iterate_over: 'quote_include_paths'
- flag: '-iquote'
- flag: '%{quote_include_paths}'
- }
- flag_group {
- iterate_over: 'include_paths'
- flag: '-I%{include_paths}'
- }
- flag_group {
- iterate_over: 'system_include_paths'
- flag: '-I'
- flag: '%{system_include_paths}'
- }
- }
- }
-}
diff --git a/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc b/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
index 6ff0448..b7146d0 100755
--- a/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
+++ b/tools/cpp/gcc_arm_none_eabi/arm-none-eabi-gcc
@@ -1,6 +1,6 @@
#!/bin/bash --norc
-PATH="${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/lib/gcc/arm-none-eabi/7.3.1:$PATH" \
+PATH="${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/lib/gcc/arm-none-eabi/13.2.1:${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/libexec/gcc/arm-none-eabi/13.2.1:${PATH}" \
exec \
${BAZEL_OUTPUT_ROOT}external/gcc_arm_none_eabi/bin/arm-none-eabi-gcc \
"$@"
diff --git a/tools/cpp/toolchain_config.bzl b/tools/cpp/toolchain_config.bzl
index 8bab5c7..5e320cd 100644
--- a/tools/cpp/toolchain_config.bzl
+++ b/tools/cpp/toolchain_config.bzl
@@ -296,7 +296,6 @@
"-pipe",
"-g",
"-fno-common",
- "-ffreestanding",
"-fbuiltin",
],
),
@@ -797,7 +796,7 @@
actions = all_cpp_compile_actions,
flag_groups = [
flag_group(
- flags = ["--std=gnu++1z", "-fno-exceptions", "-fno-rtti"],
+ flags = ["--std=gnu++20", "-fno-exceptions", "-fno-rtti"],
),
],
),
diff --git a/tools/dependency_rewrite b/tools/dependency_rewrite
index 2cc90cf..2d2e610 100644
--- a/tools/dependency_rewrite
+++ b/tools/dependency_rewrite
@@ -3,6 +3,7 @@
block *
rewrite maven.ctr-electronics.com/(.*) software.frc971.org/Build-Dependencies/maven.ctr-electronics.com/$1
rewrite github.com/(.*) software.frc971.org/Build-Dependencies/github.com/$1
+rewrite developer.arm.com/(.*) software.frc971.org/Build-Dependencies/developer.arm.com/$1
rewrite dl.google.com/(.*) software.frc971.org/Build-Dependencies/dl.google.com/$1
rewrite mirror.bazel.build/(.*) software.frc971.org/Build-Dependencies/mirror.bazel.build/$1
rewrite nodejs.org/(.*) software.frc971.org/Build-Dependencies/nodejs.org/$1
diff --git a/tools/python/requirements.lock.txt b/tools/python/requirements.lock.txt
index 7470cae..a8a8f4d 100644
--- a/tools/python/requirements.lock.txt
+++ b/tools/python/requirements.lock.txt
@@ -612,6 +612,10 @@
--hash=sha256:47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc \
--hash=sha256:c083dd0dce68dbfbe1129d5271cb90f9447dea7d52097c6e0126120c521ddea8
# via requests
+validators==0.22.0 \
+ --hash=sha256:61cf7d4a62bbae559f2e54aed3b000cea9ff3e2fdbe463f51179b92c58c9585a \
+ --hash=sha256:77b2689b172eeeb600d9605ab86194641670cdb73b60afd577142a9397873370
+ # via -r tools/python/requirements.txt
watchdog==2.1.9 \
--hash=sha256:083171652584e1b8829581f965b9b7723ca5f9a2cd7e20271edf264cfd7c1412 \
--hash=sha256:117ffc6ec261639a0209a3252546b12800670d4bf5f84fbd355957a0595fe654 \
diff --git a/tools/python/requirements.txt b/tools/python/requirements.txt
index 3b3ab3a..0a5f24a 100644
--- a/tools/python/requirements.txt
+++ b/tools/python/requirements.txt
@@ -12,6 +12,7 @@
requests
scipy
shapely
+validators
yapf
# TODO(phil): Migrate to absl-py. These are abandoned as far as I can tell.
diff --git a/tools/python/whl_overrides.json b/tools/python/whl_overrides.json
index 108a544..6cb9998 100644
--- a/tools/python/whl_overrides.json
+++ b/tools/python/whl_overrides.json
@@ -143,6 +143,10 @@
"sha256": "47cc05d99aaa09c9e72ed5809b60e7ba354e64b59c9c173ac3018642d8bb41fc",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/urllib3-1.26.13-py2.py3-none-any.whl"
},
+ "validators==0.22.0": {
+ "sha256": "61cf7d4a62bbae559f2e54aed3b000cea9ff3e2fdbe463f51179b92c58c9585a",
+ "url": "https://software.frc971.org/Build-Dependencies/wheelhouse/validators-0.22.0-py3-none-any.whl"
+ },
"watchdog==2.1.9": {
"sha256": "4f4e1c4aa54fb86316a62a87b3378c025e228178d55481d30d857c6c438897d6",
"url": "https://software.frc971.org/Build-Dependencies/wheelhouse/watchdog-2.1.9-py3-none-manylinux2014_x86_64.whl"
diff --git a/tools/rehosting/BUILD b/tools/rehosting/BUILD
new file mode 100644
index 0000000..0eb1a18
--- /dev/null
+++ b/tools/rehosting/BUILD
@@ -0,0 +1,11 @@
+py_test(
+ name = "rehost_test",
+ srcs = ["rehost_test.py"],
+ deps = [":rehost"],
+)
+
+py_binary(
+ name = "rehost",
+ srcs = ["rehost.py"],
+ deps = ["@pip//validators"],
+)
diff --git a/tools/rehosting/README b/tools/rehosting/README
new file mode 100644
index 0000000..42da3bf
--- /dev/null
+++ b/tools/rehosting/README
@@ -0,0 +1,6 @@
+Notes on setup:
+
+1. The buildkite pipeline is set up to enforce that we only run this against
+ the master branch, to prevent users from running un-checked-in scripts to
+ muck with the build dependency server.
+2. Runs against the queue=deploy buildkite queue.
diff --git a/tools/rehosting/rehost.py b/tools/rehosting/rehost.py
new file mode 100644
index 0000000..8e5cd1e
--- /dev/null
+++ b/tools/rehosting/rehost.py
@@ -0,0 +1,61 @@
+from urllib.request import urlopen
+from urllib.parse import urlparse
+import validators
+import shutil
+import stat
+from pathlib import Path
+import os
+import sys
+
+BUILD_DEPENDENCIES_PATH = "/data/files/frc971/Build-Dependencies/"
+WWW_GROUP = "www-data"
+
+
+def get_url() -> str:
+ return sys.argv[1]
+
+
+def validate_url(url: str) -> str:
+ # We have no reason to allow people do download things from IP addresses directly.
+ if not validators.url(
+ url, simple_host=True, skip_ipv4_addr=True, skip_ipv6_addr=True):
+ raise ValueError(f"Invalid URL {url}")
+ return url
+
+
+def url_to_path(url: str) -> Path:
+ parsed = urlparse(url)
+ # Strip out the http:// and any other extraneous junk:
+ path = (Path(BUILD_DEPENDENCIES_PATH) /
+ (parsed.netloc + parsed.path)).resolve()
+ # Confirm that someone didn't sneak in a URL that looks like http://foo.bar/../../../.. or something.
+ path.relative_to(BUILD_DEPENDENCIES_PATH)
+ if path.exists():
+ raise FileExistsError(f"There is already a file uploaded for {url}.")
+ return path
+
+
+def download():
+ url = validate_url(get_url())
+ path = url_to_path(url)
+ path.parent.mkdir(mode=0o775, parents=True, exist_ok=True)
+
+ with urlopen(url) as downloaded:
+ with open(path, 'wb') as output:
+ output.write(downloaded.read())
+
+ relative_path = path.relative_to(BUILD_DEPENDENCIES_PATH)
+ path.chmod(stat.S_IRUSR | stat.S_IRGRP | stat.S_IROTH)
+ try:
+ shutil.chown(path, group=WWW_GROUP)
+ for parent in relative_path.parents:
+ shutil.chown(Path(BUILD_DEPENDENCIES_PATH) / parent,
+ group=WWW_GROUP)
+ except Exception:
+ # The chown's sometimes fail if they get to a manually-created/touched
+ # directory; don't worry about that if it happens..
+ pass
+
+
+if __name__ == "__main__":
+ download()
diff --git a/tools/rehosting/rehost.sh b/tools/rehosting/rehost.sh
new file mode 100755
index 0000000..445d05e
--- /dev/null
+++ b/tools/rehosting/rehost.sh
@@ -0,0 +1,2 @@
+#!/bin/bash
+tools/bazel run //tools/rehosting:rehost -- "$(buildkite-agent meta-data get dependency-url)"
diff --git a/tools/rehosting/rehost_test.py b/tools/rehosting/rehost_test.py
new file mode 100644
index 0000000..da316e2
--- /dev/null
+++ b/tools/rehosting/rehost_test.py
@@ -0,0 +1,37 @@
+import unittest
+import unittest.mock
+from tools.rehosting import rehost
+from pathlib import Path
+import os
+
+
+class TestRehost(unittest.TestCase):
+
+ def test_url_validation(self):
+ self.assertEqual("http://google.com",
+ rehost.validate_url("http://google.com"))
+ self.assertRaisesRegex(Exception, "Invalid URL", rehost.validate_url,
+ "file:///some/secret")
+ self.assertRaisesRegex(Exception, "Invalid URL", rehost.validate_url,
+ "http://10.0.0.0/secret")
+
+ def test_url_to_path(self):
+ test_dir = os.getenv("TEST_TMPDIR", "/tmp/")
+ with unittest.mock.patch.object(rehost, "BUILD_DEPENDENCIES_PATH",
+ test_dir):
+ existing_file = test_dir + "/exists.com"
+ with open(existing_file, 'w') as f:
+ f.write('string')
+ self.assertEqual(
+ Path(test_dir) / "example.com/foo/bar",
+ rehost.url_to_path("https://example.com/foo/bar"))
+ self.assertRaisesRegex(ValueError,
+ f"not in the subpath of '{test_dir}'",
+ rehost.url_to_path,
+ "https://example.com/../../bar")
+ self.assertRaisesRegex(FileExistsError, "There is already a file",
+ rehost.url_to_path, "https://exists.com")
+
+
+if __name__ == "__main__":
+ unittest.main()
diff --git a/y2019/vision/constants.h b/y2019/vision/constants.h
index 8510f4c..959f5b7 100644
--- a/y2019/vision/constants.h
+++ b/y2019/vision/constants.h
@@ -3,6 +3,7 @@
#include <array>
#include <cmath>
+#include <cstdint>
#include <string>
namespace y2019 {
diff --git a/y2023/www/BUILD b/y2023/www/BUILD
index 404247e..94a1a13 100644
--- a/y2023/www/BUILD
+++ b/y2023/www/BUILD
@@ -33,6 +33,8 @@
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
"//frc971/control_loops:control_loops_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_can_position_ts_fbs",
+ "//frc971/control_loops/drivetrain:drivetrain_position_ts_fbs",
"//frc971/control_loops/drivetrain:drivetrain_status_ts_fbs",
"//frc971/control_loops/drivetrain/localization:localizer_output_ts_fbs",
"//frc971/vision:target_map_ts_fbs",
diff --git a/y2023/www/field.html b/y2023/www/field.html
index 6bd2fc0..6fe291d 100644
--- a/y2023/www/field.html
+++ b/y2023/www/field.html
@@ -47,6 +47,10 @@
<td>Wrist</td>
<td id="wrist"> NA </td>
</tr>
+ <tr>
+ <td>Wrist Absolute Encoder Value</td>
+ <td id="wrist_encoder"> NA </td>
+ </tr>
</table>
<table>
<tr>
@@ -87,13 +91,37 @@
<td id="arm_roll"> NA </td>
</tr>
<tr>
+ <td>Roll Joint Potentiometer Value</td>
+ <td id="arm_roll_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Roll Joint Absolute Encoder Value</td>
+ <td id="arm_roll_encoder"> NA </td>
+ </tr>
+ <tr>
<td>Proximal</td>
<td id="arm_proximal"> NA </td>
</tr>
<tr>
+ <td>Proximal Potentiometer Value</td>
+ <td id="arm_proximal_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Proximal Absolute Encoder Value</td>
+ <td id="arm_proximal_encoder"> NA </td>
+ </tr>
+ <tr>
<td>Distal</td>
<td id="arm_distal"> NA </td>
</tr>
+ <tr>
+ <td>Distal Potentiometer Value</td>
+ <td id="arm_distal_pot"> NA </td>
+ </tr>
+ <tr>
+ <td>Distal Absolute Encoder Value</td>
+ <td id="arm_distal_encoder"> NA </td>
+ </tr>
</table>
<h3>Zeroing Faults:</h3>
@@ -110,6 +138,43 @@
</div>
</div>
</div>
+ <table>
+ <tr>
+ <th colspan="2"> Drivetrain Encoder Positions </th>
+ </tr>
+ <tr>
+ <td> Left Encoder Position</td>
+ <td id="left_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Encoder Position</td>
+ <td id="right_drivetrain_encoder"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Front Falcon CAN Position</td>
+ <td id="falcon_right_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Back Falcon CAN Position</td>
+ <td id="falcon_right_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Right Under Falcon CAN Position</td>
+ <td id="falcon_right_under"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Front Falcon CAN Position</td>
+ <td id="falcon_left_front"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Back Falcon CAN Position</td>
+ <td id="falcon_left_back"> NA </td>
+ </tr>
+ <tr>
+ <td> Left Under Falcon CAN Position</td>
+ <td id="falcon_left_under"> NA </td>
+ </tr>
+ </table>
</body>
</html>
diff --git a/y2023/www/field_handler.ts b/y2023/www/field_handler.ts
index 6952a41..d6ff479 100644
--- a/y2023/www/field_handler.ts
+++ b/y2023/www/field_handler.ts
@@ -3,6 +3,8 @@
import {ServerStatistics, State as ConnectionState} from '../../aos/network/message_bridge_server_generated'
import {Connection} from '../../aos/network/www/proxy'
import {ZeroingError} from '../../frc971/control_loops/control_loops_generated'
+import {Position as DrivetrainPosition} from '../../frc971/control_loops/drivetrain/drivetrain_position_generated'
+import {CANPosition as DrivetrainCANPosition} from '../../frc971/control_loops/drivetrain/drivetrain_can_position_generated'
import {Status as DrivetrainStatus} from '../../frc971/control_loops/drivetrain/drivetrain_status_generated'
import {LocalizerOutput} from '../../frc971/control_loops/drivetrain/localization/localizer_output_generated'
import {TargetMap} from '../../frc971/vision/target_map_generated'
@@ -27,6 +29,8 @@
private canvas = document.createElement('canvas');
private localizerOutput: LocalizerOutput|null = null;
private drivetrainStatus: DrivetrainStatus|null = null;
+ private drivetrainPosition: DrivetrainPosition|null = null;
+ private drivetrainCANPosition: DrivetrainCANPosition|null = null;
private superstructureStatus: SuperstructureStatus|null = null;
// Image information indexed by timestamp (seconds since the epoch), so that
@@ -52,6 +56,8 @@
(document.getElementById('end_effector_state') as HTMLElement);
private wrist: HTMLElement =
(document.getElementById('wrist') as HTMLElement);
+ private wristAbsoluteEncoder: HTMLElement =
+ (document.getElementById('wrist_encoder') as HTMLElement);
private armState: HTMLElement =
(document.getElementById('arm_state') as HTMLElement);
private gamePiece: HTMLElement =
@@ -68,8 +74,38 @@
(document.getElementById('arm_proximal') as HTMLElement);
private distal: HTMLElement =
(document.getElementById('arm_distal') as HTMLElement);
+ private rollPotentiometer: HTMLElement =
+ (document.getElementById('arm_roll_pot') as HTMLElement);
+ private rollAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_roll_encoder') as HTMLElement);
+ private proximalPotentiometer: HTMLElement =
+ (document.getElementById('arm_proximal_pot') as HTMLElement);
+ private proximalAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_proximal_encoder') as HTMLElement);
+ private distalPotentiometer: HTMLElement =
+ (document.getElementById('arm_distal_pot') as HTMLElement);
+ private distalAbsoluteEncoder: HTMLElement =
+ (document.getElementById('arm_distal_encoder') as HTMLElement);
private zeroingFaults: HTMLElement =
(document.getElementById('zeroing_faults') as HTMLElement);
+
+ private leftDrivetrainEncoder: HTMLElement =
+ (document.getElementById('left_drivetrain_encoder') as HTMLElement);
+ private rightDrivetrainEncoder: HTMLElement =
+ (document.getElementById('right_drivetrain_encoder') as HTMLElement);
+ private falconRightFrontPosition: HTMLElement =
+ (document.getElementById('falcon_right_front') as HTMLElement);
+ private falconRightBackPosition: HTMLElement =
+ (document.getElementById('falcon_right_back') as HTMLElement);
+ private falconRightUnderPosition: HTMLElement =
+ (document.getElementById('falcon_right_under') as HTMLElement);
+ private falconLeftFrontPosition: HTMLElement =
+ (document.getElementById('falcon_left_front') as HTMLElement);
+ private falconLeftBackPosition: HTMLElement =
+ (document.getElementById('falcon_left_back') as HTMLElement);
+ private falconLeftUnderPosition: HTMLElement =
+ (document.getElementById('falcon_left_under') as HTMLElement);
+
constructor(private readonly connection: Connection) {
(document.getElementById('field') as HTMLElement).appendChild(this.canvas);
@@ -155,6 +191,14 @@
this.handleDrivetrainStatus(data);
});
this.connection.addHandler(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Position', (data) => {
+ this.handleDrivetrainPosition(data);
+ });
+ this.connection.addHandler(
+ '/drivetrain', 'y2023.control_loops.drivetrain.CANPosition', (data) => {
+ this.handleDrivetrainCANPosition(data);
+ });
+ this.connection.addHandler(
'/localizer', 'frc971.controls.LocalizerOutput', (data) => {
this.handleLocalizerOutput(data);
});
@@ -210,6 +254,16 @@
this.drivetrainStatus = DrivetrainStatus.getRootAsStatus(fbBuffer);
}
+ private handleDrivetrainPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainPosition = DrivetrainPosition.getRootAsPosition(fbBuffer);
+ }
+
+ private handleDrivetrainCANPosition(data: Uint8Array): void {
+ const fbBuffer = new ByteBuffer(data);
+ this.drivetrainCANPosition = DrivetrainCANPosition.getRootAsCANPosition(fbBuffer);
+ }
+
private handleSuperstructureStatus(data: Uint8Array): void {
const fbBuffer = new ByteBuffer(data);
this.superstructureStatus = SuperstructureStatus.getRootAsStatus(fbBuffer);
@@ -386,6 +440,8 @@
this.superstructureStatus.wrist().estimatorState().position(),
1e-3);
}
+ this.wristAbsoluteEncoder.innerHTML = this.superstructureStatus.wrist()
+ .estimatorState().position().toString();
this.armState.innerHTML =
ArmState[this.superstructureStatus.arm().state()];
this.gamePiece.innerHTML = Class[this.superstructureStatus.gamePiece()];
@@ -399,6 +455,14 @@
.rollJointEstimatorState()
.position()
.toFixed(2);
+ this.rollPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .rollJointEstimatorState()
+ .potPosition()
+ .toString();
+ this.rollAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .rollJointEstimatorState()
+ .absolutePosition()
+ .toString();
this.proximal.innerHTML = this.superstructureStatus.arm()
.proximalEstimatorState()
.position()
@@ -407,6 +471,22 @@
.distalEstimatorState()
.position()
.toFixed(2);
+ this.proximalPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .proximalEstimatorState()
+ .potPosition()
+ .toString();
+ this.proximalAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .proximalEstimatorState()
+ .absolutePosition()
+ .toString();
+ this.distalPotentiometer.innerHTML = this.superstructureStatus.arm()
+ .distalEstimatorState()
+ .potPosition()
+ .toString();
+ this.distalAbsoluteEncoder.innerHTML = this.superstructureStatus.arm()
+ .distalEstimatorState()
+ .absolutePosition()
+ .toString();
let zeroingErrors: string = 'Roll Joint Errors:' +
'<br/>';
for (let i = 0; i < this.superstructureStatus.arm()
@@ -463,6 +543,34 @@
false);
}
+ if (this.drivetrainPosition) {
+ this.leftDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.leftEncoder().toString();
+
+ this.rightDrivetrainEncoder.innerHTML =
+ this.drivetrainPosition.rightEncoder().toString();
+ }
+
+ if (this.drivetrainCANPosition) {
+ this.falconRightFrontPosition.innerHTML = //TODO: (niko) Improve this so that falcons are not hard-coded
+ this.drivetrainCANPosition.falcons(0).position().toString();
+
+ this.falconRightBackPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(1).position().toString();
+
+ this.falconRightUnderPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(2).position().toString();
+
+ this.falconLeftFrontPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(3).position().toString();
+
+ this.falconLeftBackPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(4).position().toString();
+
+ this.falconLeftUnderPosition.innerHTML =
+ this.drivetrainCANPosition.falcons(5).position().toString();
+ }
+
if (this.localizerOutput) {
if (!this.localizerOutput.zeroed()) {
this.setZeroing(this.x);