Merge "Create a folder for the scouting app"
diff --git a/.gitignore b/.gitignore
index 82b963a..8c6aa64 100644
--- a/.gitignore
+++ b/.gitignore
@@ -2,6 +2,10 @@
/bazel-*
/compile_commands.json
+# The `tools/build_rules/apache_runner.py` helper looks for this file by
+# default. We don't want folks to check it in.
+/ldap.json
+
# Hide vagrant's files that unfortunately make it into the source tree when you
# run "vagrant up".
/vm/.vagrant/
diff --git a/WORKSPACE b/WORKSPACE
index c15c957..4aaad5b 100644
--- a/WORKSPACE
+++ b/WORKSPACE
@@ -7,6 +7,10 @@
python_debs = "files",
)
load(
+ "//debian:apache2.bzl",
+ apache2_debs = "files",
+)
+load(
"//debian:patch.bzl",
patch_debs = "files",
)
@@ -82,6 +86,8 @@
generate_repositories_for_debs(ssh_debs)
+generate_repositories_for_debs(apache2_debs)
+
generate_repositories_for_debs(patch_debs)
generate_repositories_for_debs(pandoc_debs)
@@ -490,6 +496,13 @@
)
http_archive(
+ name = "apache2",
+ build_file = "@//debian:apache2.BUILD",
+ sha256 = "98b0ad6d911751ba0aa486429e6278f995e7bbabd928c7d3d44c888fa2bf371b",
+ url = "https://www.frc971.org/Build-Dependencies/apache2.tar.gz",
+)
+
+http_archive(
name = "pandoc",
build_file = "@//debian:pandoc.BUILD",
sha256 = "9f7a7adb3974a1f14715054c349ff3edc2909e920dbe3438fca437a83845f3c4",
diff --git a/build_tests/BUILD b/build_tests/BUILD
index c74407e..ca34de2 100644
--- a/build_tests/BUILD
+++ b/build_tests/BUILD
@@ -1,6 +1,7 @@
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_py_library")
load("@io_bazel_rules_go//go:def.bzl", "go_binary", "go_library")
+load("//tools/build_rules:apache.bzl", "apache_wrapper")
cc_test(
name = "gflags_build_test",
@@ -113,3 +114,15 @@
visibility = ["//visibility:private"],
deps = ["//build_tests/go_greeter"],
)
+
+py_binary(
+ name = "dummy_http_server",
+ srcs = ["dummy_http_server.py"],
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+)
+
+apache_wrapper(
+ name = "apache_https_demo",
+ binary = ":dummy_http_server",
+ target_compatible_with = ["@platforms//cpu:x86_64"],
+)
diff --git a/build_tests/dummy_http_server.py b/build_tests/dummy_http_server.py
new file mode 100644
index 0000000..f65122a
--- /dev/null
+++ b/build_tests/dummy_http_server.py
@@ -0,0 +1,48 @@
+"""This is a dummy server to demonstrate the apache_wrapper() rule.
+
+Don't run this server on its own. Instead run the
+`//build_tests:apache_https_demo` binary.
+
+When you authenticate against Apache via LDAP, this server prints the username
+you authenticated with and the path from the GET request.
+"""
+
+import base64
+from http.server import BaseHTTPRequestHandler, HTTPServer
+import os
+
+def parse_auth(authorization):
+ auth_type, auth_info = authorization.split(" ", maxsplit=1)
+ if auth_type != "Basic":
+ raise ValueError(f"Unknown auth type: {auth_type}")
+ username, _ = base64.b64decode(auth_info).decode().split(":", 1)
+ return username
+
+class Server(BaseHTTPRequestHandler):
+ def _set_response(self):
+ self.send_response(200)
+ self.send_header("Content-type", "text/html")
+ self.end_headers()
+
+ def _write(self, text):
+ self.wfile.write(text.encode("utf-8"))
+
+ def do_GET(self):
+ self._set_response()
+ self._write(f"GET request for {self.path}")
+ self._write("<br/>")
+ username = parse_auth(self.headers["Authorization"])
+ self._write(f"Hello, {username}")
+
+def main():
+ port = int(os.environ["APACHE_WRAPPED_PORT"])
+ server_address = ("", port)
+ httpd = HTTPServer(server_address, Server)
+ try:
+ httpd.serve_forever()
+ except KeyboardInterrupt:
+ pass
+ httpd.server_close()
+
+if __name__ == "__main__":
+ main()
diff --git a/debian/BUILD b/debian/BUILD
index 0540a28..76bcf28 100644
--- a/debian/BUILD
+++ b/debian/BUILD
@@ -3,6 +3,10 @@
python_debs = "files",
)
load(
+ ":apache2.bzl",
+ apache2_debs = "files",
+)
+load(
":patch.bzl",
patch_debs = "files",
)
@@ -112,6 +116,26 @@
)
download_packages(
+ name = "download_apache2_packages",
+ excludes = [
+ "libaprutil1-dbd-mysql",
+ "libaprutil1-dbd-odbc",
+ "libaprutil1-dbd-pgsql",
+ "libaprutil1-dbd-freetds",
+ "libstdc++6",
+ "lsb-base",
+ "debconf",
+ "libc6-dev",
+ ],
+ force_includes = [
+ "libaprutil1",
+ ],
+ packages = [
+ "apache2",
+ ],
+)
+
+download_packages(
name = "download_python_deps",
excludes = [
"libblas.so.3",
@@ -253,6 +277,12 @@
)
generate_deb_tarball(
+ name = "apache2",
+ files = apache2_debs,
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+generate_deb_tarball(
name = "patch",
files = patch_debs,
target_compatible_with = ["@platforms//os:linux"],
diff --git a/debian/apache2.BUILD b/debian/apache2.BUILD
new file mode 100644
index 0000000..7f3f87c
--- /dev/null
+++ b/debian/apache2.BUILD
@@ -0,0 +1,5 @@
+filegroup(
+ name = "all_files",
+ srcs = glob(["**"]),
+ visibility = ["//visibility:public"],
+)
diff --git a/debian/apache2.bzl b/debian/apache2.bzl
new file mode 100644
index 0000000..3d557c9
--- /dev/null
+++ b/debian/apache2.bzl
@@ -0,0 +1,59 @@
+files = {
+ "apache2-bin_2.4.52-1~deb11u2_amd64.deb": "dc0b906cb78fb55238bc1ddda5dac6917cbf1027a4372944e24a1a16aa8c9fdc",
+ "apache2-data_2.4.52-1~deb11u2_all.deb": "8fac5b05668e3d01e97f4ec2e8c52d9b138da0e2109425d6eacbcd005651068e",
+ "apache2-utils_2.4.52-1~deb11u2_amd64.deb": "a164cb0109bfbadb932cbd0a7cf8eeb1b4ed1242bc6e44394f5a343589ddca39",
+ "apache2_2.4.52-1~deb11u2_amd64.deb": "00bd5ca827ea67e0d2a10e6191595b26261f902c66096bd9e2b80fe6b606945a",
+ "debconf_1.5.77_all.deb": "d9ee4dff77aaad12674eed3ccefdcccd332424c9e2ac2ac00a37a1e06c84ab70",
+ "dpkg_1.20.9_amd64.deb": "ac90a4705b36f2d952a4a745ad944b33ff60c5ec69a5f89e4559a89bd0c53e01",
+ "gcc-10-base_10.2.1-6_amd64.deb": "be65535e94f95fbf04b104e8ab36790476f063374430f7dfc6c516cbe2d2cd1e",
+ "init-system-helpers_1.60_all.deb": "43420922c5e3aa747f8854236bf381a35179bba3885b242edb104751dad20644",
+ "libacl1_2.2.53-10_amd64.deb": "aa18d721be8aea50fbdb32cd9a319cb18a3f111ea6ad17399aa4ba9324c8e26a",
+ "libapr1_1.7.0-6+deb11u1_amd64.deb": "b95ce92c2530ba9c3a9af906f6ad27a0894efbefe9f0a2d9f2f91c6bdcdaf874",
+ "libaprutil1-dbd-sqlite3_1.6.1-5_amd64.deb": "18778f0cfc866d6230ab4f14ec3248d214e0f2a6efec5ded409aa6709d2d9255",
+ "libaprutil1-ldap_1.6.1-5_amd64.deb": "902a329ef05b94380ba9f7e413bd53086e096dc20cb0a913a4f346febc1e69e5",
+ "libaprutil1_1.6.1-5_amd64.deb": "d6aa77a54ed7533ea5c00240a677009f1b5d4e85f1607980d51ba35768c2eb39",
+ "libbrotli1_1.0.9-2+b2_amd64.deb": "65ca7d8b03e9dac09c5d544a89dd52d1aeb74f6a19583d32e4ff5f0c77624c24",
+ "libbz2-1.0_1.0.8-4_amd64.deb": "16e27c3ebd97981e70db3733f899963362748f178a62644df69d1f247e741379",
+ "libc6_2.31-13+deb11u2_amd64.deb": "3d9421c3fc0ef0d8ce57c0a149e1f8dbad78aba067f120be9e652af28902e346",
+ "libcrypt1_4.4.18-4_amd64.deb": "f617952df0c57b4ee039448e3941bccd3f97bfff71e9b0f87ca6dae15cb3f5ef",
+ "libcurl4_7.74.0-1.3+deb11u1_amd64.deb": "6f9c494eecc920899bb2c72d1a507a34b3703105778b0b9b9ae9aebdbdffcaab",
+ "libdb5.3_5.3.28+dfsg1-0.8_amd64.deb": "00b9e63e287f45300d4a4f59b6b88e25918443c932ae3e5845d5761ae193c530",
+ "libexpat1_2.2.10-2_amd64.deb": "eda6663f34375a9456c8c701002f1271bc90ac2627b9fb0892474e65eae1b668",
+ "libgcc-s1_10.2.1-6_amd64.deb": "e478f2709d8474165bb664de42e16950c391f30eaa55bc9b3573281d83a29daf",
+ "libgcrypt20_1.8.7-6_amd64.deb": "7a2e0eef8e0c37f03f3a5fcf7102a2e3dc70ba987f696ab71949f9abf36f35ef",
+ "libgdbm-compat4_1.19-2_amd64.deb": "e62caed68b0ffaa03b5fa539d6fdc08c4151f66236d5878949bead0b71b7bb09",
+ "libgdbm6_1.19-2_amd64.deb": "e54cfe4d8b8f209bb7df31a404ce040f7c2f9b1045114a927a7e1061cdf90727",
+ "libgpg-error0_1.38-2_amd64.deb": "16a507fb20cc58b5a524a0dc254a9cb1df02e1ce758a2d8abde0bc4a3c9b7c26",
+ "libicu67_67.1-7_amd64.deb": "2bf5c46254f527865bfd6368e1120908755fa57d83634bd7d316c9b3cfd57303",
+ "libjansson4_2.13.1-1.1_amd64.deb": "7025d03e4ad4177a06692bd8e6edcbdebf7fd3897e5f29b70ae968e17e57f6fa",
+ "liblua5.3-0_5.3.3-1.1+b1_amd64.deb": "98d5a85075a936ff014f639552882914064e46bfbbdc4bc80e80db33a9c77602",
+ "liblz4-1_1.9.3-2_amd64.deb": "79ac6e9ca19c483f2e8effcc3401d723dd9dbb3a4ae324714de802adb21a8117",
+ "liblzma5_5.2.5-2_amd64.deb": "eed58eb6b62208685dbe42a05582e78bd48c8d879d7ae0b61f114265d18824db",
+ "libncurses6_6.2+20201114-2_amd64.deb": "dfe45cb6ab048d1182175df55b007a4a188515c6d764a4dd5a44a0b47b6286a1",
+ "libncursesw6_6.2+20201114-2_amd64.deb": "ee3cd315dfa18865cf888ba6813a552077a4f3d1439dd225e4a0d0fee53aadc2",
+ "libnghttp2-14_1.43.0-1_amd64.deb": "a1a8aae24ced43025c94a9cb0c0eabfb3fc070785de9ee51c9a3a4fe86f0d11e",
+ "libpcre2-8-0_10.36-2_amd64.deb": "d31e4d6c04e847194b36b13ab59c578775aa12f04c48a840922796bd1f5eb32a",
+ "libpcre3_8.39-13_amd64.deb": "48efcf2348967c211cd9408539edf7ec3fa9d800b33041f6511ccaecc1ffa9d0",
+ "libperl5.32_5.32.1-4+deb11u2_amd64.deb": "224cafe65968deb83168113b74dff2d2f13b115a41d99eb209ed3b8f981df0b3",
+ "libprocps8_3.3.17-5_amd64.deb": "0a60017f0229cd4eec95b9f354c68312cc4ca4770ba8c01f545fd9c02b34e8a0",
+ "libpsl5_0.21.0-1.2_amd64.deb": "d716f5b4346ec85bb728f4530abeb1da4a79f696c72d7f774c59ba127c202fa7",
+ "librtmp1_2.4+20151223.gitfa8646d.1-2+b2_amd64.deb": "e1f69020dc2c466e421ec6a58406b643be8b5c382abf0f8989011c1d3df91c87",
+ "libselinux1_3.1-3_amd64.deb": "339f5ede10500c16dd7192d73169c31c4b27ab12130347275f23044ec8c7d897",
+ "libsqlite3-0_3.34.1-3_amd64.deb": "a0b8d3acf4a0483048637637d269be93af48d5c16f6f139f53edd13384ad4686",
+ "libssh2-1_1.9.0-2_amd64.deb": "f730fe45716a206003597819ececeeffe0fff754bdbbd0105425a177aa20a2de",
+ "libssl1.1_1.1.1k-1+deb11u1_amd64.deb": "82e6ded36e4fa4c28dcec00369b36ee242975f4c110f755a970a56d03d410ffb",
+ "libsystemd0_247.3-6_amd64.deb": "8c948d9d97178e6617f549822db2b89e23b1bfa1ee745ffbf0e41b6ee64f8737",
+ "libtinfo6_6.2+20201114-2_amd64.deb": "aeaf942c71ecc0ed081efdead1a1de304dcd513a9fc06791f26992e76986597b",
+ "libuuid1_2.36.1-8_amd64.deb": "94f13f58ac45ae850559e6bfe1a02be72566c66761e628a2599cc85066cb84d3",
+ "libxml2_2.9.10+dfsg-6.7_amd64.deb": "023296a15e1a28607609cb15c7ca0dd8a25160f3e89a0da58368319c7e17d4e0",
+ "libzstd1_1.4.8+dfsg-2.1_amd64.deb": "5dcadfbb743bfa1c1c773bff91c018f835e8e8c821d423d3836f3ab84773507b",
+ "mailcap_3.69_all.deb": "63fa5520f05d2aea5ca23eee95981a5e029608e1186ded4143470c8f84184158",
+ "media-types_4.0.0_all.deb": "f9835dcf3cdbaf163104d4e511c9c4e0f41a56822e147e57f28f749fcbf7d44c",
+ "mime-support_3.66_all.deb": "b964e671e6c47674879a3e54130b6737e8760fbd3da6afcc015faa174af98ba0",
+ "perl-base_5.32.1-4+deb11u2_amd64.deb": "018a3e48e58cbc478d3a4365090fb1daa151769f90f9b45984ec9d056ef96adc",
+ "perl-modules-5.32_5.32.1-4+deb11u2_all.deb": "6fa15be322c3c89ec4a07d704ad58d4a2d1aabf866135a859f6d8d58c59e9df4",
+ "perl_5.32.1-4+deb11u2_amd64.deb": "1cebc4516ed7c240b812c7bdd7e6ea0810f513152717ca17ce139ee0dfbc7b0d",
+ "procps_3.3.17-5_amd64.deb": "ac8edf0517abe09637c36651cb6a59e10948b2879f3af9003b9145b2128a7a08",
+ "tar_1.34+dfsg-1_amd64.deb": "bd8e963c6edcf1c806df97cd73560794c347aa94b9aaaf3b88eea585bb2d2f3c",
+ "zlib1g_1.2.11.dfsg-2_amd64.deb": "af36bf9249db4372a014a21ff2b84cc5a6808e3c878feda652c78e871d5964ad",
+}
diff --git a/debian/download_packages.py b/debian/download_packages.py
index e8caebd..be43120 100755
--- a/debian/download_packages.py
+++ b/debian/download_packages.py
@@ -92,6 +92,9 @@
if package == b'libblas.so.3':
yield b'libblas3'
continue
+ if package == b'debconf-2.0':
+ yield b'debconf'
+ continue
yield package
def download_deps(apt_args, packages, excludes, force_includes):
diff --git a/frc971/imu/ADIS16505.cc b/frc971/imu/ADIS16505.cc
new file mode 100644
index 0000000..dfc09cb
--- /dev/null
+++ b/frc971/imu/ADIS16505.cc
@@ -0,0 +1,694 @@
+/**
+ * Copyright (c) 2020 Raspberry Pi (Trading) Ltd.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdio.h>
+
+#include <algorithm>
+#include <cstring>
+
+#include "hardware/dma.h"
+#include "hardware/gpio.h"
+#include "hardware/irq.h"
+#include "hardware/pio.h"
+#include "hardware/pwm.h"
+#include "hardware/spi.h"
+#include "hardware/timer.h"
+#include "pico/binary_info.h"
+#include "pico/bootrom.h"
+#include "pico/double.h"
+#include "pico/stdlib.h"
+#include "quadrature_encoder.pio.h"
+
+// Pinout definitions for the imu
+#define RST_IMU 22
+#define DR_IMU 20
+#define SYNC_IMU 21
+#define DIN_IMU 19
+#define DOUT_IMU 16
+#define SCLK_IMU 18
+#define CS_IMU 17
+
+// Pinout definitions for spi to the pi through the differential drivers
+#define DR_PI 14
+#define MOSI_PI 12
+#define MISO_PI 11
+#define SCK_PI 10
+#define CS_PI 13
+
+// The two drivetrain encoders
+#define ENC1_A 6
+#define ENC1_B 7
+#define ENC2_A 0
+#define ENC2_B 1
+
+// Backup outputs to the roborio
+#define RATE_PWM 2
+#define HEADING_PWM 4
+
+#define PWM_FREQ_HZ 200
+// PWM counts to this before wrapping
+#define PWM_TOP 62499
+
+#define SPI_IMU spi0
+#define SPI_PI spi1
+#define WRITE_BIT 0x8000
+
+// length in half-words of the buffer for communicating with the IMU
+// includes 2 non-data fields
+// the first element is used for recieving zeros while the initial request is
+// made the last element is the checksum
+#define IMU_NUM_ITEMS 17
+
+// length in bytes of the packet to the pi
+#define PI_NUM_ITEMS 42
+
+// number of samples for zeroing the z-gyro axis
+#define YAW_BUF_LEN 5000
+
+#define EXPECTED_PROD_ID 0x4079
+
+// Registers on the ADIS16505
+#define GLOB_CMD 0x68
+#define DIAG_STAT 0x02
+#define FIRM_REV 0x6C
+#define FIRM_DM 0x6E
+#define FIRM_Y 0x70
+#define PROD_ID 0x72
+#define SERIAL_NUM 0x74
+#define FILT_CTRL 0x5C
+#define MSC_CTRL 0x60
+#define DEC_RATE 0x64
+
+// TODO: replace with aos/events/logging/crc32.h
+const uint32_t kCrc32Table[256] = {
+ 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f,
+ 0xe963a535, 0x9e6495a3, 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988,
+ 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, 0x1db71064, 0x6ab020f2,
+ 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7,
+ 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9,
+ 0xfa0f3d63, 0x8d080df5, 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172,
+ 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, 0x35b5a8fa, 0x42b2986c,
+ 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59,
+ 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423,
+ 0xcfba9599, 0xb8bda50f, 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924,
+ 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, 0x76dc4190, 0x01db7106,
+ 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433,
+ 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d,
+ 0x91646c97, 0xe6635c01, 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e,
+ 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, 0x65b0d9c6, 0x12b7e950,
+ 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65,
+ 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7,
+ 0xa4d1c46d, 0xd3d6f4fb, 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0,
+ 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, 0x5005713c, 0x270241aa,
+ 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f,
+ 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81,
+ 0xb7bd5c3b, 0xc0ba6cad, 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a,
+ 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, 0xe3630b12, 0x94643b84,
+ 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1,
+ 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb,
+ 0x196c3671, 0x6e6b06e7, 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc,
+ 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, 0xd6d6a3e8, 0xa1d1937e,
+ 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b,
+ 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55,
+ 0x316e8eef, 0x4669be79, 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236,
+ 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, 0xc5ba3bbe, 0xb2bd0b28,
+ 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d,
+ 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f,
+ 0x72076785, 0x05005713, 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38,
+ 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, 0x86d3d2d4, 0xf1d4e242,
+ 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777,
+ 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69,
+ 0x616bffd3, 0x166ccf45, 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2,
+ 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, 0xaed16a4a, 0xd9d65adc,
+ 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9,
+ 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693,
+ 0x54de5729, 0x23d967bf, 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94,
+ 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d};
+
+// Buffer to start a burst read and recive 15 items + checksum
+static uint16_t imu_write_buf[IMU_NUM_ITEMS] = {0x6800, 0, 0, 0, 0, 0, 0, 0, 0,
+ 0, 0, 0, 0, 0, 0, 0, 0};
+// recieves a byte of zeros followed by 15 items + checksum
+static uint16_t imu_data_buffer[IMU_NUM_ITEMS];
+
+static dma_channel_config imu_tx_config;
+static dma_channel_config imu_rx_config;
+static uint imu_dma_tx;
+static uint imu_dma_rx;
+
+// The packet to the pi contains the whole burst read from the IMU (30 bytes)
+// followed by a timestamp, encoder values, and checksum
+// DIAG_STAT, X_GYRO_LOW, X_GYRO_OUT, Y_GYRO_LOW, Y_GYRO_OUT, Z_GYRO_LOW,
+// Z_GYRO_OUT, X_ACCL_LOW, X_ACCL_OUT, Y_ACCL_LOW, Y_ACCL_OUT, Z_ACCL_LOW,
+// Z_ACCL_OUT, TEMP_OUT, DATA_CNTR, TIMESTAMP (32 bit), ENC1_POS, ENC2_POS,
+// CHECKSUM (32-bit)
+
+// the staging buffer gets updated everytime new data is received
+static uint8_t pi_staging_buffer[PI_NUM_ITEMS];
+// the data from the staging buffer is latched into the sending buffer while the
+// pi is reading
+static uint8_t pi_sending_buffer[PI_NUM_ITEMS];
+
+// for now just recieves zeros
+// but is useful because directing the dma to a nullptr messes up the transfer
+// finished interrupt
+static uint8_t pi_recieve_buffer[PI_NUM_ITEMS];
+
+static dma_channel_config pi_tx_config;
+static dma_channel_config pi_rx_config;
+static uint pi_dma_tx;
+static uint pi_dma_rx;
+
+// zeroed yaw from the latest message from the imu
+static double yaw = 0;
+static double yaw_rate = 0;
+
+// TODO: fields for the janky, not encapsulated zeroing function
+static double yaw_rate_offset = 0;
+static bool yaw_zeroed = false;
+
+static int16_t encoder1_count = 0;
+static int16_t encoder2_count = 0;
+
+static uint32_t data_collect_timestamp = 0;
+
+// useful debug counters
+static uint checksum_mismatch_count;
+static uint message_recieved_count;
+static uint message_sent_count;
+// the number of times we had to defer sending new data because another
+// transfer was still in progress
+static uint timing_overrun_count;
+
+// the time it takes from servicing DR_IMU to finishing the transfer to the pi
+static int send_time = 0;
+
+void data_ready();
+void maybe_send_pi_packet();
+
+void gpio_irq_handler(uint gpio, uint32_t events) {
+ if (gpio == DR_IMU && (events & GPIO_IRQ_EDGE_RISE)) {
+ data_ready();
+ }
+}
+
+static inline void cs_select() {
+ gpio_put(CS_IMU, 0); // Active low
+ sleep_us(1);
+}
+
+static inline void cs_deselect() { gpio_put(CS_IMU, 1); }
+
+static uint16_t read_register(uint8_t reg_low) {
+ // For this particular device, we send the device the register we want to read
+ // first, then subsequently read from the device.
+ uint16_t output;
+
+ // The low byte of the register is first in the IMU's memory
+ uint16_t reg = ((uint16_t)reg_low) << 8;
+
+ cs_select();
+ spi_write16_blocking(SPI_IMU, ®, 1);
+ cs_deselect();
+ sleep_us(20); // wait the stall period
+ cs_select();
+ spi_read16_blocking(SPI_IMU, 0x0000, &output, 1);
+ cs_deselect();
+
+ return output;
+}
+
+static void write_register(uint8_t reg, uint16_t value) {
+ uint16_t low_byte = ((uint16_t)value) & 0xFF;
+ uint16_t high_byte = ((uint16_t)value) >> 8;
+
+ uint16_t command = (((uint16_t)reg) << 8) | low_byte | WRITE_BIT;
+
+ cs_select();
+ spi_write16_blocking(SPI_IMU, &command, 1);
+ cs_deselect();
+
+ sleep_us(20); // wait the stall period
+
+ command = ((((uint16_t)reg) + 1) << 8) | high_byte | WRITE_BIT;
+
+ cs_select();
+ spi_write16_blocking(SPI_IMU, &command, 1);
+ cs_deselect();
+}
+
+static void adis16505_reset() {
+ gpio_put(RST_IMU, 0); // Active low
+ sleep_ms(10);
+ gpio_put(RST_IMU, 1); // Active low
+ sleep_ms(310);
+}
+
+static uint32_t calculate_crc32(uint8_t *data, size_t len) {
+ uint32_t crc = 0xFFFFFFFF;
+ for (size_t i = 0; i < len; i++) {
+ crc = kCrc32Table[(crc ^ data[i]) & 0xFF] ^ (crc >> 8);
+ }
+ return crc;
+}
+
+static bool check_checksum(uint16_t *buf) {
+ uint16_t sum = 0;
+ for (int i = 1; i < IMU_NUM_ITEMS - 1; i++) {
+ uint16_t low = buf[i] & 0xff;
+ uint16_t high = (buf[i] >> 8) & 0xff;
+
+ sum += high + low;
+ }
+
+ uint16_t checksum = buf[IMU_NUM_ITEMS - 1];
+ return sum == checksum;
+}
+
+static void zero_yaw(double yaw) {
+ static double yaw_buffer[YAW_BUF_LEN];
+ static int num_items;
+
+ if (num_items < YAW_BUF_LEN) {
+ yaw_buffer[num_items] = yaw;
+ num_items++;
+ } else if (!yaw_zeroed) {
+ double sum = 0;
+ for (int i = 0; i < YAW_BUF_LEN; i++) {
+ sum += yaw_buffer[i];
+ }
+ yaw_rate_offset = -sum / YAW_BUF_LEN;
+ yaw = 0;
+ printf("offset: %f\n", yaw_rate_offset);
+ yaw_zeroed = true;
+ }
+}
+
+void pack_pi_packet() {
+ // zero the buffer
+ for (int i = 0; i < PI_NUM_ITEMS; i++) {
+ pi_staging_buffer[i] = 0;
+ }
+
+ // skip empty item
+ uint8_t *imu_packet = (uint8_t *)(imu_data_buffer + 1);
+ // skip empty item and checksum and convert to bytes
+ const size_t imu_packet_len = (IMU_NUM_ITEMS - 2) * sizeof(uint16_t);
+
+ memcpy(pi_staging_buffer, imu_packet, imu_packet_len);
+ memcpy(pi_staging_buffer + imu_packet_len, &data_collect_timestamp, 4);
+ memcpy(pi_staging_buffer + imu_packet_len + 4, &encoder1_count, 2);
+ memcpy(pi_staging_buffer + imu_packet_len + 6, &encoder2_count, 2);
+
+ // exclude the part of the buffer that will be the checksum
+ uint32_t crc = calculate_crc32(pi_staging_buffer, PI_NUM_ITEMS - 4);
+ memcpy(pi_staging_buffer + imu_packet_len + 8, &crc, 4);
+
+ static_assert(PI_NUM_ITEMS == imu_packet_len +
+ sizeof(data_collect_timestamp) +
+ sizeof(encoder1_count) +
+ sizeof(encoder2_count) + sizeof(crc),
+ "PI_NUM_ITEMS needs to be able to hold the imu message + the "
+ "timestamp + the encoders + the checksum");
+}
+
+void data_ready() {
+ // save timestamp
+ data_collect_timestamp = time_us_32();
+
+ // read encoders
+ quadrature_encoder_request_count(pio0, 0);
+ quadrature_encoder_request_count(pio0, 1);
+
+ cs_select();
+ dma_channel_configure(imu_dma_tx, &imu_tx_config,
+ &spi_get_hw(SPI_IMU)->dr, // write address
+ &imu_write_buf, // read address
+ IMU_NUM_ITEMS, // element count (each element is of
+ // size transfer_data_size)
+ false); // don't start yet
+ dma_channel_configure(imu_dma_rx, &imu_rx_config,
+ &imu_data_buffer, // write address
+ &spi_get_hw(SPI_IMU)->dr, // read address
+ IMU_NUM_ITEMS, // element count (each element is of
+ // size transfer_data_size)
+ false); // don't start yet
+ dma_start_channel_mask((1u << imu_dma_tx) | (1u << imu_dma_rx));
+
+ encoder1_count = quadrature_encoder_fetch_count(pio0, 0);
+ encoder2_count = quadrature_encoder_fetch_count(pio0, 1);
+}
+
+void imu_read_finished() {
+ cs_deselect();
+
+ // TODO: check status and if necessary set flag to reset in main loop
+
+ message_recieved_count++;
+ bool checksum_passed = check_checksum(imu_data_buffer);
+ if (!checksum_passed) {
+ checksum_mismatch_count++;
+ } else {
+ static const double dt = 0.0005; // seconds
+ int32_t z_gyro_out;
+ memcpy(&z_gyro_out, imu_data_buffer + 6, 4);
+ yaw_rate = (double)z_gyro_out / 655360.0 + yaw_rate_offset; // degrees
+ yaw += yaw_rate * dt;
+
+ // 50% is 0; -2000 deg/sec to 2000 deg/sec
+ uint16_t rate_level =
+ (std::clamp(yaw_rate, -2000.0, 2000.0) / 4000.0 + 0.5) * PWM_TOP;
+ pwm_set_gpio_level(RATE_PWM, rate_level);
+
+ // 0 to 360
+ uint16_t heading_level = (int16_t)(fmod(yaw, 360) / 360.0 * PWM_TOP);
+ pwm_set_gpio_level(HEADING_PWM, heading_level);
+
+ // fill out message and add it to the queue
+ pack_pi_packet();
+ }
+
+ // TODO: this has a sleep in it
+ // stay in sync with the pi by resetting the spi fifos each transfer
+ spi_init(SPI_PI, 2000 * 1000);
+ spi_set_slave(SPI_PI, true);
+ // these settings were the most stable even though the PI is using
+ // polarity 1 and phase 1
+ spi_set_format(SPI_PI, 8, SPI_CPOL_0, SPI_CPHA_1, SPI_MSB_FIRST);
+
+ maybe_send_pi_packet();
+
+ // clear the interrupt
+ dma_hw->ints0 = 1u << imu_dma_rx;
+}
+
+void maybe_send_pi_packet() {
+ // active low; if the pi isn't connected/booted, this will
+ // also look active
+ bool cs_asserted_or_unplugged = !gpio_get(CS_PI);
+
+ if (cs_asserted_or_unplugged) {
+ // the pi is still recieving something else from us
+ timing_overrun_count++;
+ return;
+ }
+
+ gpio_put(DR_PI, 1);
+ memcpy(pi_sending_buffer, pi_staging_buffer, PI_NUM_ITEMS);
+
+ dma_channel_configure(pi_dma_tx, &pi_tx_config,
+ &spi_get_hw(SPI_PI)->dr, // write address
+ &pi_sending_buffer, // read address
+ PI_NUM_ITEMS, // element count (each element is
+ // of size transfer_data_size)
+ false); // don't start yet
+ dma_channel_configure(pi_dma_rx, &pi_rx_config,
+ &pi_recieve_buffer, // write address
+ &spi_get_hw(SPI_PI)->dr, // read address
+ PI_NUM_ITEMS, // element count (each element is of
+ // size transfer_data_size)
+ false); // don't start yet
+
+ // start hardware calculation of the CRC-32; currently calculated in software
+ dma_sniffer_enable(pi_dma_tx, 0x0, true);
+ dma_hw->sniff_data = 0;
+
+ // start both at exactly the same time
+ dma_start_channel_mask((1u << pi_dma_tx) | (1u << pi_dma_rx));
+}
+
+void pi_transfer_finished() {
+ message_sent_count++;
+ gpio_put(DR_PI, 0);
+
+ send_time = time_us_32() - data_collect_timestamp;
+
+ dma_hw->ints1 = 1u << pi_dma_rx;
+}
+
+int main() {
+ stdio_init_all();
+
+ // Use 1MHz with the IMU as that is the limit for burst mode.
+ spi_init(SPI_IMU, 1000 * 1000);
+ spi_set_format(SPI_IMU, 16, SPI_CPOL_1, SPI_CPHA_1, SPI_MSB_FIRST);
+ gpio_set_function(DOUT_IMU, GPIO_FUNC_SPI);
+ gpio_set_function(SCLK_IMU, GPIO_FUNC_SPI);
+ gpio_set_function(DIN_IMU, GPIO_FUNC_SPI);
+ // Make the SPI pins available to picotool
+ bi_decl(bi_3pins_with_func(DOUT_IMU, DIN_IMU, SCLK_IMU, GPIO_FUNC_SPI));
+
+ // Chip select is active-low, so we'll initialise it to a driven-high state
+ gpio_init(CS_IMU);
+ gpio_set_dir(CS_IMU, GPIO_OUT);
+ gpio_put(CS_IMU, 1);
+ // Make the CS pin available to picotool
+ bi_decl(bi_1pin_with_name(CS_IMU, "IMU CS"));
+
+ // Reset is active-low, so we'll initialise it to a driven-high state
+ gpio_init(RST_IMU);
+ gpio_set_dir(RST_IMU, GPIO_OUT);
+ gpio_put(RST_IMU, 1);
+ // Make the RST pin available to picotool
+ bi_decl(bi_1pin_with_name(RST_IMU, "IMU RESET"));
+
+ imu_dma_tx = dma_claim_unused_channel(true);
+ imu_dma_rx = dma_claim_unused_channel(true);
+
+ // We set the outbound DMA to transfer from a memory buffer to the SPI
+ // transmit FIFO paced by the SPI TX FIFO DREQ The default is for the read
+ // address to increment every element (in this case 2 bytes - DMA_SIZE_16) and
+ // for the write address to remain unchanged.
+
+ imu_tx_config = dma_channel_get_default_config(imu_dma_tx);
+ channel_config_set_transfer_data_size(&imu_tx_config, DMA_SIZE_16);
+ channel_config_set_dreq(&imu_tx_config, spi_get_dreq(SPI_IMU, true));
+ channel_config_set_read_increment(&imu_tx_config, true);
+ channel_config_set_write_increment(&imu_tx_config, false);
+
+ // We set the inbound DMA to transfer from the SPI receive FIFO to a memory
+ // buffer paced by the SPI RX FIFO DREQ We configure the read address to
+ // remain unchanged for each element, but the write address to increment (so
+ // data is written throughout the buffer)
+ imu_rx_config = dma_channel_get_default_config(imu_dma_rx);
+ channel_config_set_transfer_data_size(&imu_rx_config, DMA_SIZE_16);
+ channel_config_set_dreq(&imu_rx_config, spi_get_dreq(SPI_IMU, false));
+ channel_config_set_read_increment(&imu_rx_config, false);
+ channel_config_set_write_increment(&imu_rx_config, true);
+
+ // Tell the DMA to raise IRQ line 0 when the channel finishes a block
+ dma_channel_set_irq0_enabled(imu_dma_rx, true);
+
+ // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
+ irq_set_exclusive_handler(DMA_IRQ_0, imu_read_finished);
+ irq_set_enabled(DMA_IRQ_0, true);
+
+ // Use 2MHz with the PI as that is the maximum speed for the pico
+ gpio_set_function(MISO_PI, GPIO_FUNC_SPI);
+ gpio_set_function(MOSI_PI, GPIO_FUNC_SPI);
+ gpio_set_function(SCK_PI, GPIO_FUNC_SPI);
+ gpio_set_function(CS_PI, GPIO_FUNC_SPI);
+ // Make the SPI pins available to picotool
+ bi_decl(bi_3pins_with_func(DOUT_IMU, DIN_IMU, SCLK_IMU, GPIO_FUNC_SPI));
+
+ gpio_init(DR_PI);
+ gpio_set_dir(DR_PI, GPIO_OUT);
+ gpio_put(DR_PI, 0);
+ // Make the CS pin available to picotool
+ bi_decl(bi_1pin_with_name(DR_PI, "DATA READY PI"));
+
+ pi_dma_tx = dma_claim_unused_channel(true);
+ pi_dma_rx = dma_claim_unused_channel(true);
+
+ // We set the outbound DMA to transfer from a memory buffer to the SPI
+ // transmit FIFO paced by the SPI TX FIFO DREQ The default is for the read
+ // address to increment every element (in this case 2 bytes - DMA_SIZE_16) and
+ // for the write address to remain unchanged.
+
+ pi_tx_config = dma_channel_get_default_config(pi_dma_tx);
+ channel_config_set_transfer_data_size(&pi_tx_config, DMA_SIZE_8);
+ channel_config_set_dreq(&pi_tx_config, spi_get_dreq(SPI_PI, true));
+ channel_config_set_read_increment(&pi_tx_config, true);
+ channel_config_set_write_increment(&pi_tx_config, false);
+
+ // We set the inbound DMA to transfer from the SPI receive FIFO to a memory
+ // buffer paced by the SPI RX FIFO DREQ We configure the read address to
+ // remain unchanged for each element, but the write address to increment (so
+ // data is written throughout the buffer)
+ pi_rx_config = dma_channel_get_default_config(pi_dma_rx);
+ channel_config_set_transfer_data_size(&pi_rx_config, DMA_SIZE_8);
+ channel_config_set_dreq(&pi_rx_config, spi_get_dreq(SPI_PI, false));
+ channel_config_set_read_increment(&pi_rx_config, false);
+ channel_config_set_write_increment(&pi_rx_config, true);
+
+ // Tell the DMA to raise IRQ line 1 when the channel finishes a block
+ dma_channel_set_irq1_enabled(pi_dma_rx, true);
+
+ // Configure the processor to run dma_handler() when DMA IRQ 0 is asserted
+ irq_set_exclusive_handler(DMA_IRQ_1, pi_transfer_finished);
+ irq_set_enabled(DMA_IRQ_1, true);
+
+ /* All IRQ priorities are initialized to PICO_DEFAULT_IRQ_PRIORITY by the pico
+ * runtime at startup.
+ *
+ * Handler | Interrupt | Cause of interrupt
+ * --------------------|--------------|---------------------------------------
+ * imu_read_finished | DMA_IRQ_0 | When the dma read from the imu is done
+ * pi_transfer_finished| DMA_IRQ_1 | When the dma read to the pi is
+ * done data_ready | IO_IRQ_BANK0 | On the rising edge of DR_IMU
+ */
+
+ // Tell the GPIOs they are allocated to PWM
+ gpio_set_function(HEADING_PWM, GPIO_FUNC_PWM);
+ gpio_set_function(RATE_PWM, GPIO_FUNC_PWM);
+
+ // Find out which PWM slice is connected to each gpio pin
+ uint heading_slice = pwm_gpio_to_slice_num(HEADING_PWM);
+ uint rate_slice = pwm_gpio_to_slice_num(RATE_PWM);
+
+ /* frequency_pwm = f_sys / ((TOP + 1) * (CSR_PHASE_CORRECT + 1) * (DIV_INT +
+ * DIV_FRAC / 16))
+ *
+ * f_sys = 125 mhz
+ * CSR_PHASE_CORRECT = 0; no phase correct
+ * TARGET_FREQ = 200 hz
+ *
+ * 125 mhz / x = 200 hz * 62500
+ *
+ * TOP = 62499
+ * DIV_INT = 10
+ */
+
+ float divisor = clock_get_hz(clk_sys) / (PWM_TOP + 1) / PWM_FREQ_HZ;
+
+ pwm_config cfg = pwm_get_default_config();
+ pwm_config_set_clkdiv_mode(&cfg, PWM_DIV_FREE_RUNNING);
+ pwm_config_set_clkdiv(&cfg, divisor);
+ pwm_config_set_wrap(&cfg, PWM_TOP);
+
+ pwm_init(heading_slice, &cfg, true);
+ pwm_init(rate_slice, &cfg, true);
+
+ // the two state machine instances use the same program memory from pio0
+ uint offset = pio_add_program(pio0, &quadrature_encoder_program);
+ quadrature_encoder_program_init(pio0, 0, offset, ENC1_A, 0);
+ quadrature_encoder_program_init(pio0, 1, offset, ENC2_A, 0);
+
+ sleep_ms(3 * 1000);
+
+ printf("Hello ADIS16505\n");
+
+ printf(
+ "System clock: %d hz\n"
+ "PWM target frequency: %d hz, PWM clock divisor: "
+ "%f, PWM TOP: %d\n",
+ clock_get_hz(clk_sys), PWM_FREQ_HZ, divisor, PWM_TOP);
+
+ while (true) {
+ adis16505_reset();
+ // See if SPI is working - interrogate the device for its product ID
+ // number, should be 0x4079
+ uint16_t id = read_register(PROD_ID);
+ if (id == EXPECTED_PROD_ID) {
+ printf("Product id: 0x%04x == expected 0x%04x\n", id, EXPECTED_PROD_ID);
+ break;
+ } else {
+ printf("Got 0x%04x for prod id, expected 0x%04x\ntrying again\n", id,
+ EXPECTED_PROD_ID);
+ }
+ }
+
+ uint16_t firmware_revision = read_register(FIRM_REV);
+ uint16_t firmware_day_month = read_register(FIRM_DM);
+ uint16_t firmware_year = read_register(FIRM_Y);
+ uint16_t serial_number = read_register(SERIAL_NUM);
+
+ printf(
+ "Firmware revision: 0x%04x, \nFirmware day month: 0x%04x, \nFirmware "
+ "year: "
+ "0x%04x, \nSerial number: 0x%04x, \n",
+ firmware_revision, firmware_day_month, firmware_year, serial_number);
+
+ // run self test
+ int num_failures = 0;
+ while (true) {
+ write_register(GLOB_CMD, 1u << 2);
+ sleep_ms(24);
+ uint16_t diag_stat = read_register(DIAG_STAT);
+
+ // check the sensor failure bit
+ bool sensor_failure = diag_stat & (1u << 5);
+ printf("Diag stat: 0b%016b, \n", diag_stat);
+
+ if (sensor_failure) {
+ num_failures++;
+ printf("%d failures, trying again\n", num_failures);
+ } else {
+ break;
+ }
+ }
+
+ write_register(FILT_CTRL, 0 /* no filtering */);
+ write_register(
+ MSC_CTRL,
+ (1u << 9) /* enable 32-bit mode for burst reads */ |
+ (0u << 8) /* send gyro and accelerometer data in burst mode */ |
+ (1u << 7) /* enable gyro linear g compensation */ |
+ (1u << 6) /* enable point of percussion alignment */ |
+ (0u << 2) /* internal clock mode */ |
+ (0u << 1) /* sync polarity, doesn't matter */ |
+ (1u << 0) /* data ready is active high */);
+ // Rate of the output will be 2000 / (DEC_RATE + 1) Hz.
+ write_register(DEC_RATE, 0 /* no decimation */);
+
+ sleep_us(200);
+
+ gpio_set_irq_enabled_with_callback(DR_IMU, GPIO_IRQ_EDGE_RISE, true,
+ &gpio_irq_handler);
+
+ while (!yaw_zeroed) {
+ dma_channel_wait_for_finish_blocking(imu_dma_rx);
+ sleep_us(100);
+ zero_yaw(yaw_rate);
+ }
+
+ printf("Press q to enter bootloader\n");
+
+ while (1) {
+ dma_channel_wait_for_finish_blocking(imu_dma_rx);
+ // we want the interrupts to happen before or during the sleep so that we
+ // can get a good picture of what's going on without things moving around
+ sleep_us(100);
+
+ // debug
+ // one printf is faster than many printfs
+ printf(
+ "Z pos is %f encoder: %d %d\n"
+ "Num failed checksums: %d, Total messages recieved: %d,\n"
+ "Num messages to pi: %d, Timing overrun count: %d,\n"
+ "Send time: %d us\n",
+ yaw, encoder1_count, encoder2_count, checksum_mismatch_count,
+ message_recieved_count, message_sent_count, timing_overrun_count,
+ send_time);
+
+ // allow the user to enter the bootloader without removing power or having
+ // to install a reset button
+ char user_input = getchar_timeout_us(0);
+ if (user_input == 'q') {
+ printf("Going down! entering bootloader\n");
+ reset_usb_boot(0, 0);
+ }
+
+ sleep_ms(50);
+ }
+
+ printf("All good\n");
+ dma_channel_unclaim(imu_dma_rx);
+ dma_channel_unclaim(imu_dma_tx);
+ dma_channel_unclaim(pi_dma_rx);
+ dma_channel_unclaim(pi_dma_tx);
+ return 0;
+}
diff --git a/frc971/imu/CMakeLists.txt b/frc971/imu/CMakeLists.txt
new file mode 100644
index 0000000..e97ba8d
--- /dev/null
+++ b/frc971/imu/CMakeLists.txt
@@ -0,0 +1,47 @@
+cmake_minimum_required(VERSION 3.12)
+
+# Pull in SDK (must be before project)
+include(pico_sdk_import.cmake)
+
+project(ADIS16505_communication C CXX ASM)
+set(CMAKE_C_STANDARD 11)
+set(CMAKE_CXX_STANDARD 17)
+
+if (PICO_SDK_VERSION_STRING VERSION_LESS "1.3.0")
+ message(FATAL_ERROR "Raspberry Pi Pico SDK version 1.3.0 (or later) required. Your version is ${PICO_SDK_VERSION_STRING}")
+endif()
+
+# Initialize the SDK
+pico_sdk_init()
+
+add_compile_options(-Wall
+ -Wno-format # int != int32_t as far as the compiler is concerned because gcc has int32_t as long int
+ -Wno-unused-function # we have some for the docs that aren't called
+ -Wno-maybe-uninitialized
+ )
+
+add_executable(pico_imu_board)
+
+pico_generate_pio_header(pico_imu_board ${CMAKE_CURRENT_LIST_DIR}/quadrature_encoder.pio)
+
+target_sources(pico_imu_board PRIVATE ADIS16505.cc)
+
+# pull in common dependencies
+target_link_libraries(pico_imu_board
+ pico_stdlib
+ hardware_spi
+ hardware_dma
+ hardware_irq
+ hardware_pwm
+ hardware_pio
+ hardware_timer
+ pico_bootrom
+ pico_double
+ )
+
+# enable usb output, disable uart output
+pico_enable_stdio_usb(pico_imu_board 1)
+pico_enable_stdio_uart(pico_imu_board 1)
+
+# create map/bin/hex file etc.
+pico_add_extra_outputs(pico_imu_board)
diff --git a/frc971/imu/pico_sdk_import.cmake b/frc971/imu/pico_sdk_import.cmake
new file mode 100644
index 0000000..28efe9e
--- /dev/null
+++ b/frc971/imu/pico_sdk_import.cmake
@@ -0,0 +1,62 @@
+# This is a copy of <PICO_SDK_PATH>/external/pico_sdk_import.cmake
+
+# This can be dropped into an external project to help locate this SDK
+# It should be include()ed prior to project()
+
+if (DEFINED ENV{PICO_SDK_PATH} AND (NOT PICO_SDK_PATH))
+ set(PICO_SDK_PATH $ENV{PICO_SDK_PATH})
+ message("Using PICO_SDK_PATH from environment ('${PICO_SDK_PATH}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT} AND (NOT PICO_SDK_FETCH_FROM_GIT))
+ set(PICO_SDK_FETCH_FROM_GIT $ENV{PICO_SDK_FETCH_FROM_GIT})
+ message("Using PICO_SDK_FETCH_FROM_GIT from environment ('${PICO_SDK_FETCH_FROM_GIT}')")
+endif ()
+
+if (DEFINED ENV{PICO_SDK_FETCH_FROM_GIT_PATH} AND (NOT PICO_SDK_FETCH_FROM_GIT_PATH))
+ set(PICO_SDK_FETCH_FROM_GIT_PATH $ENV{PICO_SDK_FETCH_FROM_GIT_PATH})
+ message("Using PICO_SDK_FETCH_FROM_GIT_PATH from environment ('${PICO_SDK_FETCH_FROM_GIT_PATH}')")
+endif ()
+
+set(PICO_SDK_PATH "${PICO_SDK_PATH}" CACHE PATH "Path to the Raspberry Pi Pico SDK")
+set(PICO_SDK_FETCH_FROM_GIT "${PICO_SDK_FETCH_FROM_GIT}" CACHE BOOL "Set to ON to fetch copy of SDK from git if not otherwise locatable")
+set(PICO_SDK_FETCH_FROM_GIT_PATH "${PICO_SDK_FETCH_FROM_GIT_PATH}" CACHE FILEPATH "location to download SDK")
+
+if (NOT PICO_SDK_PATH)
+ if (PICO_SDK_FETCH_FROM_GIT)
+ include(FetchContent)
+ set(FETCHCONTENT_BASE_DIR_SAVE ${FETCHCONTENT_BASE_DIR})
+ if (PICO_SDK_FETCH_FROM_GIT_PATH)
+ get_filename_component(FETCHCONTENT_BASE_DIR "${PICO_SDK_FETCH_FROM_GIT_PATH}" REALPATH BASE_DIR "${CMAKE_SOURCE_DIR}")
+ endif ()
+ FetchContent_Declare(
+ pico_sdk
+ GIT_REPOSITORY https://github.com/raspberrypi/pico-sdk
+ GIT_TAG master
+ )
+ if (NOT pico_sdk)
+ message("Downloading Raspberry Pi Pico SDK")
+ FetchContent_Populate(pico_sdk)
+ set(PICO_SDK_PATH ${pico_sdk_SOURCE_DIR})
+ endif ()
+ set(FETCHCONTENT_BASE_DIR ${FETCHCONTENT_BASE_DIR_SAVE})
+ else ()
+ message(FATAL_ERROR
+ "SDK location was not specified. Please set PICO_SDK_PATH or set PICO_SDK_FETCH_FROM_GIT to on to fetch from git."
+ )
+ endif ()
+endif ()
+
+get_filename_component(PICO_SDK_PATH "${PICO_SDK_PATH}" REALPATH BASE_DIR "${CMAKE_BINARY_DIR}")
+if (NOT EXISTS ${PICO_SDK_PATH})
+ message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' not found")
+endif ()
+
+set(PICO_SDK_INIT_CMAKE_FILE ${PICO_SDK_PATH}/pico_sdk_init.cmake)
+if (NOT EXISTS ${PICO_SDK_INIT_CMAKE_FILE})
+ message(FATAL_ERROR "Directory '${PICO_SDK_PATH}' does not appear to contain the Raspberry Pi Pico SDK")
+endif ()
+
+set(PICO_SDK_PATH ${PICO_SDK_PATH} CACHE PATH "Path to the Raspberry Pi Pico SDK" FORCE)
+
+include(${PICO_SDK_INIT_CMAKE_FILE})
diff --git a/frc971/imu/quadrature_encoder.pio b/frc971/imu/quadrature_encoder.pio
new file mode 100644
index 0000000..b2a0b82
--- /dev/null
+++ b/frc971/imu/quadrature_encoder.pio
@@ -0,0 +1,165 @@
+;
+; Copyright (c) 2021 pmarques-dev @ github
+;
+; SPDX-License-Identifier: BSD-3-Clause
+;
+
+.program quadrature_encoder
+
+; this code must be loaded into address 0, but at 29 instructions, it probably
+; wouldn't be able to share space with other programs anyway
+.origin 0
+
+
+; the code works by running a loop that continuously shifts the 2 phase pins into
+; ISR and looks at the lower 4 bits to do a computed jump to an instruction that
+; does the proper "do nothing" | "increment" | "decrement" action for that pin
+; state change (or no change)
+
+; ISR holds the last state of the 2 pins during most of the code. The Y register
+; keeps the current encoder count and is incremented / decremented according to
+; the steps sampled
+
+; writing any non zero value to the TX FIFO makes the state machine push the
+; current count to RX FIFO between 6 to 18 clocks afterwards. The worst case
+; sampling loop takes 14 cycles, so this program is able to read step rates up
+; to sysclk / 14 (e.g., sysclk 125MHz, max step rate = 8.9 Msteps/sec)
+
+
+; 00 state
+ JMP update ; read 00
+ JMP decrement ; read 01
+ JMP increment ; read 10
+ JMP update ; read 11
+
+; 01 state
+ JMP increment ; read 00
+ JMP update ; read 01
+ JMP update ; read 10
+ JMP decrement ; read 11
+
+; 10 state
+ JMP decrement ; read 00
+ JMP update ; read 01
+ JMP update ; read 10
+ JMP increment ; read 11
+
+; to reduce code size, the last 2 states are implemented in place and become the
+; target for the other jumps
+
+; 11 state
+ JMP update ; read 00
+ JMP increment ; read 01
+decrement:
+ ; note: the target of this instruction must be the next address, so that
+ ; the effect of the instruction does not depend on the value of Y. The
+ ; same is true for the "JMP X--" below. Basically "JMP Y--, <next addr>"
+ ; is just a pure "decrement Y" instruction, with no other side effects
+ JMP Y--, update ; read 10
+
+ ; this is where the main loop starts
+.wrap_target
+update:
+ ; we start by checking the TX FIFO to see if the main code is asking for
+ ; the current count after the PULL noblock, OSR will have either 0 if
+ ; there was nothing or the value that was there
+ SET X, 0
+ PULL noblock
+
+ ; since there are not many free registers, and PULL is done into OSR, we
+ ; have to do some juggling to avoid losing the state information and
+ ; still place the values where we need them
+ MOV X, OSR
+ MOV OSR, ISR
+
+ ; the main code did not ask for the count, so just go to "sample_pins"
+ JMP !X, sample_pins
+
+ ; if it did ask for the count, then we push it
+ MOV ISR, Y ; we trash ISR, but we already have a copy in OSR
+ PUSH
+
+sample_pins:
+ ; we shift into ISR the last state of the 2 input pins (now in OSR) and
+ ; the new state of the 2 pins, thus producing the 4 bit target for the
+ ; computed jump into the correct action for this state
+ MOV ISR, NULL
+ IN OSR, 2
+ IN PINS, 2
+ MOV PC, ISR
+
+ ; the PIO does not have a increment instruction, so to do that we do a
+ ; negate, decrement, negate sequence
+increment:
+ MOV X, !Y
+ JMP X--, increment_cont
+increment_cont:
+ MOV Y, !X
+.wrap ; the .wrap here avoids one jump instruction and saves a cycle too
+
+
+
+% c-sdk {
+
+#include "hardware/clocks.h"
+#include "hardware/gpio.h"
+
+// max_step_rate is used to lower the clock of the state machine to save power
+// if the application doesn't require a very high sampling rate. Passing zero
+// will set the clock to the maximum, which gives a max step rate of around
+// 8.9 Msteps/sec at 125MHz
+
+static inline void quadrature_encoder_program_init(PIO pio, uint sm, uint offset, uint pin, int max_step_rate)
+{
+ pio_sm_set_consecutive_pindirs(pio, sm, pin, 2, false);
+ pio_gpio_init(pio, pin);
+ gpio_pull_up(pin);
+
+ pio_sm_config c = quadrature_encoder_program_get_default_config(offset);
+ sm_config_set_in_pins(&c, pin); // for WAIT, IN
+ sm_config_set_jmp_pin(&c, pin); // for JMP
+ // shift to left, autopull disabled
+ sm_config_set_in_shift(&c, false, false, 32);
+ // don't join FIFO's
+ sm_config_set_fifo_join(&c, PIO_FIFO_JOIN_NONE);
+
+ // passing "0" as the sample frequency,
+ if (max_step_rate == 0) {
+ sm_config_set_clkdiv(&c, 1.0);
+ } else {
+ // one state machine loop takes at most 14 cycles
+ float div = (float)clock_get_hz(clk_sys) / (14 * max_step_rate);
+ sm_config_set_clkdiv(&c, div);
+ }
+
+ pio_sm_init(pio, sm, offset, &c);
+ pio_sm_set_enabled(pio, sm, true);
+}
+
+
+// When requesting the current count we may have to wait a few cycles (average
+// ~11 sysclk cycles) for the state machine to reply. If we are reading multiple
+// encoders, we may request them all in one go and then fetch them all, thus
+// avoiding doing the wait multiple times. If we are reading just one encoder,
+// we can use the "get_count" function to request and wait
+
+static inline void quadrature_encoder_request_count(PIO pio, uint sm)
+{
+ pio->txf[sm] = 1;
+}
+
+static inline int32_t quadrature_encoder_fetch_count(PIO pio, uint sm)
+{
+ while (pio_sm_is_rx_fifo_empty(pio, sm))
+ tight_loop_contents();
+ return pio->rxf[sm];
+}
+
+static inline int32_t quadrature_encoder_get_count(PIO pio, uint sm)
+{
+ quadrature_encoder_request_count(pio, sm);
+ return quadrature_encoder_fetch_count(pio, sm);
+}
+
+%}
+
diff --git a/y2020/vision/rootfs/99-usb-mount.rules b/frc971/raspi/rootfs/99-usb-mount.rules
similarity index 100%
rename from y2020/vision/rootfs/99-usb-mount.rules
rename to frc971/raspi/rootfs/99-usb-mount.rules
diff --git a/frc971/raspi/rootfs/README.md b/frc971/raspi/rootfs/README.md
new file mode 100644
index 0000000..a9c7251
--- /dev/null
+++ b/frc971/raspi/rootfs/README.md
@@ -0,0 +1,50 @@
+This modifies a stock debian root filesystem to be able to operate as a vision
+pi. It is not trying to be reproducible, but should be good enough for FRC
+purposes.
+
+The default hostname and IP is pi-971-1, 10.9.71.101.
+ Username pi, password raspberry.
+
+Download 2021-10-30-raspios-bullseye-armhf-lite.img (or any newer
+bullseye version, as a .zip file) from
+`https://www.raspberrypi.org/downloads/raspberry-pi-os/`, extract
+(unzip) the .img file, and edit `modify_rootfs.sh` to point to it.
+
+Run modify_rootfs.sh to build the filesystem (you might need to hit
+return in a spot or two and will need sudo privileges to mount the
+partition):
+ * `modify_root.sh`
+
+VERY IMPORTANT NOTE: Before doing the next step, use `lsblk` to find
+the device and make absolutely sure this isn't your hard drive or
+something else. It will target /dev/sda by default, which in some
+computers is your default hard drive.
+
+After confirming the target device, edit the `make_sd.sh` script to point to the correct IMAGE filename, and run the `make_sd.sh` command,
+which takes the name of the pi as an argument:
+ * `make_sd.sh pi-971-1`
+
+OR, if you want to manually run this, you can deploy the image by
+copying the contents of the image to the SD card. You can do this
+manually, via
+ `dd if=2020-02-13-raspbian-bullseye-lite-frc-mods.img of=/dev/sdX bs=1M`
+
+From there, transfer the SD card to the pi, log in, `sudo` to `root`,
+and run `/root/bin/change_hostname.sh` to change the hostname to the
+actual target.
+
+
+A couple additional notes on setting this up:
+ * You'll likely need to install (`sudo apt install`) the emulation packages `proot` and `qemu-user-static` (or possibly `qemu-arm-static`)
+ * If the modify_root.sh script fails, you may need to manually unmount the image (`sudo umount ${PARTITION}` and `rmdir ${PARTITION}`) before running it again
+ * Don't be clever and try to link to the image file in a different folder. These scripts need access directly to the file and will fail otherwise
+
+
+Things to do once the SD card is complete, and you've booted a PI with it:
+
+ * Download the code:
+ Once this is completed, you can boot the pi with the newly flashed SD
+ card, and download the code to the pi using:
+ `bazel run -c opt --cpu=armv7 //y2022:pi_download_stripped -- PI_IP_ADDR
+
+ where PI_IP_ADDR is the IP address of the target pi, e.g., 10.9.71.101
diff --git a/y2020/vision/rootfs/change_hostname.sh b/frc971/raspi/rootfs/change_hostname.sh
similarity index 100%
rename from y2020/vision/rootfs/change_hostname.sh
rename to frc971/raspi/rootfs/change_hostname.sh
diff --git a/y2020/vision/rootfs/dhcpcd.conf b/frc971/raspi/rootfs/dhcpcd.conf
similarity index 100%
rename from y2020/vision/rootfs/dhcpcd.conf
rename to frc971/raspi/rootfs/dhcpcd.conf
diff --git a/y2020/vision/rootfs/frc971.service b/frc971/raspi/rootfs/frc971.service
similarity index 100%
rename from y2020/vision/rootfs/frc971.service
rename to frc971/raspi/rootfs/frc971.service
diff --git a/y2020/vision/rootfs/logind.conf b/frc971/raspi/rootfs/logind.conf
similarity index 100%
rename from y2020/vision/rootfs/logind.conf
rename to frc971/raspi/rootfs/logind.conf
diff --git a/frc971/raspi/rootfs/make_sd.sh b/frc971/raspi/rootfs/make_sd.sh
new file mode 100755
index 0000000..81f7727
--- /dev/null
+++ b/frc971/raspi/rootfs/make_sd.sh
@@ -0,0 +1,46 @@
+#!/bin/bash
+
+set -e
+
+# Disk image to use for creating SD card
+# NOTE: You MUST run modify_rootfs.sh on this image BEFORE running make_sd.sh
+ORIG_IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
+IMAGE=`echo ${ORIG_IMAGE} | sed s/.img/-frc-mods.img/`
+DEVICE="/dev/sda"
+
+if mount | grep "${DEVICE}" >/dev/null ;
+then
+ echo "Overwriting a mounted partition, is ${DEVICE} the sd card?"
+ exit 1
+fi
+
+sudo dd if=${IMAGE} of=${DEVICE} bs=1M status=progress
+
+PARTITION="${IMAGE}.partition"
+
+mkdir -p "${PARTITION}"
+sudo mount "${DEVICE}2" "${PARTITION}"
+
+function target() {
+ HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
+}
+
+if [ "${1}" == "" ]; then
+ echo "No hostname specified, so skipping setting it."
+ echo "You do this manually on the pi by running /root/bin/change_hostname.sh PI_NAME"
+else
+ target /root/bin/change_hostname.sh "${1}"
+fi
+
+echo "Starting a shell for any manual configuration"
+target /bin/bash --rcfile /root/.bashrc
+
+# Put a timestamp on when this card got created and by whom
+TIMESTAMP_FILE="${PARTITION}/home/pi/.DiskFlashedDate.txt"
+date > "${TIMESTAMP_FILE}"
+git rev-parse HEAD >> "${TIMESTAMP_FILE}"
+whoami >> "${TIMESTAMP_FILE}"
+
+# Found I had to do a lazy force unmount ("-l" flag) to make it work reliably
+sudo umount -l "${PARTITION}"
+rmdir "${PARTITION}"
diff --git a/y2020/vision/rootfs/modify_rootfs.sh b/frc971/raspi/rootfs/modify_rootfs.sh
similarity index 77%
rename from y2020/vision/rootfs/modify_rootfs.sh
rename to frc971/raspi/rootfs/modify_rootfs.sh
index 1367314..a340c64 100755
--- a/y2020/vision/rootfs/modify_rootfs.sh
+++ b/frc971/raspi/rootfs/modify_rootfs.sh
@@ -2,8 +2,8 @@
set -xe
-# Full path to Raspberry Pi Buster disk image
-IMAGE="2020-08-20-raspios-buster-armhf-lite.img"
+# Full path to Raspberry Pi Bullseye disk image
+IMAGE="2021-10-30-raspios-bullseye-armhf-lite.img"
BOOT_PARTITION="${IMAGE}.boot_partition"
PARTITION="${IMAGE}.partition"
@@ -34,8 +34,11 @@
if ! grep "gpu_mem=128" "${BOOT_PARTITION}/config.txt"; then
echo "gpu_mem=128" | sudo tee -a "${BOOT_PARTITION}/config.txt"
fi
+# For now, disable the new libcamera driver in favor of legacy ones
+sudo sed -i s/^camera_auto_detect=1/#camera_auto_detect=1/ "${BOOT_PARTITION}/config.txt"
-sudo umount "${BOOT_PARTITION}"
+# Seeing a race condition with umount, so doing lazy umount
+sudo umount -l "${BOOT_PARTITION}"
rmdir "${BOOT_PARTITION}"
if mount | grep "${PARTITION}" >/dev/null ;
@@ -86,10 +89,21 @@
target /bin/mkdir -p /home/pi/.ssh/
cat ~/.ssh/id_rsa.pub | target tee /home/pi/.ssh/authorized_keys
+# Downloads and installs our target libraries
target /bin/bash /tmp/target_configure.sh
+# Add a file to show when this image was last modified and by whom
+TIMESTAMP_FILE="${PARTITION}/home/pi/.ImageModifiedDate.txt"
+date > "${TIMESTAMP_FILE}"
+git rev-parse HEAD >> "${TIMESTAMP_FILE}"
+whoami >> "${TIMESTAMP_FILE}"
+
# Run a prompt as root inside the target to poke around and check things.
target /bin/bash --rcfile /root/.bashrc
-sudo umount "${PARTITION}"
+sudo umount -l "${PARTITION}"
rmdir "${PARTITION}"
+
+# Move the image to a different name, to indicated we've modified it
+MOD_IMAGE_NAME=`echo ${IMAGE} | sed s/.img/-frc-mods.img/`
+mv ${IMAGE} ${MOD_IMAGE_NAME}
diff --git a/y2020/vision/rootfs/rt.conf b/frc971/raspi/rootfs/rt.conf
similarity index 100%
rename from y2020/vision/rootfs/rt.conf
rename to frc971/raspi/rootfs/rt.conf
diff --git a/y2020/vision/rootfs/sctp.conf b/frc971/raspi/rootfs/sctp.conf
similarity index 100%
rename from y2020/vision/rootfs/sctp.conf
rename to frc971/raspi/rootfs/sctp.conf
diff --git a/y2020/vision/rootfs/target_configure.sh b/frc971/raspi/rootfs/target_configure.sh
similarity index 78%
rename from y2020/vision/rootfs/target_configure.sh
rename to frc971/raspi/rootfs/target_configure.sh
index 9f2e289..3fd4d40 100755
--- a/y2020/vision/rootfs/target_configure.sh
+++ b/frc971/raspi/rootfs/target_configure.sh
@@ -19,28 +19,29 @@
git \
python3-pip \
cpufrequtils \
- libopencv-calib3d3.2 \
- libopencv-contrib3.2 \
- libopencv-core3.2 \
- libopencv-features2d3.2 \
- libopencv-flann3.2 \
- libopencv-highgui3.2 \
- libopencv-imgcodecs3.2 \
- libopencv-imgproc3.2 \
- libopencv-ml3.2 \
- libopencv-objdetect3.2 \
- libopencv-photo3.2 \
- libopencv-shape3.2 \
- libopencv-stitching3.2 \
- libopencv-superres3.2 \
- libopencv-video3.2 \
- libopencv-videoio3.2 \
- libopencv-videostab3.2 \
- libopencv-viz3.2 \
+ libopencv-calib3d4.5 \
+ libopencv-contrib4.5 \
+ libopencv-core4.5 \
+ libopencv-features2d4.5 \
+ libopencv-flann4.5 \
+ libopencv-highgui4.5 \
+ libopencv-imgcodecs4.5 \
+ libopencv-imgproc4.5 \
+ libopencv-ml4.5 \
+ libopencv-objdetect4.5 \
+ libopencv-photo4.5 \
+ libopencv-shape4.5 \
+ libopencv-stitching4.5 \
+ libopencv-superres4.5 \
+ libopencv-video4.5 \
+ libopencv-videoio4.5 \
+ libopencv-videostab4.5 \
+ libopencv-viz4.5 \
python3-opencv \
libnice10 \
pmount \
- libnice-dev
+ libnice-dev \
+ feh
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
diff --git a/y2020/vision/rootfs/usb-mount@.service b/frc971/raspi/rootfs/usb-mount@.service
similarity index 100%
rename from y2020/vision/rootfs/usb-mount@.service
rename to frc971/raspi/rootfs/usb-mount@.service
diff --git a/frc971/vision/BUILD b/frc971/vision/BUILD
new file mode 100644
index 0000000..1e3ed58
--- /dev/null
+++ b/frc971/vision/BUILD
@@ -0,0 +1,35 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
+
+flatbuffer_cc_library(
+ name = "vision_fbs",
+ srcs = ["vision.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_ts_library(
+ name = "vision_ts_fbs",
+ srcs = ["vision.fbs"],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_library(
+ name = "v4l2_reader",
+ srcs = [
+ "v4l2_reader.cc",
+ ],
+ hdrs = [
+ "v4l2_reader.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":vision_fbs",
+ "//aos/events:event_loop",
+ "//aos/scoped:scoped_fd",
+ "@com_github_google_glog//:glog",
+ "@com_google_absl//absl/base",
+ ],
+)
diff --git a/y2020/vision/v4l2_reader.cc b/frc971/vision/v4l2_reader.cc
similarity index 97%
rename from y2020/vision/v4l2_reader.cc
rename to frc971/vision/v4l2_reader.cc
index 3f24f1e..793d8cd 100644
--- a/y2020/vision/v4l2_reader.cc
+++ b/frc971/vision/v4l2_reader.cc
@@ -1,4 +1,4 @@
-#include "y2020/vision/v4l2_reader.h"
+#include "frc971/vision/v4l2_reader.h"
#include <fcntl.h>
#include <linux/videodev2.h>
@@ -60,8 +60,8 @@
}
bool V4L2Reader::ReadLatestImage() {
- // First, enqueue any old buffer we already have. This is the one which may
- // have been sent.
+ // First, enqueue any old buffer we already have. This is the one which
+ // may have been sent.
if (saved_buffer_) {
EnqueueBuffer(saved_buffer_.index);
saved_buffer_.Clear();
@@ -189,8 +189,8 @@
if (result == 0) {
return;
}
- // Some devices (like Alex's webcam) return this if streaming isn't currently
- // on, unlike what the documentations says should happen.
+ // Some devices (like Alex's webcam) return this if streaming isn't
+ // currently on, unlike what the documentations says should happen.
if (errno == EBUSY) {
return;
}
diff --git a/y2020/vision/v4l2_reader.h b/frc971/vision/v4l2_reader.h
similarity index 95%
rename from y2020/vision/v4l2_reader.h
rename to frc971/vision/v4l2_reader.h
index b9b3ce4..6f90cec 100644
--- a/y2020/vision/v4l2_reader.h
+++ b/frc971/vision/v4l2_reader.h
@@ -1,5 +1,5 @@
-#ifndef Y2020_VISION_V4L2_READER_H_
-#define Y2020_VISION_V4L2_READER_H_
+#ifndef FRC971_VISION_V4L2_READER_H_
+#define FRC971_VISION_V4L2_READER_H_
#include <array>
#include <string>
@@ -9,7 +9,7 @@
#include "aos/events/event_loop.h"
#include "aos/scoped/scoped_fd.h"
-#include "y2020/vision/vision_generated.h"
+#include "frc971/vision/vision_generated.h"
namespace frc971 {
namespace vision {
@@ -119,4 +119,4 @@
} // namespace vision
} // namespace frc971
-#endif // Y2020_VISION_V4L2_READER_H_
+#endif // FRC971_VISION_V4L2_READER_H_
diff --git a/y2020/vision/vision.fbs b/frc971/vision/vision.fbs
similarity index 100%
rename from y2020/vision/vision.fbs
rename to frc971/vision/vision.fbs
diff --git a/tools/build_rules/BUILD b/tools/build_rules/BUILD
index cf27ca0..aeb8517 100644
--- a/tools/build_rules/BUILD
+++ b/tools/build_rules/BUILD
@@ -12,3 +12,15 @@
visibility = ["//visibility:public"],
deps = ["@python_jinja2"],
)
+
+py_binary(
+ name = "apache_runner",
+ srcs = ["apache_runner.py"],
+ data = [
+ "apache_template.conf",
+ "@apache2//:all_files",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = ["@python_jinja2"],
+)
diff --git a/tools/build_rules/apache.bzl b/tools/build_rules/apache.bzl
new file mode 100644
index 0000000..9fb2ef5
--- /dev/null
+++ b/tools/build_rules/apache.bzl
@@ -0,0 +1,82 @@
+def _apache_binary_impl(ctx):
+ binary_path = ctx.attr.binary.files_to_run.executable.short_path
+
+ out = ctx.actions.declare_file(ctx.label.name)
+ ctx.actions.write(out, """\
+#!/bin/bash
+
+exec ./tools/build_rules/apache_runner --binary "{}" "$@"
+""".format(binary_path), is_executable = True)
+
+ # Collect files and runfiles for the tools that we're wrapping.
+ files = depset(transitive = [
+ ctx.attr._apache_runner.files,
+ ctx.attr.binary.files,
+ ])
+
+ runfiles = ctx.attr._apache_runner.default_runfiles
+ runfiles = runfiles.merge(ctx.attr.binary.default_runfiles)
+
+ return [
+ DefaultInfo(
+ executable = out,
+ files = files,
+ runfiles = runfiles,
+ ),
+ ]
+
+apache_wrapper = rule(
+ implementation = _apache_binary_impl,
+ attrs = {
+ "binary": attr.label(
+ mandatory = True,
+ executable = True,
+ cfg = "target",
+ doc = "The binary that we're wrapping with LDAP+HTTPS.",
+ ),
+ "_apache_runner": attr.label(
+ default = "@//tools/build_rules:apache_runner",
+ executable = True,
+ cfg = "target",
+ ),
+ },
+ doc = """\
+This rule wraps another web server and provides LDAP and HTTPS support.
+
+It's not intended to be used in production. It's intended to provide team
+members with a way to test their code in a production-like environment. E.g. to
+test whether your server makes use of LDAP credentials correctly.
+
+Write this as a wrapper around another binary like so:
+
+ apache_wrapper(
+ name = "wrapped_server",
+ binary = "//path/to:server_binary",
+ )
+
+Then you can run Apache and the wrapped binary like so:
+
+ $ bazel run :wrapped_server
+
+The wrapped binary can find the port that Apache is wrapping via the
+APACHE_WRAPPED_PORT environment variable.
+
+This rule assumes that you have a file at the root of the workspace called
+"ldap.json". You can customize this path with the `--ldap_info` argument. The
+JSON file has to have these three entries in it:
+
+ {
+ "ldap_bind_dn": "...",
+ "ldap_url": "...",
+ "ldap_password": "..."
+ }
+
+where the "..." values are replaced with the information to connect to an LDAP
+server. If you want to connect to our FRC971 LDAP server, please contact a
+Software mentor. Or ask on the `#coding` Slack channel.
+
+If the default ports of 7000 and 7500 are already taken, you can change them via
+the `--https_port` and `--wrapped_port` arguments.
+""",
+ executable = True,
+)
diff --git a/tools/build_rules/apache_runner.py b/tools/build_rules/apache_runner.py
new file mode 100644
index 0000000..3364216
--- /dev/null
+++ b/tools/build_rules/apache_runner.py
@@ -0,0 +1,121 @@
+"""Starts up Apache to provide HTTPS + LDAP for another web server.
+
+This script is used by the apache_wrapper() rule as the main entrypoint for its
+"executable". This script sets up a minimal Apache environment in a directory
+in /tmp.
+
+Both Apache and the wrapped server binary are started by this script. The
+wrapped server should bind to the port specified by the APACHE_WRAPPED_PORT
+environment variable.
+
+See the documentation for apache_wrapper() for more information.
+"""
+
+import argparse
+import json
+import os
+from pathlib import Path
+import signal
+import subprocess
+import sys
+import tempfile
+
+import jinja2
+
+DUMMY_CERT_ANSWERS = """\
+US
+California
+Mountain View
+FRC971
+Software
+frc971.org
+dummy@frc971.org
+"""
+
+def main(argv):
+ parser = argparse.ArgumentParser()
+ parser.add_argument("--binary", type=str, required=True)
+ parser.add_argument("--https_port", type=int, default=7000)
+ parser.add_argument("--wrapped_port", type=int, default=7500)
+ parser.add_argument(
+ "--ldap_info",
+ type=str,
+ help="JSON file containing 'ldap_bind_dn', 'ldap_url', and 'ldap_password' entries.",
+ default="",
+ )
+ args = parser.parse_args(argv[1:])
+
+ if not args.ldap_info:
+ args.ldap_info = os.path.join(os.environ["BUILD_WORKSPACE_DIRECTORY"], "ldap.json")
+
+ with open("tools/build_rules/apache_template.conf", "r") as file:
+ template = jinja2.Template(file.read())
+
+ with open(args.ldap_info, "r") as file:
+ substitutions = json.load(file)
+
+ for key in ("ldap_bind_dn", "ldap_url", "ldap_password"):
+ if key not in substitutions:
+ raise KeyError(f"The ldap_info JSON file must contain key '{key}'.")
+
+ substitutions.update({
+ "https_port": args.https_port,
+ "wrapped_port": args.wrapped_port,
+ })
+
+ config_text = template.render(substitutions)
+
+ with tempfile.TemporaryDirectory() as temp_dir:
+ temp_dir = Path(temp_dir)
+ with open(temp_dir / "apache2.conf", "w") as file:
+ file.write(config_text)
+
+ # Create a directory for error logs and such.
+ logs_dir = temp_dir / "logs"
+ os.mkdir(logs_dir)
+
+ print("-" * 60)
+ print(f"Logs are in {logs_dir}/")
+ print("-" * 60)
+
+ # Make modules available.
+ modules_path = Path("external/apache2/usr/lib/apache2/modules")
+ os.symlink(modules_path.resolve(), temp_dir / "modules")
+
+ # Generate a testing cert.
+ subprocess.run([
+ "openssl",
+ "req",
+ "-x509",
+ "-nodes",
+ "-days=365",
+ "-newkey=rsa:2048",
+ "-keyout=" + str(temp_dir / "apache-selfsigned.key"),
+ "-out=" + str(temp_dir / "apache-selfsigned.crt"),
+ ],
+ check=True,
+ input=DUMMY_CERT_ANSWERS,
+ text=True,
+ )
+
+ # Start the wrapped binary in the background.
+ # Tell it via the environment what port to listen on.
+ env = os.environ.copy()
+ env["APACHE_WRAPPED_PORT"] = str(args.wrapped_port)
+ wrapped_binary = subprocess.Popen([args.binary], env=env)
+
+ # Start the apache server.
+ env = os.environ.copy()
+ env["LD_LIBRARY_PATH"] = "external/apache2/usr/lib/x86_64-linux-gnu"
+ try:
+ subprocess.run(
+ ["external/apache2/usr/sbin/apache2", "-X", "-d", str(temp_dir)],
+ check=True,
+ env=env,
+ )
+ finally:
+ wrapped_binary.send_signal(signal.SIGINT)
+ wrapped_binary.wait()
+
+if __name__ == "__main__":
+ sys.exit(main(sys.argv))
diff --git a/tools/build_rules/apache_template.conf b/tools/build_rules/apache_template.conf
new file mode 100644
index 0000000..91a8338
--- /dev/null
+++ b/tools/build_rules/apache_template.conf
@@ -0,0 +1,57 @@
+PidFile logs/httpd.pid
+
+ServerTokens Prod
+UseCanonicalName On
+TraceEnable Off
+
+Listen 127.0.0.1:{{ https_port }}
+
+LoadModule mpm_event_module modules/mod_mpm_event.so
+LoadModule authn_core_module modules/mod_authn_core.so
+LoadModule authz_core_module modules/mod_authz_core.so
+LoadModule authz_user_module modules/mod_authz_user.so
+LoadModule auth_basic_module modules/mod_auth_basic.so
+LoadModule authnz_ldap_module modules/mod_authnz_ldap.so
+LoadModule ldap_module modules/mod_ldap.so
+LoadModule proxy_module modules/mod_proxy.so
+LoadModule proxy_http_module modules/mod_proxy_http.so
+LoadModule ssl_module modules/mod_ssl.so
+
+{% raw %}
+ErrorLogFormat "[%{cu}t] [%-m:%-l] %-a %-L %M"
+LogFormat "%h %l %u [%{%Y-%m-%d %H:%M:%S}t.%{usec_frac}t] \"%r\" %>s %b \
+\"%{Referer}i\" \"%{User-Agent}i\"" combined
+{% endraw %}
+
+LogLevel debug
+ErrorLog logs/error.log
+CustomLog logs/access.log combined
+
+LDAPCacheEntries 1024
+LDAPCacheTTL 600
+LDAPTrustedGlobalCert CA_BASE64 "apache-selfsigned.crt"
+LDAPTrustedMode STARTTLS
+LDAPLibraryDebug 7
+LDAPVerifyServerCert OFF
+
+<VirtualHost *:{{ https_port }}>
+ ServerName localhost
+ ServerAdmin root@localhost
+
+ SSLEngine on
+ SSLProxyEngine On
+ SSLCertificateFile apache-selfsigned.crt
+ SSLCertificateKeyFile apache-selfsigned.key
+
+ ProxyPass "/" http://localhost:{{ wrapped_port }}/
+
+ <Location />
+ AuthName "Enter your Robotics 971 credentials"
+ AuthType Basic
+ AuthBasicProvider ldap
+ AuthLDAPBindDN "{{ ldap_bind_dn }}"
+ AuthLDAPBindPassword {{ ldap_password }}
+ AuthLDAPURL "{{ ldap_url }}"
+ Require valid-user
+ </Location>
+</VirtualHost>
diff --git a/y2020/BUILD b/y2020/BUILD
index 0a32370..82a7848 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -155,9 +155,9 @@
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
"//aos/network:timestamp_fbs",
+ "//frc971/vision:vision_fbs",
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision:vision_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -179,12 +179,12 @@
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//aos/network:remote_message_fbs",
"//aos/network:timestamp_fbs",
+ "//frc971/vision:vision_fbs",
+ "//y2020/vision:galactic_search_path_fbs",
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision:vision_fbs",
- "//aos/network:remote_message_fbs",
- "//y2020/vision:galactic_search_path_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
@@ -209,11 +209,11 @@
flatbuffers = [
"//aos/network:message_bridge_client_fbs",
"//aos/network:message_bridge_server_fbs",
+ "//aos/network:remote_message_fbs",
"//aos/network:timestamp_fbs",
+ "//frc971/vision:vision_fbs",
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
- "//y2020/vision:vision_fbs",
- "//aos/network:remote_message_fbs",
],
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
diff --git a/y2020/vision/BUILD b/y2020/vision/BUILD
index d94de5a..5156b59 100644
--- a/y2020/vision/BUILD
+++ b/y2020/vision/BUILD
@@ -1,12 +1,4 @@
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
-
-flatbuffer_cc_library(
- name = "vision_fbs",
- srcs = ["vision.fbs"],
- gen_reflections = 1,
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
-)
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
flatbuffer_cc_library(
name = "galactic_search_path_fbs",
@@ -16,25 +8,6 @@
visibility = ["//y2020:__subpackages__"],
)
-cc_library(
- name = "v4l2_reader",
- srcs = [
- "v4l2_reader.cc",
- ],
- hdrs = [
- "v4l2_reader.h",
- ],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
- deps = [
- ":vision_fbs",
- "//aos/events:event_loop",
- "//aos/scoped:scoped_fd",
- "@com_github_google_glog//:glog",
- "@com_google_absl//absl/base",
- ],
-)
-
cc_binary(
name = "camera_reader",
srcs = [
@@ -61,13 +34,13 @@
"//y2020:config",
],
target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
+ visibility = ["//y2020:__subpackages__"] + ["//y2022:__subpackages__"],
deps = [
- ":v4l2_reader",
- ":vision_fbs",
"//aos:flatbuffer_merge",
"//aos/events:event_loop",
"//aos/network:team_number",
+ "//frc971/vision:v4l2_reader",
+ "//frc971/vision:vision_fbs",
"//third_party:opencv",
"//y2020/vision/sift:sift971",
"//y2020/vision/sift:sift_fbs",
@@ -76,13 +49,6 @@
],
)
-flatbuffer_ts_library(
- name = "vision_ts_fbs",
- srcs = ["vision.fbs"],
- target_compatible_with = ["@platforms//os:linux"],
- visibility = ["//y2020:__subpackages__"],
-)
-
cc_binary(
name = "viewer",
srcs = [
@@ -94,9 +60,10 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2020:__subpackages__"],
deps = [
- ":vision_fbs",
"//aos:init",
"//aos/events:shm_event_loop",
+ "//frc971/vision:v4l2_reader",
+ "//frc971/vision:vision_fbs",
"//third_party:opencv",
"//y2020/vision/sift:sift_fbs",
],
@@ -113,12 +80,12 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2020:__subpackages__"],
deps = [
- ":vision_fbs",
"//aos:flatbuffers",
"//aos/events:event_loop",
"//aos/network:message_bridge_server_fbs",
"//aos/network:team_number",
"//frc971/control_loops:quaternion_utils",
+ "//frc971/vision:vision_fbs",
"//third_party:opencv",
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
@@ -142,10 +109,10 @@
visibility = ["//y2020:__subpackages__"],
deps = [
":charuco_lib",
- ":vision_fbs",
"//aos:init",
"//aos/events:shm_event_loop",
"//frc971/control_loops/drivetrain:improved_down_estimator",
+ "//frc971/vision:vision_fbs",
"//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//third_party:opencv",
@@ -168,10 +135,10 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//y2020:__subpackages__"],
deps = [
- ":vision_fbs",
"//aos:init",
"//aos/events:simulated_event_loop",
"//aos/events/logging:log_reader",
+ "//frc971/vision:vision_fbs",
"//third_party:opencv",
],
)
diff --git a/y2020/vision/camera_reader.cc b/y2020/vision/camera_reader.cc
index 1e28e82..9966a5b 100644
--- a/y2020/vision/camera_reader.cc
+++ b/y2020/vision/camera_reader.cc
@@ -9,12 +9,12 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
#include "y2020/vision/sift/sift971.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
#include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/v4l2_reader.h"
-#include "y2020/vision/vision_generated.h"
DEFINE_bool(skip_sift, false,
"If true don't run any feature extraction. Just forward images.");
diff --git a/y2020/vision/camera_reader.h b/y2020/vision/camera_reader.h
index c05ebea..37fb5a9 100644
--- a/y2020/vision/camera_reader.h
+++ b/y2020/vision/camera_reader.h
@@ -10,12 +10,12 @@
#include "aos/events/event_loop.h"
#include "aos/flatbuffer_merge.h"
#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
#include "y2020/vision/sift/sift971.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
#include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/v4l2_reader.h"
-#include "y2020/vision/vision_generated.h"
namespace frc971 {
namespace vision {
diff --git a/y2020/vision/charuco_lib.cc b/y2020/vision/charuco_lib.cc
index 21bdcc3..0df820b 100644
--- a/y2020/vision/charuco_lib.cc
+++ b/y2020/vision/charuco_lib.cc
@@ -11,11 +11,11 @@
#include "aos/flatbuffers.h"
#include "aos/network/team_number.h"
#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/vision_generated.h"
#include "glog/logging.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
#include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/vision_generated.h"
DEFINE_uint32(min_targets, 10,
"The mininum number of targets required to match.");
@@ -133,8 +133,7 @@
const monotonic_clock::time_point eof = eof_source_node - offset;
- const monotonic_clock::duration age =
- event_loop_->monotonic_now() - eof;
+ const monotonic_clock::duration age = event_loop_->monotonic_now() - eof;
const double age_double =
std::chrono::duration_cast<std::chrono::duration<double>>(age).count();
if (age > std::chrono::milliseconds(100)) {
diff --git a/y2020/vision/extrinsics_calibration.cc b/y2020/vision/extrinsics_calibration.cc
index 1d4de28..c7ef752 100644
--- a/y2020/vision/extrinsics_calibration.cc
+++ b/y2020/vision/extrinsics_calibration.cc
@@ -12,13 +12,13 @@
#include "frc971/analysis/in_process_plotter.h"
#include "frc971/control_loops/drivetrain/improved_down_estimator.h"
#include "frc971/control_loops/quaternion_utils.h"
+#include "frc971/vision/vision_generated.h"
#include "frc971/wpilib/imu_batch_generated.h"
#include "y2020/vision/calibration_accumulator.h"
#include "y2020/vision/charuco_lib.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
#include "y2020/vision/tools/python_code/sift_training_data.h"
-#include "y2020/vision/vision_generated.h"
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_string(pi, "pi-7971-2", "Pi name to calibrate.");
@@ -120,7 +120,7 @@
const Eigen::Quaternion<Scalar> &orientation() const { return orientation_; }
- std::vector<Eigen::Matrix<Scalar, 3, 1> > errors_;
+ std::vector<Eigen::Matrix<Scalar, 3, 1>> errors_;
// Returns the angular errors for each camera sample.
size_t num_errors() const { return errors_.size(); }
@@ -136,18 +136,18 @@
result.template block<4, 1>(0, 0) = q.coeffs();
result.template block<6, 1>(4, 0) = x_hat;
result.template block<36, 1>(10, 0) =
- Eigen::Map<Eigen::Matrix<Scalar, 36, 1> >(p.data(), p.size());
+ Eigen::Map<Eigen::Matrix<Scalar, 36, 1>>(p.data(), p.size());
return result;
}
std::tuple<Eigen::Quaternion<Scalar>, Eigen::Matrix<Scalar, 6, 1>,
- Eigen::Matrix<Scalar, 6, 6> >
+ Eigen::Matrix<Scalar, 6, 6>>
UnPack(Eigen::Matrix<Scalar, 46, 1> input) {
Eigen::Quaternion<Scalar> q(input.template block<4, 1>(0, 0));
Eigen::Matrix<Scalar, 6, 1> x_hat(input.template block<6, 1>(4, 0));
Eigen::Matrix<Scalar, 6, 6> p =
- Eigen::Map<Eigen::Matrix<Scalar, 6, 6> >(input.data() + 10, 6, 6);
+ Eigen::Map<Eigen::Matrix<Scalar, 6, 6>>(input.data() + 10, 6, 6);
return std::make_tuple(q, x_hat, p);
}
@@ -361,8 +361,8 @@
std::vector<double> imu_ratez;
std::vector<double> times_;
- std::vector<Eigen::Matrix<double, 6, 1> > x_hats_;
- std::vector<Eigen::Quaternion<double> > orientations_;
+ std::vector<Eigen::Matrix<double, 6, 1>> x_hats_;
+ std::vector<Eigen::Quaternion<double>> orientations_;
Eigen::Matrix<double, 3, 1> last_accel_ = Eigen::Matrix<double, 3, 1>::Zero();
};
diff --git a/y2020/vision/rootfs/README.md b/y2020/vision/rootfs/README.md
deleted file mode 100644
index 0f66864..0000000
--- a/y2020/vision/rootfs/README.md
+++ /dev/null
@@ -1,32 +0,0 @@
-This modifies a stock debian root filesystem to be able to operate as a vision
-pi. It is not trying to be reproducible, but should be good enough for FRC
-purposes.
-
-The default hostname and IP is pi-8971-1, 10.89.71.101.
- Username pi, password raspberry.
-
-Download 2020-08-20-raspios-buster-armhf-lite.img (or any newer buster
-version, as a .zip file) from
-`https://www.raspberrypi.org/downloads/raspberry-pi-os/` (or
-`https://downloads.raspberrypi.org/raspios_lite_armhf/images/` for
-older images), extract (unzip) the .img file, and edit
-`modify_rootfs.sh` to point to it.
-
-Run modify_rootfs.sh to build the filesystem (you might need to hit
-return in a spot or two and will need sudo privileges to mount the
-partition).
-
-After confirming the target device, deploy by copying the contents of the image
-to the SD card.
-
- `dd if=2020-02-13-raspbian-buster-lite.img of=/dev/sdX bs=1M`
-
-Use `lsblk` to find the device and make absolutely sure this isn't your hard
-drive or something else.
-
-From there, log in, `sudo` to `root`, and run `/root/bin/change_hostname.sh` to
-change the hostname to the actual target.
-
-A couple additional notes on setting this up:
- * You'll likely need to install (`sudo apt install`) the emulation packages `proot` and `qemu-user-static` (or possibly `qemu-arm-static`)
- * If the modify_root.sh script fails, you may need to manually unmount the image (`sudo umount ${IMAGE}`) before running it again
diff --git a/y2020/vision/rootfs/make_sd.sh b/y2020/vision/rootfs/make_sd.sh
deleted file mode 100755
index eba47bc..0000000
--- a/y2020/vision/rootfs/make_sd.sh
+++ /dev/null
@@ -1,30 +0,0 @@
-#!/bin/bash
-
-set -e
-
-IMAGE="2020-08-20-raspios-buster-armhf-lite.img"
-DEVICE="/dev/sda"
-
-if mount | grep "${DEVICE}" >/dev/null ;
-then
- echo "Overwriting a mounted partition, is ${DEVICE} the sd card?"
- exit 1
-fi
-
-sudo dd if=${IMAGE} of=${DEVICE} bs=1M status=progress
-
-PARTITION="${IMAGE}.partition"
-
-mkdir -p "${PARTITION}"
-sudo mount "${DEVICE}2" "${PARTITION}"
-
-function target() {
- HOME=/root/ USER=root sudo proot -0 -q qemu-arm-static -w / -r "${PARTITION}" "$@"
-}
-
-target /root/bin/change_hostname.sh "${1}"
-
-echo "Starting a shell for any manual configuration"
-target /bin/bash --rcfile /root/.bashrc
-
-sudo umount "${PARTITION}"
diff --git a/y2020/vision/tools/python_code/BUILD b/y2020/vision/tools/python_code/BUILD
index b495658..d259ba7 100644
--- a/y2020/vision/tools/python_code/BUILD
+++ b/y2020/vision/tools/python_code/BUILD
@@ -192,8 +192,8 @@
deps = [
":sift_training_data_test",
"//aos/testing:googletest",
+ "//frc971/vision:vision_fbs",
"//third_party:opencv",
- "//y2020/vision:vision_fbs",
"//y2020/vision/sift:sift_fbs",
"//y2020/vision/sift:sift_training_fbs",
],
diff --git a/y2020/vision/tools/python_code/camera_definition.py b/y2020/vision/tools/python_code/camera_definition.py
index 465f7bd..3e3623d 100644
--- a/y2020/vision/tools/python_code/camera_definition.py
+++ b/y2020/vision/tools/python_code/camera_definition.py
@@ -147,8 +147,8 @@
dist_coeffs = np.asarray(calib_dict["dist_coeffs"]).reshape(
(1, 5))
- glog.info("Found calib for " + node_name + ", team #" +
- str(team_number))
+ glog.debug("Found calib for " + node_name + ", team #" +
+ str(team_number))
camera_params = CameraParameters()
camera_params.camera_ext, camera_params.turret_ext = compute_extrinsic_by_pi(
diff --git a/y2020/vision/tools/python_code/camera_param_test.cc b/y2020/vision/tools/python_code/camera_param_test.cc
index 483fe75..a2d3a75 100644
--- a/y2020/vision/tools/python_code/camera_param_test.cc
+++ b/y2020/vision/tools/python_code/camera_param_test.cc
@@ -12,9 +12,9 @@
#include "y2020/vision/tools/python_code/sift_training_data.h"
#endif
+#include "frc971/vision/vision_generated.h"
#include "y2020/vision/sift/sift_generated.h"
#include "y2020/vision/sift/sift_training_generated.h"
-#include "y2020/vision/vision_generated.h"
namespace frc971 {
namespace vision {
diff --git a/y2020/vision/tools/python_code/target_definition.py b/y2020/vision/tools/python_code/target_definition.py
index e518c8c..8d1c601 100644
--- a/y2020/vision/tools/python_code/target_definition.py
+++ b/y2020/vision/tools/python_code/target_definition.py
@@ -5,6 +5,7 @@
import json
import math
import numpy as np
+import os
import camera_definition
import define_training_data as dtd
@@ -153,7 +154,10 @@
# otherwise get them directly from the training targets.
for target in (ideal_target_list
if AUTO_PROJECTION else training_target_list):
- glog.debug("\nPreparing target for image %s" % target.image_filename)
+ # TODO<Jim>: Save this info to flatbuffer and publish on start
+ # And then, make this debug message again
+ glog.debug("\nPreparing target for image %s" %
+ os.path.basename(target.image_filename))
target.extract_features(feature_extractor)
target.filter_keypoints_by_polygons()
target.compute_reprojection_maps()
diff --git a/y2020/vision/tools/python_code/train_and_match.py b/y2020/vision/tools/python_code/train_and_match.py
index 6250da1..ca67095 100644
--- a/y2020/vision/tools/python_code/train_and_match.py
+++ b/y2020/vision/tools/python_code/train_and_match.py
@@ -52,13 +52,13 @@
# Load feature extractor based on extractor name
# Returns feature extractor object
def load_feature_extractor():
- if FEATURE_EXTRACTOR_NAME is 'SIFT':
+ if FEATURE_EXTRACTOR_NAME == 'SIFT':
# Initiate SIFT detector
feature_extractor = cv2.SIFT_create()
- elif FEATURE_EXTRACTOR_NAME is 'SURF':
+ elif FEATURE_EXTRACTOR_NAME == 'SURF':
# Initiate SURF detector
feature_extractor = cv2.xfeatures2d.SURF_create()
- elif FEATURE_EXTRACTOR_NAME is 'ORB':
+ elif FEATURE_EXTRACTOR_NAME == 'ORB':
# Initiate ORB detector
feature_extractor = cv2.ORB_create()
@@ -78,7 +78,7 @@
kp, des = feature_extractor.detectAndCompute(image_list[i], None)
descriptor_lists.append(des)
keypoint_lists.append(kp)
- elif FEATURE_EXTRACTOR_NAME is 'ORB':
+ elif FEATURE_EXTRACTOR_NAME == 'ORB':
# TODO: Check whether ORB extractor can do detectAndCompute.
# If so, we don't need to have this branch for ORB
for i in range(len(image_list)):
@@ -104,7 +104,7 @@
matcher = cv2.FlannBasedMatcher(index_params, search_params)
matcher.add(descriptor_lists)
matcher.train()
- elif FEATURE_EXTRACTOR_NAME is 'ORB':
+ elif FEATURE_EXTRACTOR_NAME == 'ORB':
# Use FLANN LSH for ORB
FLANN_INDEX_LSH = 6
index_params = dict(
@@ -144,7 +144,7 @@
good_matches_list.append(good_matches)
- elif FEATURE_EXTRACTOR_NAME is 'ORB':
+ elif FEATURE_EXTRACTOR_NAME == 'ORB':
matches = matcher.knnMatch(train_keypoint_lists[0], desc_query, k=2)
good_matches = []
for m in matches:
diff --git a/y2020/vision/viewer.cc b/y2020/vision/viewer.cc
index 2aff3fb..37ff4a3 100644
--- a/y2020/vision/viewer.cc
+++ b/y2020/vision/viewer.cc
@@ -8,8 +8,9 @@
#include "aos/events/shm_event_loop.h"
#include "aos/init.h"
#include "aos/time/time.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
#include "y2020/vision/sift/sift_generated.h"
-#include "y2020/vision/vision_generated.h"
DEFINE_string(config, "config.json", "Path to the config file to use.");
DEFINE_bool(show_features, true, "Show the SIFT features that matched.");
diff --git a/y2020/vision/viewer_replay.cc b/y2020/vision/viewer_replay.cc
index 93e531d..a818859 100644
--- a/y2020/vision/viewer_replay.cc
+++ b/y2020/vision/viewer_replay.cc
@@ -6,7 +6,7 @@
#include "aos/events/logging/log_reader.h"
#include "aos/events/simulated_event_loop.h"
#include "aos/init.h"
-#include "y2020/vision/vision_generated.h"
+#include "frc971/vision/vision_generated.h"
DEFINE_string(node, "pi1", "Node name to replay.");
DEFINE_string(image_save_prefix, "/tmp/img",
diff --git a/y2020/www/BUILD b/y2020/www/BUILD
index 27c0392..8467994 100644
--- a/y2020/www/BUILD
+++ b/y2020/www/BUILD
@@ -15,7 +15,7 @@
"//aos/network:connect_ts_fbs",
"//aos/network:web_proxy_ts_fbs",
"//aos/network/www:proxy",
- "//y2020/vision:vision_ts_fbs",
+ "//frc971/vision:vision_ts_fbs",
"//y2020/vision/sift:sift_ts_fbs",
"@com_github_google_flatbuffers//ts:flatbuffers_ts",
],
diff --git a/y2020/www/image_handler.ts b/y2020/www/image_handler.ts
index b254f2c..661100b 100644
--- a/y2020/www/image_handler.ts
+++ b/y2020/www/image_handler.ts
@@ -4,7 +4,7 @@
import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
import {Long} from 'org_frc971/external/com_github_google_flatbuffers/ts/long';
import * as sift from 'org_frc971/y2020/vision/sift/sift_generated'
-import * as vision from 'org_frc971/y2020/vision/vision_generated';
+import * as vision from 'org_frc971/frc971/vision/vision_generated';
import * as web_proxy from 'org_frc971/aos/network/web_proxy_generated';
import Channel = configuration.aos.Channel;
diff --git a/y2022/BUILD b/y2022/BUILD
index dba5796..2173877 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -1,19 +1,152 @@
load("//frc971:downloader.bzl", "robot_downloader")
load("//aos:config.bzl", "aos_config")
+load("//tools/build_rules:template.bzl", "jinja2_template")
robot_downloader(
+ binaries = [
+ "//aos/network:web_proxy_main",
+ ],
data = [
":config",
],
start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:web_proxy_main",
":joystick_reader",
":wpilib_interface",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//y2022/actors:binaries",
"//y2022/control_loops/drivetrain:drivetrain",
"//y2022/control_loops/superstructure:superstructure",
- "//y2022/actors:binaries",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
+robot_downloader(
+ name = "pi_download",
+ binaries = [
+ "//y2022/vision:viewer",
+ ],
+ data = [
+ ":config",
+ ],
+ start_binaries = [
+ "//aos/events/logging:logger_main",
+ "//aos/network:message_bridge_client",
+ "//aos/network:message_bridge_server",
+ "//aos/network:web_proxy_main",
+ "//y2022/vision:camera_reader",
+ ],
+ target_compatible_with = ["//tools/platforms/hardware:raspberry_pi"],
+ target_type = "pi",
+)
+
+aos_config(
+ name = "config",
+ src = "y2022.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//frc971/input:robot_state_fbs",
+ "//frc971/vision:vision_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ ":config_logger",
+ ":config_pi1",
+ ":config_pi2",
+ ":config_pi3",
+ ":config_pi4",
+ ":config_pi5",
+ ":config_roborio",
],
)
+[
+ aos_config(
+ name = "config_" + pi,
+ src = "y2022_" + pi + ".json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:vision_fbs",
+ "//y2022/vision:target_estimate_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:config",
+ "//frc971/control_loops/drivetrain:config",
+ "//frc971/input:config",
+ ],
+ )
+ for pi in [
+ "pi1",
+ "pi2",
+ "pi3",
+ "pi4",
+ "pi5",
+ ]
+]
+
+aos_config(
+ name = "config_logger",
+ src = "y2022_logger.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//frc971/vision:vision_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:config",
+ "//frc971/control_loops/drivetrain:config",
+ "//frc971/input:config",
+ ],
+)
+
+aos_config(
+ name = "config_roborio",
+ src = "y2022_roborio.json",
+ flatbuffers = [
+ "//aos/network:remote_message_fbs",
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//y2019/control_loops/drivetrain:target_selector_fbs",
+ "//y2022/control_loops/superstructure:superstructure_goal_fbs",
+ "//y2022/control_loops/superstructure:superstructure_output_fbs",
+ "//y2022/control_loops/superstructure:superstructure_position_fbs",
+ "//y2022/control_loops/superstructure:superstructure_status_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ "//aos/events:config",
+ "//frc971/autonomous:config",
+ "//frc971/control_loops/drivetrain:config",
+ "//frc971/input:config",
+ "//frc971/wpilib:config",
+ ],
+)
+
+[
+ jinja2_template(
+ name = "y2022_pi" + str(num) + ".json",
+ src = "y2022_pi_template.json",
+ parameters = {"NUM": str(num)},
+ target_compatible_with = ["@platforms//os:linux"],
+ )
+ for num in range(1, 6)
+]
+
cc_library(
name = "constants",
srcs = [
@@ -96,21 +229,3 @@
"//y2022/control_loops/superstructure:superstructure_status_fbs",
],
)
-
-aos_config(
- name = "config",
- src = "y2022.json",
- flatbuffers = [
- "//y2022/control_loops/superstructure:superstructure_goal_fbs",
- "//y2022/control_loops/superstructure:superstructure_output_fbs",
- "//y2022/control_loops/superstructure:superstructure_position_fbs",
- "//y2022/control_loops/superstructure:superstructure_status_fbs",
- ],
- visibility = ["//visibility:public"],
- deps = [
- "//frc971/autonomous:config",
- "//frc971/control_loops/drivetrain:config",
- "//frc971/input:config",
- "//frc971/wpilib:config",
- ],
-)
diff --git a/y2022/control_loops/superstructure/superstructure.cc b/y2022/control_loops/superstructure/superstructure.cc
index e985779..b2c89d1 100644
--- a/y2022/control_loops/superstructure/superstructure.cc
+++ b/y2022/control_loops/superstructure/superstructure.cc
@@ -26,7 +26,7 @@
if (output != nullptr && unsafe_goal != nullptr) {
OutputT output_struct;
-
+ output_struct.climber_voltage = unsafe_goal->climber_speed();
output->CheckOk(output->Send(Output::Pack(*output->fbb(), &output_struct)));
}
@@ -36,8 +36,8 @@
status_builder.add_estopped(false);
if (unsafe_goal != nullptr) {
+ status_builder.add_climber_speed(unsafe_goal->climber_speed());
}
-
(void)status->Send(status_builder.Finish());
}
diff --git a/y2022/control_loops/superstructure/superstructure_goal.fbs b/y2022/control_loops/superstructure/superstructure_goal.fbs
index 439c0fb..3e0679e 100644
--- a/y2022/control_loops/superstructure/superstructure_goal.fbs
+++ b/y2022/control_loops/superstructure/superstructure_goal.fbs
@@ -3,7 +3,9 @@
namespace y2022.control_loops.superstructure;
table Goal {
-
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_speed:double (id: 0);
}
root_type Goal;
diff --git a/y2022/control_loops/superstructure/superstructure_output.fbs b/y2022/control_loops/superstructure/superstructure_output.fbs
index e95fbe3..78b2c01 100644
--- a/y2022/control_loops/superstructure/superstructure_output.fbs
+++ b/y2022/control_loops/superstructure/superstructure_output.fbs
@@ -1,7 +1,9 @@
namespace y2022.control_loops.superstructure;
table Output {
-
+ // Voltage of the climber falcon
+ // - is down + is up
+ climber_voltage:double (id: 0);
}
root_type Output;
diff --git a/y2022/control_loops/superstructure/superstructure_status.fbs b/y2022/control_loops/superstructure/superstructure_status.fbs
index deff2f6..5f8cdc3 100644
--- a/y2022/control_loops/superstructure/superstructure_status.fbs
+++ b/y2022/control_loops/superstructure/superstructure_status.fbs
@@ -9,6 +9,10 @@
// If true, we have aborted. This is the or of all subsystem estops.
estopped:bool (id: 1);
+
+ // Goal voltage of the climber falcon
+ // - is down + is up
+ climber_speed:double (id: 2);
}
root_type Status;
\ No newline at end of file
diff --git a/y2022/vision/BUILD b/y2022/vision/BUILD
index e69de29..e893e54 100644
--- a/y2022/vision/BUILD
+++ b/y2022/vision/BUILD
@@ -0,0 +1,103 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+
+cc_binary(
+ name = "camera_reader",
+ srcs = [
+ "camera_reader_main.cc",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":camera_reader_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ ],
+)
+
+cc_library(
+ name = "camera_reader_lib",
+ srcs = [
+ "camera_reader.cc",
+ ],
+ hdrs = [
+ "camera_reader.h",
+ ],
+ data = [
+ "//y2022:config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":blob_detector_lib",
+ ":target_estimator_lib",
+ "//aos:flatbuffer_merge",
+ "//aos/events:event_loop",
+ "//aos/network:team_number",
+ "//frc971/vision:v4l2_reader",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ "//y2020/vision/sift:sift_fbs",
+ "//y2020/vision/sift:sift_training_fbs",
+ "//y2020/vision/tools/python_code:sift_training_data",
+ ],
+)
+
+cc_library(
+ name = "blob_detector_lib",
+ srcs = [
+ "blob_detector.cc",
+ ],
+ hdrs = [
+ "blob_detector.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ "//aos/network:team_number",
+ "//third_party:opencv",
+ ],
+)
+
+cc_library(
+ name = "target_estimator_lib",
+ srcs = [
+ "target_estimator.cc",
+ ],
+ hdrs = [
+ "target_estimator.h",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":target_estimate_fbs",
+ "//third_party:opencv",
+ ],
+)
+
+flatbuffer_cc_library(
+ name = "target_estimate_fbs",
+ srcs = ["target_estimate.fbs"],
+ gen_reflections = 1,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+)
+
+cc_binary(
+ name = "viewer",
+ srcs = [
+ "viewer.cc",
+ ],
+ data = [
+ "//y2022:config",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//y2022:__subpackages__"],
+ deps = [
+ ":blob_detector_lib",
+ ":target_estimator_lib",
+ "//aos:init",
+ "//aos/events:shm_event_loop",
+ "//frc971/vision:vision_fbs",
+ "//third_party:opencv",
+ ],
+)
diff --git a/y2022/vision/blob_detector.cc b/y2022/vision/blob_detector.cc
new file mode 100644
index 0000000..b76ffc5
--- /dev/null
+++ b/y2022/vision/blob_detector.cc
@@ -0,0 +1,148 @@
+#include "y2022/vision/blob_detector.h"
+
+#include <cmath>
+#include <string>
+
+#include "aos/network/team_number.h"
+#include "opencv2/features2d.hpp"
+#include "opencv2/imgproc.hpp"
+
+DEFINE_uint64(green_delta, 50,
+ "Required difference between green pixels vs. red and blue");
+DEFINE_bool(use_outdoors, false,
+ "If true, change thresholds to handle outdoor illumination");
+
+namespace y2022 {
+namespace vision {
+
+cv::Mat BlobDetector::ThresholdImage(cv::Mat rgb_image) {
+ cv::Mat binarized_image(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
+ for (int row = 0; row < rgb_image.rows; row++) {
+ for (int col = 0; col < rgb_image.cols; col++) {
+ cv::Vec3b pixel = rgb_image.at<cv::Vec3b>(row, col);
+ uint8_t blue = pixel.val[0];
+ uint8_t green = pixel.val[1];
+ uint8_t red = pixel.val[2];
+ // Simple filter that looks for green pixels sufficiently brigher than
+ // red and blue
+ if ((green > blue + FLAGS_green_delta) &&
+ (green > red + FLAGS_green_delta)) {
+ binarized_image.at<uint8_t>(row, col) = 255;
+ } else {
+ binarized_image.at<uint8_t>(row, col) = 0;
+ }
+ }
+ }
+
+ return binarized_image;
+}
+
+std::vector<std::vector<cv::Point>> BlobDetector::FindBlobs(
+ cv::Mat binarized_image) {
+ // find the contours (blob outlines)
+ std::vector<std::vector<cv::Point>> contours;
+ std::vector<cv::Vec4i> hierarchy;
+ cv::findContours(binarized_image, contours, hierarchy, cv::RETR_CCOMP,
+ cv::CHAIN_APPROX_SIMPLE);
+
+ return contours;
+}
+
+std::vector<BlobDetector::BlobStats> BlobDetector::ComputeStats(
+ std::vector<std::vector<cv::Point>> blobs) {
+ std::vector<BlobDetector::BlobStats> blob_stats;
+ for (auto blob : blobs) {
+ // Make the blob convex before finding bounding box
+ std::vector<cv::Point> convex_blob;
+ cv::convexHull(blob, convex_blob);
+ auto blob_size = cv::boundingRect(convex_blob).size();
+ cv::Moments moments = cv::moments(convex_blob);
+
+ const auto centroid =
+ cv::Point(moments.m10 / moments.m00, moments.m01 / moments.m00);
+ const double aspect_ratio =
+ static_cast<double>(blob_size.width) / blob_size.height;
+ const double area = moments.m00;
+ const size_t points = blob.size();
+
+ blob_stats.emplace_back(BlobStats{centroid, aspect_ratio, area, points});
+ }
+ return blob_stats;
+}
+
+// Filter blobs to get rid of noise, too large items, etc.
+std::vector<std::vector<cv::Point>> BlobDetector::FilterBlobs(
+ std::vector<std::vector<cv::Point>> blobs,
+ std::vector<BlobDetector::BlobStats> blob_stats) {
+ std::vector<std::vector<cv::Point>> filtered_blobs;
+
+ auto blob_it = blobs.begin();
+ auto stats_it = blob_stats.begin();
+ while (blob_it < blobs.end() && stats_it < blob_stats.end()) {
+ // To estimate the maximum y, we can figure out the y value of the blobs
+ // when the camera is the farthest from the target, at the field corner.
+ // We can solve for the pitch of the blob:
+ // blob_pitch = atan((height_tape - height_camera) / depth) + camera_pitch
+ // The triangle with the height of the tape above the camera and the camera
+ // depth is similar to the one with the focal length in y pixels and the y
+ // coordinate offset from the center of the image.
+ // Therefore y_offset = focal_length_y * tan(blob_pitch), and
+ // y = -(y_offset - offset_y)
+ constexpr int kMaxY = 400;
+ constexpr double kTapeAspectRatio = 5.0 / 2.0;
+ constexpr double kAspectRatioThreshold = 1.5;
+ constexpr double kMinArea = 10;
+ constexpr size_t kMinPoints = 6;
+
+ // Remove all blobs that are at the bottom of the image, have a different
+ // aspect ratio than the tape, or have too little area or points
+ // TODO(milind): modify to take into account that blobs will be on the side.
+ if ((stats_it->centroid.y <= kMaxY) &&
+ (std::abs(kTapeAspectRatio - stats_it->aspect_ratio) <
+ kAspectRatioThreshold) &&
+ (stats_it->area >= kMinArea) && (stats_it->points >= kMinPoints)) {
+ filtered_blobs.push_back(*blob_it);
+ }
+ blob_it++;
+ stats_it++;
+ }
+
+ return filtered_blobs;
+}
+
+void BlobDetector::DrawBlobs(
+ cv::Mat view_image,
+ const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+ const std::vector<std::vector<cv::Point>> &filtered_blobs,
+ const std::vector<BlobStats> &blob_stats) {
+ CHECK_GT(view_image.cols, 0);
+ if (unfiltered_blobs.size() > 0) {
+ // Draw blobs unfilled, with red color border
+ cv::drawContours(view_image, unfiltered_blobs, -1, cv::Scalar(0, 0, 255),
+ 0);
+ }
+
+ cv::drawContours(view_image, filtered_blobs, -1, cv::Scalar(0, 255, 0),
+ cv::FILLED);
+
+ // Draw blob centroids
+ for (auto stats : blob_stats) {
+ cv::circle(view_image, stats.centroid, 2, cv::Scalar(255, 0, 0),
+ cv::FILLED);
+ }
+}
+
+void BlobDetector::ExtractBlobs(
+ cv::Mat rgb_image, cv::Mat binarized_image, cv::Mat blob_image,
+ std::vector<std::vector<cv::Point>> &filtered_blobs,
+ std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+ std::vector<BlobStats> &blob_stats) {
+ binarized_image = ThresholdImage(rgb_image);
+ unfiltered_blobs = FindBlobs(binarized_image);
+ blob_stats = ComputeStats(unfiltered_blobs);
+ filtered_blobs = FilterBlobs(unfiltered_blobs, blob_stats);
+ DrawBlobs(blob_image, unfiltered_blobs, filtered_blobs, blob_stats);
+}
+
+} // namespace vision
+} // namespace y2022
diff --git a/y2022/vision/blob_detector.h b/y2022/vision/blob_detector.h
new file mode 100644
index 0000000..84e504d
--- /dev/null
+++ b/y2022/vision/blob_detector.h
@@ -0,0 +1,54 @@
+#ifndef Y2022_BLOB_DETECTOR_H_
+#define Y2022_BLOB_DETECTOR_H_
+
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+
+namespace y2022 {
+namespace vision {
+
+class BlobDetector {
+ public:
+ struct BlobStats {
+ cv::Point centroid;
+ double aspect_ratio;
+ double area;
+ size_t points;
+ };
+
+ BlobDetector() {}
+ // Given an image, threshold it to find "green" pixels
+ // Input: Color image
+ // Output: Grayscale (binarized) image with green pixels set to 255
+ static cv::Mat ThresholdImage(cv::Mat rgb_image);
+
+ // Given binary image, extract blobs
+ static std::vector<std::vector<cv::Point>> FindBlobs(cv::Mat threshold_image);
+
+ // Extract stats for each blob
+ static std::vector<BlobStats> ComputeStats(
+ std::vector<std::vector<cv::Point>> blobs);
+
+ // Filter blobs to get rid of noise, too large items, etc.
+ static std::vector<std::vector<cv::Point>> FilterBlobs(
+ std::vector<std::vector<cv::Point>> blobs,
+ std::vector<BlobStats> blob_stats);
+
+ // Draw Blobs on image
+ // Optionally draw all blobs and filtered blobs
+ static void DrawBlobs(
+ cv::Mat view_image,
+ const std::vector<std::vector<cv::Point>> &filtered_blobs,
+ const std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+ const std::vector<BlobStats> &blob_stats);
+
+ static void ExtractBlobs(
+ cv::Mat rgb_image, cv::Mat binarized_image, cv::Mat blob_image,
+ std::vector<std::vector<cv::Point>> &filtered_blobs,
+ std::vector<std::vector<cv::Point>> &unfiltered_blobs,
+ std::vector<BlobStats> &blob_stats);
+};
+} // namespace vision
+} // namespace y2022
+
+#endif // Y2022_BLOB_DETECTOR_H_
diff --git a/y2022/vision/camera_reader.cc b/y2022/vision/camera_reader.cc
new file mode 100644
index 0000000..f466c8b
--- /dev/null
+++ b/y2022/vision/camera_reader.cc
@@ -0,0 +1,85 @@
+#include "y2022/vision/camera_reader.h"
+
+#include <math.h>
+
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2022/vision/blob_detector.h"
+#include "y2022/vision/target_estimator.h"
+
+namespace y2022 {
+namespace vision {
+
+using namespace frc971::vision;
+
+const sift::CameraCalibration *CameraReader::FindCameraCalibration() const {
+ const std::string_view node_name = event_loop_->node()->name()->string_view();
+ const int team_number = aos::network::GetTeamNumber();
+ for (const sift::CameraCalibration *candidate :
+ *training_data_->camera_calibrations()) {
+ if (candidate->node_name()->string_view() != node_name) {
+ continue;
+ }
+ if (candidate->team_number() != team_number) {
+ continue;
+ }
+ return candidate;
+ }
+ LOG(FATAL) << ": Failed to find camera calibration for " << node_name
+ << " on " << team_number;
+}
+
+void CameraReader::ProcessImage(const CameraImage &image) {
+ // Remember, we're getting YUYV images, so we start by converting to RGB
+
+ // TOOD: Need to code this up for blob detection
+ cv::Mat image_mat(image.rows(), image.cols(), CV_8U);
+ CHECK(image_mat.isContinuous());
+ const int number_pixels = image.rows() * image.cols();
+ for (int i = 0; i < number_pixels; ++i) {
+ reinterpret_cast<uint8_t *>(image_mat.data)[i] =
+ image.data()->data()[i * 2];
+ }
+
+ // Now, send our two messages-- one large, with details for remote
+ // debugging(features), and one smaller
+ // TODO: Send debugging message as well
+ std::vector<std::vector<cv::Point> > filtered_blobs, unfiltered_blobs;
+ std::vector<BlobDetector::BlobStats> blob_stats;
+ cv::Mat binarized_image =
+ cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC1);
+ cv::Mat ret_image =
+ cv::Mat::zeros(cv::Size(image_mat.cols, image_mat.rows), CV_8UC3);
+ BlobDetector::ExtractBlobs(image_mat, binarized_image, ret_image,
+ filtered_blobs, unfiltered_blobs, blob_stats);
+ // TODO(milind): use actual centroid
+ TargetEstimateT target = TargetEstimator::EstimateTargetLocation(
+ blob_stats[0].centroid, CameraIntrinsics(), CameraExtrinsics());
+
+ auto builder = target_estimate_sender_.MakeBuilder();
+ builder.CheckOk(builder.Send(TargetEstimate::Pack(*builder.fbb(), &target)));
+}
+
+void CameraReader::ReadImage() {
+ if (!reader_->ReadLatestImage()) {
+ read_image_timer_->Setup(event_loop_->monotonic_now() +
+ std::chrono::milliseconds(10));
+ return;
+ }
+
+ ProcessImage(reader_->LatestImage());
+
+ reader_->SendLatestImage();
+ read_image_timer_->Setup(event_loop_->monotonic_now());
+}
+
+} // namespace vision
+} // namespace y2022
diff --git a/y2022/vision/camera_reader.h b/y2022/vision/camera_reader.h
new file mode 100644
index 0000000..6880248
--- /dev/null
+++ b/y2022/vision/camera_reader.h
@@ -0,0 +1,92 @@
+#ifndef Y2022_VISION_CAMERA_READER_H_
+#define Y2022_VISION_CAMERA_READER_H_
+
+#include <math.h>
+
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/imgproc.hpp>
+
+#include "aos/events/event_loop.h"
+#include "aos/flatbuffer_merge.h"
+#include "aos/network/team_number.h"
+#include "frc971/vision/v4l2_reader.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2020/vision/sift/sift_generated.h"
+#include "y2020/vision/sift/sift_training_generated.h"
+#include "y2020/vision/tools/python_code/sift_training_data.h"
+#include "y2022/vision/target_estimate_generated.h"
+
+namespace y2022 {
+namespace vision {
+
+using namespace frc971::vision;
+
+// TODO<Jim/Milind>: Need to add in senders to send out the blob data/stats
+
+class CameraReader {
+ public:
+ CameraReader(aos::EventLoop *event_loop,
+ const sift::TrainingData *training_data, V4L2Reader *reader)
+ : event_loop_(event_loop),
+ training_data_(training_data),
+ camera_calibration_(FindCameraCalibration()),
+ reader_(reader),
+ image_sender_(event_loop->MakeSender<CameraImage>("/camera")),
+ target_estimate_sender_(
+ event_loop->MakeSender<TargetEstimate>("/camera")),
+ read_image_timer_(event_loop->AddTimer([this]() { ReadImage(); })) {
+ event_loop->OnRun(
+ [this]() { read_image_timer_->Setup(event_loop_->monotonic_now()); });
+ }
+
+ private:
+ const sift::CameraCalibration *FindCameraCalibration() const;
+
+ // Processes an image (including sending the results).
+ void ProcessImage(const CameraImage &image);
+
+ // Reads an image, and then performs all of our processing on it.
+ void ReadImage();
+
+ cv::Mat CameraIntrinsics() const {
+ const cv::Mat result(3, 3, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration_->intrinsics()->data())));
+ CHECK_EQ(result.total(), camera_calibration_->intrinsics()->size());
+ return result;
+ }
+
+ cv::Mat CameraExtrinsics() const {
+ const cv::Mat result(
+ 4, 4, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration_->fixed_extrinsics()->data()->data())));
+ CHECK_EQ(result.total(),
+ camera_calibration_->fixed_extrinsics()->data()->size());
+ return result;
+ }
+
+ cv::Mat CameraDistCoeffs() const {
+ const cv::Mat result(5, 1, CV_32F,
+ const_cast<void *>(static_cast<const void *>(
+ camera_calibration_->dist_coeffs()->data())));
+ CHECK_EQ(result.total(), camera_calibration_->dist_coeffs()->size());
+ return result;
+ }
+
+ aos::EventLoop *const event_loop_;
+ const sift::TrainingData *const training_data_;
+ const sift::CameraCalibration *const camera_calibration_;
+ V4L2Reader *const reader_;
+ aos::Sender<CameraImage> image_sender_;
+ aos::Sender<TargetEstimate> target_estimate_sender_;
+
+ // We schedule this immediately to read an image. Having it on a timer
+ // means other things can run on the event loop in between.
+ aos::TimerHandler *const read_image_timer_;
+};
+
+} // namespace vision
+} // namespace y2022
+#endif // Y2022_VISION_CAMERA_READER_H_
diff --git a/y2022/vision/camera_reader_main.cc b/y2022/vision/camera_reader_main.cc
new file mode 100644
index 0000000..6f65a6d
--- /dev/null
+++ b/y2022/vision/camera_reader_main.cc
@@ -0,0 +1,51 @@
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "y2022/vision/camera_reader.h"
+
+// config used to allow running camera_reader independently. E.g.,
+// bazel run //y2022/vision:camera_reader -- --config y2022/config.json
+// --override_hostname pi-7971-1 --ignore_timestamps true
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_uint32(exposure, 5, "Exposure time, in 100us increments");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+using namespace frc971::vision;
+
+void CameraReaderMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ const aos::FlatbufferSpan<sift::TrainingData> training_data(
+ SiftTrainingData());
+ CHECK(training_data.Verify());
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ // First, log the data for future reference.
+ {
+ aos::Sender<sift::TrainingData> training_data_sender =
+ event_loop.MakeSender<sift::TrainingData>("/camera");
+ CHECK_EQ(training_data_sender.Send(training_data),
+ aos::RawSender::Error::kOk);
+ }
+
+ V4L2Reader v4l2_reader(&event_loop, "/dev/video0");
+ v4l2_reader.SetExposure(FLAGS_exposure);
+
+ CameraReader camera_reader(&event_loop, &training_data.message(),
+ &v4l2_reader);
+
+ event_loop.Run();
+}
+
+} // namespace
+} // namespace vision
+} // namespace y2022
+
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ y2022::vision::CameraReaderMain();
+}
diff --git a/y2022/vision/target_estimate.fbs b/y2022/vision/target_estimate.fbs
new file mode 100644
index 0000000..e22654a
--- /dev/null
+++ b/y2022/vision/target_estimate.fbs
@@ -0,0 +1,14 @@
+namespace y2022.vision;
+
+// Contains the information the EKF wants from blobs from a single image.
+table TargetEstimate {
+ // Horizontal distance from the camera to the center of the upper hub
+ distance:double (id: 0);
+ // Angle from the camera to the target (horizontal angle in rad).
+ // Positive means right of center, negative means left.
+ angle_to_target:double (id: 1);
+
+ // TODO(milind): add confidence and blob stats
+}
+
+root_type TargetEstimate;
diff --git a/y2022/vision/target_estimator.cc b/y2022/vision/target_estimator.cc
new file mode 100644
index 0000000..c51eb08
--- /dev/null
+++ b/y2022/vision/target_estimator.cc
@@ -0,0 +1,31 @@
+#include "y2022/vision/target_estimator.h"
+
+namespace y2022::vision {
+
+TargetEstimateT TargetEstimator::EstimateTargetLocation(
+ cv::Point2i blob_point, const cv::Mat &intrinsics,
+ const cv::Mat &extrinsics) {
+ const cv::Point2d focal_length(intrinsics.at<double>(0, 0),
+ intrinsics.at<double>(1, 1));
+ const cv::Point2d offset(intrinsics.at<double>(0, 2),
+ intrinsics.at<double>(1, 2));
+
+ // Blob pitch in camera reference frame
+ const double blob_pitch =
+ std::atan(static_cast<double>(-(blob_point.y - offset.y)) /
+ static_cast<double>(focal_length.y));
+ const double camera_height = extrinsics.at<double>(2, 3);
+ // Depth from camera to blob
+ const double depth = (kTapeHeight - camera_height) / std::tan(blob_pitch);
+
+ TargetEstimateT target;
+ target.angle_to_target =
+ std::atan2(static_cast<double>(blob_point.x - offset.x),
+ static_cast<double>(focal_length.x));
+ target.distance =
+ (depth / std::cos(target.angle_to_target)) + kUpperHubRadius;
+
+ return target;
+}
+
+} // namespace y2022::vision
diff --git a/y2022/vision/target_estimator.h b/y2022/vision/target_estimator.h
new file mode 100644
index 0000000..4988c3c
--- /dev/null
+++ b/y2022/vision/target_estimator.h
@@ -0,0 +1,26 @@
+#ifndef Y2022_VISION_POSE_ESTIMATOR_H_
+#define Y2022_VISION_POSE_ESTIMATOR_H_
+
+#include "opencv2/imgproc.hpp"
+#include "y2022/vision/target_estimate_generated.h"
+
+namespace y2022::vision {
+
+class TargetEstimator {
+ public:
+ // Computes the location of the target.
+ // blob_point is the mean (x, y) of blob pixels.
+ static TargetEstimateT EstimateTargetLocation(cv::Point2i blob_point,
+ const cv::Mat &intrinsics,
+ const cv::Mat &extrinsics);
+
+ private:
+ // Height of the center of the tape (m)
+ static constexpr double kTapeHeight = 2.58 + (0.05 / 2);
+ // Horizontal distance from tape to center of hub (m)
+ static constexpr double kUpperHubRadius = 1.22 / 2;
+};
+
+} // namespace y2022::vision
+
+#endif // Y2022_VISION_POSE_ESTIMATOR_H_
diff --git a/y2022/vision/viewer.cc b/y2022/vision/viewer.cc
new file mode 100644
index 0000000..8ecb36d
--- /dev/null
+++ b/y2022/vision/viewer.cc
@@ -0,0 +1,144 @@
+#include <map>
+#include <opencv2/calib3d.hpp>
+#include <opencv2/features2d.hpp>
+#include <opencv2/highgui/highgui.hpp>
+#include <opencv2/imgproc.hpp>
+#include <random>
+
+#include "aos/events/shm_event_loop.h"
+#include "aos/init.h"
+#include "aos/time/time.h"
+#include "frc971/vision/vision_generated.h"
+#include "y2022/vision/blob_detector.h"
+
+DEFINE_string(capture, "",
+ "If set, capture a single image and save it to this filename.");
+DEFINE_string(channel, "/camera", "Channel name for the image.");
+DEFINE_string(config, "config.json", "Path to the config file to use.");
+DEFINE_string(png_dir, "LED_Ring_exp", "Path to a set of images to display.");
+DEFINE_bool(show_features, true, "Show the blobs.");
+
+namespace y2022 {
+namespace vision {
+namespace {
+
+aos::Fetcher<frc971::vision::CameraImage> image_fetcher;
+
+bool DisplayLoop() {
+ int64_t image_timestamp = 0;
+ const frc971::vision::CameraImage *image;
+ // Read next image
+ if (!image_fetcher.FetchNext()) {
+ VLOG(2) << "Couldn't fetch next image";
+ return true;
+ }
+
+ image = image_fetcher.get();
+ CHECK(image != nullptr) << "Couldn't read image";
+ image_timestamp = image->monotonic_timestamp_ns();
+ VLOG(2) << "Got image at timestamp: " << image_timestamp;
+
+ // Create color image:
+ cv::Mat image_color_mat(cv::Size(image->cols(), image->rows()), CV_8UC2,
+ (void *)image->data()->data());
+ cv::Mat rgb_image(cv::Size(image->cols(), image->rows()), CV_8UC3);
+ cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+
+ if (!FLAGS_capture.empty()) {
+ cv::imwrite(FLAGS_capture, rgb_image);
+ return false;
+ }
+
+ cv::Mat binarized_image, ret_image;
+ std::vector<std::vector<cv::Point>> unfiltered_blobs, filtered_blobs;
+ std::vector<BlobDetector::BlobStats> blob_stats;
+ BlobDetector::ExtractBlobs(rgb_image, binarized_image, ret_image,
+ filtered_blobs, unfiltered_blobs, blob_stats);
+
+ LOG(INFO) << image->monotonic_timestamp_ns()
+ << ": # blobs: " << filtered_blobs.size();
+
+ // Downsize for viewing
+ cv::resize(rgb_image, rgb_image,
+ cv::Size(rgb_image.cols / 2, rgb_image.rows / 2),
+ cv::INTER_LINEAR);
+
+ cv::imshow("image", rgb_image);
+ cv::imshow("blobs", ret_image);
+
+ int keystroke = cv::waitKey(1);
+ if ((keystroke & 0xFF) == static_cast<int>('c')) {
+ // Convert again, to get clean image
+ cv::cvtColor(image_color_mat, rgb_image, cv::COLOR_YUV2BGR_YUYV);
+ std::stringstream name;
+ name << "capture-" << aos::realtime_clock::now() << ".png";
+ cv::imwrite(name.str(), rgb_image);
+ LOG(INFO) << "Saved image file: " << name.str();
+ } else if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ return false;
+ }
+ return true;
+}
+
+void ViewerMain() {
+ aos::FlatbufferDetachedBuffer<aos::Configuration> config =
+ aos::configuration::ReadConfig(FLAGS_config);
+
+ aos::ShmEventLoop event_loop(&config.message());
+
+ image_fetcher =
+ event_loop.MakeFetcher<frc971::vision::CameraImage>(FLAGS_channel);
+
+ // Run the display loop
+ event_loop.AddPhasedLoop(
+ [&event_loop](int) {
+ if (!DisplayLoop()) {
+ LOG(INFO) << "Calling event_loop Exit";
+ event_loop.Exit();
+ };
+ },
+ ::std::chrono::milliseconds(100));
+
+ event_loop.Run();
+
+ image_fetcher = aos::Fetcher<frc971::vision::CameraImage>();
+}
+
+void ViewerLocal() {
+ std::vector<cv::String> file_list;
+ cv::glob(FLAGS_png_dir + "/*.png", file_list, false);
+ for (auto file : file_list) {
+ LOG(INFO) << "Reading file " << file;
+ cv::Mat rgb_image = cv::imread(file.c_str());
+ std::vector<std::vector<cv::Point>> filtered_blobs, unfiltered_blobs;
+ std::vector<BlobDetector::BlobStats> blob_stats;
+ cv::Mat binarized_image =
+ cv::Mat::zeros(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC1);
+ cv::Mat ret_image =
+ cv::Mat::zeros(cv::Size(rgb_image.cols, rgb_image.rows), CV_8UC3);
+ BlobDetector::ExtractBlobs(rgb_image, binarized_image, ret_image,
+ filtered_blobs, unfiltered_blobs, blob_stats);
+
+ LOG(INFO) << ": # blobs: " << filtered_blobs.size() << " (# removed: "
+ << unfiltered_blobs.size() - filtered_blobs.size() << ")";
+ cv::imshow("image", rgb_image);
+ cv::imshow("blobs", ret_image);
+
+ int keystroke = cv::waitKey(0);
+ if ((keystroke & 0xFF) == static_cast<int>('q')) {
+ return;
+ }
+ }
+}
+} // namespace
+} // namespace vision
+} // namespace y2022
+
+// Quick and lightweight viewer for images
+int main(int argc, char **argv) {
+ aos::InitGoogle(&argc, &argv);
+ if (FLAGS_png_dir != "")
+ y2022::vision::ViewerLocal();
+ else
+ y2022::vision::ViewerMain();
+}
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 5f43b70..f4cbac8 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -193,17 +193,33 @@
: ::frc971::wpilib::LoopOutputHandler<superstructure::Output>(
event_loop, "/superstructure") {}
+ void set_climber_falcon(
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX> t) {
+ climber_falcon_ = ::std::move(t);
+ climber_falcon_->ConfigSupplyCurrentLimit(
+ {true, Values::kClimberSupplyCurrentLimit(),
+ Values::kClimberSupplyCurrentLimit(), 0});
+ }
+
private:
- void WriteToFalcon(const double voltage,
- ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
+ void WriteToFalconCan(const double voltage,
+ ::ctre::phoenix::motorcontrol::can::TalonFX *falcon) {
falcon->Set(
ctre::phoenix::motorcontrol::ControlMode::PercentOutput,
std::clamp(voltage, -kMaxBringupPower, kMaxBringupPower) / 12.0);
}
- void Write(const superstructure::Output & /* output */) override {}
+ void Write(const superstructure::Output &output) override {
+ WriteToFalconCan(output.climber_voltage(), climber_falcon_.get());
+ }
- void Stop() override { AOS_LOG(WARNING, "Superstructure output too old.\n"); }
+ void Stop() override {
+ AOS_LOG(WARNING, "Superstructure output too old.\n");
+ climber_falcon_->Set(ctre::phoenix::motorcontrol::ControlMode::Disabled, 0);
+ }
+
+ ::std::unique_ptr<::ctre::phoenix::motorcontrol::can::TalonFX>
+ climber_falcon_;
};
class WPILibRobot : public ::frc971::wpilib::WPILibRobotBase {
@@ -246,6 +262,9 @@
SuperstructureWriter superstructure_writer(&output_event_loop);
+ superstructure_writer.set_climber_falcon(
+ make_unique<::ctre::phoenix::motorcontrol::can::TalonFX>(0));
+
AddLoop(&output_event_loop);
RunLoops();
diff --git a/y2022/y2022.json b/y2022/y2022.json
index 4abbd5e..010c675 100644
--- a/y2022/y2022.json
+++ b/y2022/y2022.json
@@ -1,39 +1,23 @@
{
- "channels":
- [
+ "channel_storage_duration": 2000000000,
+ "maps": [
{
- "name": "/superstructure",
- "type": "y2022.control_loops.superstructure.Goal",
- "frequency": 200
- },
- {
- "name": "/superstructure",
- "type": "y2022.control_loops.superstructure.Status",
- "frequency": 200
- },
- {
- "name": "/superstructure",
- "type": "y2022.control_loops.superstructure.Output",
- "frequency": 200
- },
- {
- "name": "/superstructure",
- "type": "y2022.control_loops.superstructure.Position",
- "frequency": 200
- }
- ],
- "applications": [
- {
- "name": "drivetrain"
- },
- {
- "name": "superstructure"
+ "match": {
+ "name": "/aos",
+ "type": "aos.RobotState"
+ },
+ "rename": {
+ "name": "/roborio/aos"
+ }
}
],
"imports": [
- "../frc971/input/robot_state_config.json",
- "../frc971/control_loops/drivetrain/drivetrain_config.json",
- "../frc971/autonomous/autonomous_config.json",
- "../frc971/wpilib/wpilib_config.json"
+ "y2022_roborio.json",
+ "y2022_pi1.json",
+ "y2022_pi2.json",
+ "y2022_pi3.json",
+ "y2022_pi4.json",
+ "y2022_pi5.json",
+ "y2022_logger.json"
]
}
diff --git a/y2022/y2022_pi_template.json b/y2022/y2022_pi_template.json
index 81b96d2..4994a36 100644
--- a/y2022/y2022_pi_template.json
+++ b/y2022/y2022_pi_template.json
@@ -99,6 +99,13 @@
"num_senders": 18
},
{
+ "name": "/pi{{ NUM }}/camera",
+ "type": "y2022.vision.TargetEstimate",
+ "source_node": "pi{{ NUM }}",
+ "frequency": 25,
+ "num_senders": 2
+ },
+ {
"name": "/logger/aos",
"type": "aos.starter.StarterRpc",
"source_node": "logger",