Merge "Start y2022 imu_main automatically"
diff --git a/aos/starter/starter.sh b/aos/starter/starter.sh
index d5812c2..9bc7605 100755
--- a/aos/starter/starter.sh
+++ b/aos/starter/starter.sh
@@ -1,62 +1,22 @@
#!/bin/bash
+set -x
+
if [[ "$(hostname)" == "roboRIO"* ]]; then
/usr/local/natinst/etc/init.d/systemWebServer stop
- ROBOT_CODE="/home/admin/robot_code"
-
- # Get the CTRE libraries in the shared library search path
- for f in $(ls *.so);
- do
- ln -f -s /home/admin/robot_code/$f /usr/local/frc/third-party/lib/$f
- done
+ ROBOT_CODE="/home/admin/bin"
+ cd "${ROBOT_CODE}"
ln -s /var/local/natinst/log/FRC_UserProgram.log /tmp/FRC_UserProgram.log
ln -s /var/local/natinst/log/FRC_UserProgram.log "${ROBOT_CODE}/FRC_UserProgram.log"
elif [[ "$(hostname)" == "pi-"* ]]; then
- function chrtirq() {
- ps -ef | grep "\\[$1\\]" | awk '{print $2}' | xargs chrt $2 -p $3
- }
-
- chrtirq "irq/20-fe00b880" -f 50
- chrtirq "irq/66-xhci_hcd" -f 1
- chrtirq "irq/50-VCHIQ do" -o 0
- chrtirq "irq/27-DMA IRQ" -f 50
- chrtirq "irq/51-mmc1" -o 0
- chrtirq "irq/51-mmc0" -o 0
- chrtirq "irq/51-s-mmc0" -o 0
- chrtirq "irq/64-v3d" -o 0
- chrtirq "irq/24-vc4 hvs" -o 0
- chrtirq "irq/42-vc4 hdmi" -o 0
- chrtirq "irq/43-vc4 hdmi" -o 0
- chrtirq "irq/39-vc4 hdmi" -o 0
- chrtirq "irq/39-s-vc4 hd" -o 0
- chrtirq "irq/38-vc4 hdmi" -o 0
- chrtirq "irq/38-s-vc4 hd" -o 0
- chrtirq "irq/29-DMA IRQ" -f 50
- chrtirq "irq/48-vc4 hdmi" -o 0
- chrtirq "irq/49-vc4 hdmi" -o 0
- chrtirq "irq/45-vc4 hdmi" -o 0
- chrtirq "irq/45-s-vc4 hd" -o 0
- chrtirq "irq/44-vc4 hdmi" -o 0
- chrtirq "irq/44-s-vc4 hd" -o 0
- chrtirq "irq/30-DMA IRQ" -f 50
- chrtirq "irq/19-fe004000" -f 50
- chrtirq "irq/34-vc4 crtc" -o 0
- chrtirq "irq/35-vc4 crtc" -o 0
- chrtirq "irq/36-vc4 crtc" -o 0
- chrtirq "irq/35-vc4 crtc" -o 0
- chrtirq "irq/37-vc4 crtc" -o 0
- chrtirq "irq/23-uart-pl0" -o 0
- chrtirq "irq/57-eth0" -f 10
- chrtirq "irq/58-eth0" -f 10
-
# We have systemd configured to handle restarting, so just exec.
- export PATH="${PATH}:/home/pi/robot_code"
+ export PATH="${PATH}:/home/pi/bin"
rm -rf /dev/shm/aos
exec starterd
else
- ROBOT_CODE="${HOME}/robot_code"
+ ROBOT_CODE="${HOME}/bin"
fi
cd "${ROBOT_CODE}"
diff --git a/frc971/config/robotCommand b/frc971/config/robotCommand
index 7b726a7..cb1d6f1 100755
--- a/frc971/config/robotCommand
+++ b/frc971/config/robotCommand
@@ -1 +1 @@
-/home/admin/robot_code/starter.sh
+/home/admin/bin/starter.sh
diff --git a/frc971/config/setup_roborio.sh b/frc971/config/setup_roborio.sh
index f00212f..3434482 100755
--- a/frc971/config/setup_roborio.sh
+++ b/frc971/config/setup_roborio.sh
@@ -30,11 +30,11 @@
ssh "admin@${ROBOT_HOSTNAME}" 'echo "alias l=\"ls -la\"" >> /etc/profile'
echo "Adding symbolic link to loging directory"
ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1 logs
- ssh "admin@${ROBOT_HOSTNAME}" mkdir robot_code
- ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current robot_code/aos_log-current
+ ssh "admin@${ROBOT_HOSTNAME}" mkdir bin
+ ssh "admin@${ROBOT_HOSTNAME}" ln -s /media/sda1/aos_log-current bin/aos_log-current
echo "Adding aos_dump autocomplete to profile"
- ssh "admin@${ROBOT_HOSTNAME}" 'echo "if [ -f /home/admin/robot_code/aos_dump_autocomplete.sh ]; then source /home/admin/robot_code/aos_dump_autocomplete.sh; fi;" >> /etc/profile'
- ssh "admin@${ROBOT_HOSTNAME}" 'echo "export PATH=\"\${PATH}:/home/admin/robot_code:/home/admin/bin\"" >> /etc/profile'
+ ssh "admin@${ROBOT_HOSTNAME}" 'echo "if [ -f /home/admin/bin/aos_dump_autocomplete.sh ]; then source /home/admin/bin/aos_dump_autocomplete.sh; fi;" >> /etc/profile'
+ ssh "admin@${ROBOT_HOSTNAME}" 'echo "export PATH=\"\${PATH}:/home/admin/bin\"" >> /etc/profile'
fi
ssh "admin@${ROBOT_HOSTNAME}" "sed -i 's/vm\.overcommit_memory=2/vm\.overcommit_memory=0/' /etc/sysctl.conf"
diff --git a/frc971/downloader.bzl b/frc971/downloader.bzl
index aebcb87..ec93f5b 100644
--- a/frc971/downloader.bzl
+++ b/frc971/downloader.bzl
@@ -25,7 +25,7 @@
] if target_type == "roborio" else []) + start_binaries,
srcs = [
"//aos:prime_binaries",
- ] + binaries + data,
+ ] + binaries + data + ["//frc971/raspi/rootfs:chrt.sh"],
dirs = dirs,
target_type = target_type,
default_target = default_target,
@@ -40,7 +40,7 @@
[expand_label(binary) + ".stripped" for binary in start_binaries],
srcs = [
"//aos:prime_binaries_stripped",
- ] + [expand_label(binary) + ".stripped" for binary in binaries] + data,
+ ] + [expand_label(binary) + ".stripped" for binary in binaries] + data + ["//frc971/raspi/rootfs:chrt.sh"],
dirs = dirs,
target_type = target_type,
default_target = default_target,
diff --git a/frc971/downloader/downloader.py b/frc971/downloader/downloader.py
index dc14df1..4314845 100644
--- a/frc971/downloader/downloader.py
+++ b/frc971/downloader/downloader.py
@@ -69,7 +69,7 @@
user = "pi"
elif args.type == "roborio":
user = "admin"
- target_dir = "/home/" + user + "/robot_code"
+ target_dir = "/home/" + user + "/bin"
ssh_target = "%s@%s" % (user, hostname)
diff --git a/frc971/raspi/rootfs/BUILD b/frc971/raspi/rootfs/BUILD
new file mode 100644
index 0000000..ee5984a
--- /dev/null
+++ b/frc971/raspi/rootfs/BUILD
@@ -0,0 +1 @@
+exports_files(["chrt.sh"])
diff --git a/frc971/raspi/rootfs/chrt.sh b/frc971/raspi/rootfs/chrt.sh
new file mode 100755
index 0000000..535f4c6
--- /dev/null
+++ b/frc971/raspi/rootfs/chrt.sh
@@ -0,0 +1,37 @@
+#!/bin/bash
+
+set -e
+
+function chrtirq() {
+ PIDS="$(ps -ef | grep "\\[$1\\]" | awk '{print $2}')"
+
+ for PID in $PIDS; do
+ chrt $2 -p $3 "${PID}"
+
+ ps -q "${PID}" -o comm= | tr -d '[:space:]'
+ echo -n " "
+ chrt -p "${PID}"
+ done
+
+ if [ -z "${PID}" ]; then
+ echo "No such IRQ ${1}"
+ fi
+}
+
+chrtirq "irq/[0-9]*-fe00b880" -f 50
+chrtirq "irq/[0-9]*-fe204000" -f 60
+chrtirq "irq/[0-9]*-adis1650" -f 61
+chrtirq "irq/[0-9]*-xhci_hcd" -f 1
+chrtirq "irq/[0-9]*-VCHIQ do" -o 0
+chrtirq "irq/[0-9]*-DMA IRQ" -f 50
+chrtirq "irq/[0-9]*-mmc1" -o 0
+chrtirq "irq/[0-9]*-mmc0" -o 0
+chrtirq "irq/[0-9]*-s-mmc0" -o 0
+chrtirq "irq/[0-9]*-v3d" -o 0
+chrtirq "irq/24-vc4 hvs" -o 0
+chrtirq "irq/[0-9]*-vc4 hdmi" -o 0
+chrtirq "irq/[0-9]*-s-vc4 hd" -o 0
+chrtirq "irq/19-fe004000" -f 50
+chrtirq "irq/[0-9]*-vc4 crtc" -o 0
+chrtirq "irq/23-uart-pl0" -o 0
+chrtirq "irq/[0-9]*-eth0" -f 10
diff --git a/frc971/raspi/rootfs/enable_imu.sh b/frc971/raspi/rootfs/enable_imu.sh
new file mode 100755
index 0000000..3725207
--- /dev/null
+++ b/frc971/raspi/rootfs/enable_imu.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+
+CONFIG=/boot/config.txt
+
+if grep -q adis16505 "${CONFIG}"; then
+ echo "Already enabled"
+ exit 0;
+fi
+
+sed -i '1h;1!H;$!d;x;s/.*dtparam=spi[^\n]*/&\n\n# Enable the IMU\ndtoverlay=adis16505/' "${CONFIG}"
+
+echo "Enabled 16505"
diff --git a/frc971/raspi/rootfs/frc971.service b/frc971/raspi/rootfs/frc971.service
index cdfb347..be6b37f 100644
--- a/frc971/raspi/rootfs/frc971.service
+++ b/frc971/raspi/rootfs/frc971.service
@@ -7,8 +7,8 @@
User=pi
Group=pi
Type=simple
-WorkingDirectory=/home/pi/robot_code
-ExecStart=/home/pi/robot_code/starter.sh
+WorkingDirectory=/home/pi/bin
+ExecStart=/home/pi/bin/starter.sh
KillMode=mixed
TimeoutStopSec=10
LimitRTPRIO=60
diff --git a/frc971/raspi/rootfs/frc971chrt.service b/frc971/raspi/rootfs/frc971chrt.service
index cc68934..6086fa0 100644
--- a/frc971/raspi/rootfs/frc971chrt.service
+++ b/frc971/raspi/rootfs/frc971chrt.service
@@ -3,7 +3,7 @@
[Service]
Type=oneshot
-ExecStart=/home/pi/robot_code/chrt.sh
+ExecStart=/home/pi/bin/chrt.sh
[Install]
WantedBy=multi-user.target
diff --git a/frc971/raspi/rootfs/modify_rootfs.sh b/frc971/raspi/rootfs/modify_rootfs.sh
index fd9bec9..15bdd80 100755
--- a/frc971/raspi/rootfs/modify_rootfs.sh
+++ b/frc971/raspi/rootfs/modify_rootfs.sh
@@ -36,8 +36,13 @@
if ! grep "gpu_mem=128" "${BOOT_PARTITION}/config.txt"; then
echo "gpu_mem=128" | sudo tee -a "${BOOT_PARTITION}/config.txt"
fi
+if ! grep "enable_uart=1" "${BOOT_PARTITION}/config.txt"; then
+ echo "enable_uart=1" | 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"
+# Enable SPI.
+sudo sed -i s/^.*dtparam=spi=on/dtparam=spi=on/ "${BOOT_PARTITION}/config.txt"
sudo tar -zxvf "${KERNEL}" --strip-components 2 -C ${BOOT_PARTITION}/ ./fat32
@@ -80,11 +85,17 @@
sudo mount -o loop,offset=${OFFSET} "${IMAGE}" "${PARTITION}"
fi
+if [[ ! -e wiringpi-2.70-1.deb ]]; then
+ wget --continue https://software.frc971.org/Build-Dependencies/wiringpi-2.70-1.deb
+fi
+
sudo cp target_configure.sh "${PARTITION}/tmp/"
+sudo cp wiringpi-2.70-1.deb "${PARTITION}/tmp/"
sudo cp dhcpcd.conf "${PARTITION}/tmp/dhcpcd.conf"
sudo cp sctp.conf "${PARTITION}/etc/sysctl.d/sctp.conf"
sudo cp logind.conf "${PARTITION}/etc/systemd/logind.conf"
sudo cp change_hostname.sh "${PARTITION}/tmp/change_hostname.sh"
+sudo cp enable_imu.sh "${PARTITION}/tmp/"
sudo cp frc971.service "${PARTITION}/etc/systemd/system/frc971.service"
sudo cp frc971chrt.service "${PARTITION}/etc/systemd/system/frc971chrt.service"
sudo cp rt.conf "${PARTITION}/etc/security/limits.d/rt.conf"
@@ -96,6 +107,8 @@
sudo rm -rf "${PARTITION}/lib/modules/"*
sudo tar -zxvf "${KERNEL}" --strip-components 4 -C "${PARTITION}/lib/modules/" ./ext4/lib/modules/
+sudo cp adis16505.ko "${PARTITION}/lib/modules/5.10.78-rt55-v8+/kernel/"
+target /usr/sbin/depmod 5.10.78-rt55-v8+
# Downloads and installs our target libraries
target /bin/bash /tmp/target_configure.sh
diff --git a/frc971/raspi/rootfs/target_configure.sh b/frc971/raspi/rootfs/target_configure.sh
index 6d9171e..388d291 100755
--- a/frc971/raspi/rootfs/target_configure.sh
+++ b/frc971/raspi/rootfs/target_configure.sh
@@ -9,9 +9,12 @@
# And provide a script to change it.
cp /tmp/change_hostname.sh /root/bin/
+cp /tmp/enable_imu.sh /root/bin/
chmod a+x /root/bin/change_hostname.sh
+chmod a+x /root/bin/enable_imu.sh
chown -R pi.pi /home/pi/.ssh
+chown -R pi.pi /home/pi/bin
apt-get update
@@ -41,13 +44,7 @@
libnice-dev \
feh
-# Install WiringPi gpio for PWM control
-if [[ ! -e "/usr/bin/gpio" ]]; then
- cd /tmp
- git clone https://github.com/WiringPi/WiringPi.git
- cd WiringPi
- ./build
-fi
+dpkg -i /tmp/wiringpi-2.70-1.deb
echo 'GOVERNOR="performance"' > /etc/default/cpufrequtils
diff --git a/third_party/BUILD b/third_party/BUILD
index cff81fd..8dbb529 100644
--- a/third_party/BUILD
+++ b/third_party/BUILD
@@ -24,6 +24,10 @@
cc_library(
name = "phoenix",
+ linkopts = [
+ "-Wl,-rpath",
+ "-Wl,.",
+ ],
target_compatible_with = ["//tools/platforms/hardware:roborio"],
visibility = ["//visibility:public"],
deps = [
diff --git a/third_party/gperftools/BUILD b/third_party/gperftools/BUILD
index 2022ee6..14ac821 100644
--- a/third_party/gperftools/BUILD
+++ b/third_party/gperftools/BUILD
@@ -32,6 +32,8 @@
"-Wno-switch-enum",
"-Wno-error=cast-align",
"-Wno-error=cast-qual",
+ "-Wno-deprecated-volatile",
+ "-Wno-cast-qual",
# //build_tests:tcmalloc_build_test relies on this.
"-DENABLE_LARGE_ALLOC_REPORT=1",
diff --git a/y2016/dashboard/dashboard.cc b/y2016/dashboard/dashboard.cc
index d01243f..a7cc821 100644
--- a/y2016/dashboard/dashboard.cc
+++ b/y2016/dashboard/dashboard.cc
@@ -301,7 +301,7 @@
server.serve("www", 1180);
#else
// Absolute directory of www folder on the robot.
- server.serve("/home/admin/robot_code/www", 1180);
+ server.serve("/home/admin/bin/www", 1180);
#endif
socket_handler.Quit();
diff --git a/y2019/vision/server/server.cc b/y2019/vision/server/server.cc
index 7334ab4..817da17 100644
--- a/y2019/vision/server/server.cc
+++ b/y2019/vision/server/server.cc
@@ -300,13 +300,13 @@
bool serve_www = false;
{
struct stat result;
- if (stat("/home/admin/robot_code/www", &result) == 0) {
+ if (stat("/home/admin/bin/www", &result) == 0) {
serve_www = true;
}
}
server.serve(
- serve_www ? "/home/admin/robot_code/www" : "y2019/vision/server/www",
+ serve_www ? "/home/admin/bin/www" : "y2019/vision/server/www",
1180);
return 0;
diff --git a/y2020/vision/galactic_search_path.py b/y2020/vision/galactic_search_path.py
index 5e8ef54..00b572a 100755
--- a/y2020/vision/galactic_search_path.py
+++ b/y2020/vision/galactic_search_path.py
@@ -77,8 +77,8 @@
AOS_SEND_PATH = "bazel-bin/aos/aos_send"
def setup_if_pi():
- if os.path.isdir("/home/pi/robot_code"):
- AOS_SEND_PATH = "/home/pi/robot_code/aos_send.stripped"
+ if os.path.isdir("/home/pi/bin"):
+ AOS_SEND_PATH = "/home/pi/bin/aos_send.stripped"
os.system("./starter_cmd stop camera_reader")
setup_if_pi()
diff --git a/y2022/constants.h b/y2022/constants.h
index c0c49e7..160da1e 100644
--- a/y2022/constants.h
+++ b/y2022/constants.h
@@ -23,9 +23,7 @@
static constexpr double kDrivetrainEncoderCountsPerRevolution() {
return kDrivetrainCyclesPerRevolution() * 4;
}
- static constexpr double kDrivetrainEncoderRatio() {
- return (14.0 / 54.0) * (22.0 / 56.0);
- }
+ static constexpr double kDrivetrainEncoderRatio() { return 1.0; }
static constexpr double kMaxDrivetrainEncoderPulsesPerSecond() {
return control_loops::drivetrain::kFreeSpeed / (2.0 * M_PI) *
control_loops::drivetrain::kHighOutputRatio /
diff --git a/y2022/control_loops/superstructure/superstructure_position.fbs b/y2022/control_loops/superstructure/superstructure_position.fbs
index cd0bc67..8de272b 100644
--- a/y2022/control_loops/superstructure/superstructure_position.fbs
+++ b/y2022/control_loops/superstructure/superstructure_position.fbs
@@ -13,6 +13,12 @@
// Zero is straight and positive is open
flipper_arm_left:frc971.RelativePosition (id: 4);
flipper_arm_right:frc971.RelativePosition (id: 5);
+
+
+ // True is broken
+ intake_beambreak_front:bool (id:6);
+ intake_beambreak_back:bool (id:7);
+ turret_beambreak:bool (id:8);
}
root_type Position;
diff --git a/y2022/localizer/imu.cc b/y2022/localizer/imu.cc
index eb76e00..fed9ceb 100644
--- a/y2022/localizer/imu.cc
+++ b/y2022/localizer/imu.cc
@@ -20,6 +20,8 @@
imu_sender_(
event_loop_->MakeSender<frc971::IMUValuesBatch>("/localizer")) {
event_loop->SetRuntimeRealtimePriority(30);
+ PCHECK(system("sudo chmod 644 /dev/adis16505") == 0)
+ << ": Failed to set read permissions on IMU device.";
imu_fd_ = open("/dev/adis16505", O_RDONLY | O_NONBLOCK);
PCHECK(imu_fd_ != -1) << ": Failed to open SPI device for IMU.";
aos::internal::EPoll *epoll = event_loop_->epoll();
diff --git a/y2022/localizer/kernel/Makefile b/y2022/localizer/kernel/Makefile
new file mode 100644
index 0000000..ad4b9b0
--- /dev/null
+++ b/y2022/localizer/kernel/Makefile
@@ -0,0 +1,9 @@
+obj-m += adis16505.o
+
+PWD := $(CURDIR)
+
+all:
+ ARCH=arm64 CROSS_COMPILE=aarch64-linux-gnu- make -C ../../../../linux M=$(PWD) modules
+
+clean:
+ ARCH=arm64 CROSS_COMPILE=aarch64-linux-gnu- make -C ../../../../linux M=$(PWD) clean
diff --git a/y2022/localizer/kernel/README b/y2022/localizer/kernel/README
new file mode 100644
index 0000000..10f4e7b
--- /dev/null
+++ b/y2022/localizer/kernel/README
@@ -0,0 +1 @@
+As of 2022.02.08 this is the current version of the imu kernel driver being used
diff --git a/y2022/localizer/kernel/adis16505.c b/y2022/localizer/kernel/adis16505.c
new file mode 100644
index 0000000..a91006e
--- /dev/null
+++ b/y2022/localizer/kernel/adis16505.c
@@ -0,0 +1,325 @@
+/*
+ * adis16505.c - Driver for the adis16505 IMU used by 971.
+ */
+#include <linux/cdev.h>
+#include <linux/debugfs.h>
+#include <linux/delay.h>
+#include <linux/interrupt.h>
+#include <linux/kernel.h> /* Needed for pr_info() */
+#include <linux/kfifo.h>
+#include <linux/module.h> /* Needed by all modules */
+#include <linux/poll.h>
+
+#include <linux/of_gpio.h>
+#include <linux/spi/spi.h>
+#include <linux/of_address.h>
+#include <linux/of.h>
+#include <linux/of_platform.h>
+
+
+MODULE_LICENSE("GPL");
+MODULE_AUTHOR("frc971");
+MODULE_DESCRIPTION("adis16505 rp2040 driver");
+
+#define MODULE_NAME "adis16505"
+
+//! filter for the device tree class
+static struct of_device_id adis16505_match[] = {
+ {.compatible = "frc971,adis16505"}, {}};
+
+MODULE_DEVICE_TABLE(of, adis16505_match);
+
+#define TRANSFER_SIZE 42
+struct imu_sample {
+ char time[8];
+ char d[TRANSFER_SIZE];
+};
+
+struct adis16505_state {
+ dev_t character_device;
+ struct class *device_class;
+ struct cdev handle_cdev;
+
+ struct spi_device *spi;
+
+ struct spi_message spi_msg;
+
+ struct spi_transfer spi_xfer;
+
+ char tx_buff[128];
+ char rx_buff[128];
+
+ int count;
+
+ spinlock_t lock;
+
+ wait_queue_head_t wq;
+
+ spinlock_t fifo_read_lock;
+ DECLARE_KFIFO(fifo, struct imu_sample, 32);
+};
+
+static int adis16505_dev_open(struct inode *in, struct file *f) {
+ struct adis16505_state *ts =
+ container_of(in->i_cdev, struct adis16505_state, handle_cdev);
+ int count;
+
+ f->private_data = ts;
+
+ spin_lock(&ts->lock);
+ count = ts->count;
+ if (count == 0) {
+ ++(ts->count);
+ }
+ spin_unlock(&ts->lock);
+
+ printk("open %p, count %d\n", ts, count);
+ if (count > 0) {
+ return -EBUSY;
+ }
+ return 0;
+}
+
+static int adis16505_dev_release(struct inode *in, struct file *f) {
+ struct adis16505_state *ts;
+ ts = container_of(in->i_cdev, struct adis16505_state, handle_cdev);
+
+ printk("release %p\n", ts);
+ spin_lock(&ts->lock);
+ --(ts->count);
+ spin_unlock(&ts->lock);
+
+ return 0;
+}
+
+static ssize_t adis16505_dev_read(struct file *f, char *d, size_t s,
+ loff_t *of) {
+ struct adis16505_state *ts = f->private_data;
+ int err;
+
+ if (s != sizeof(struct imu_sample)) {
+ return -EINVAL;
+ }
+
+ while (true) {
+ struct imu_sample sample;
+ int elements;
+
+ spin_lock(&ts->fifo_read_lock);
+ elements = kfifo_get(&ts->fifo, &sample);
+ spin_unlock(&ts->fifo_read_lock);
+
+ if (elements == 0) {
+ bool empty;
+ if (f->f_flags & O_NONBLOCK) {
+ return -EAGAIN;
+ }
+
+ err = wait_event_interruptible(ts->wq,
+ (spin_lock(&ts->fifo_read_lock),
+ empty = !kfifo_is_empty(&ts->fifo),
+ spin_unlock(&ts->fifo_read_lock), empty));
+ if (err != 0) {
+ return err;
+ }
+ continue;
+ }
+
+ memcpy(d, &sample, sizeof(sample));
+ return sizeof(sample);
+ }
+}
+
+static unsigned int adis16505_dev_poll(struct file *f,
+ struct poll_table_struct *wait) {
+ struct adis16505_state *ts = f->private_data;
+ __poll_t mask = 0;
+
+ poll_wait(f, &ts->wq, wait);
+
+ spin_lock(&ts->fifo_read_lock);
+ if (!kfifo_is_empty(&ts->fifo)) {
+ mask |= (POLLIN | POLLRDNORM);
+ }
+ spin_unlock(&ts->fifo_read_lock);
+
+ return mask;
+}
+
+static const struct file_operations adis16505_cdev_opps = {
+ .read = adis16505_dev_read,
+ .open = adis16505_dev_open,
+ .release = adis16505_dev_release,
+ .poll = adis16505_dev_poll,
+};
+
+static void all_done(void *ts_ptr) {
+ // struct adis16505_state *ts = ts_ptr;
+ // printk("All done %x %x\n", ts->rx_buff[0], ts->rx_buff[1]);
+}
+
+static irqreturn_t adis16505_irq(int irq, void *handle) {
+ struct adis16505_state *ts = handle;
+ struct imu_sample s;
+ int err;
+ int i;
+
+ u64 time = ktime_get_ns();
+ memcpy(&s.time, &time, sizeof(time));
+
+ spi_message_init(&ts->spi_msg);
+ for (i = 0; i < TRANSFER_SIZE; ++i) {
+ ts->tx_buff[i] = i;
+ }
+
+ ts->spi_xfer.tx_buf = ts->tx_buff;
+ ts->spi_xfer.rx_buf = ts->rx_buff;
+ ts->spi_xfer.len = TRANSFER_SIZE;
+
+ spi_message_add_tail(&ts->spi_xfer, &ts->spi_msg);
+
+ ts->spi_msg.complete = all_done;
+ ts->spi_msg.context = ts;
+
+ err = spi_sync(ts->spi, &ts->spi_msg);
+
+ // TODO(austin): Timestamp. Also decode the packet for real.
+ for (i = 0; i < TRANSFER_SIZE; ++i) {
+ s.d[i] = ts->rx_buff[i];
+ }
+
+ // Attempt to emplace. If it fails, just drop the data.
+ kfifo_put(&ts->fifo, s);
+
+ wake_up_interruptible(&ts->wq);
+
+ return IRQ_HANDLED;
+}
+
+static int adis16505_probe(struct spi_device *spi) {
+ int err;
+ struct adis16505_state *ts;
+
+ if (!spi->irq) {
+ dev_dbg(&spi->dev, "no IRQ?\n");
+ return -EINVAL;
+ }
+
+ if (spi->max_speed_hz > 10000000) {
+ dev_err(&spi->dev, "f(sample) %d KHz?\n", spi->max_speed_hz / 1000);
+ return -EINVAL;
+ }
+
+ spi->bits_per_word = 8;
+ spi->mode = SPI_MODE_3;
+ spi->max_speed_hz = 2000000;
+
+ err = spi_setup(spi);
+ if (err < 0) {
+ return err;
+ }
+
+ ts = kzalloc(sizeof(struct adis16505_state), GFP_KERNEL);
+
+ if (!ts) {
+ err = -ENOMEM;
+ goto err_free_mem;
+ }
+
+ printk("Ts allocated %p\n", ts);
+
+ spin_lock_init(&ts->lock);
+ spin_lock_init(&ts->fifo_read_lock);
+ ts->count = 0;
+ INIT_KFIFO(ts->fifo);
+ init_waitqueue_head(&ts->wq);
+
+ spi_set_drvdata(spi, ts);
+ ts->spi = spi;
+
+ // Flags are sourced from the device tree.
+ err = request_threaded_irq(spi->irq, NULL, adis16505_irq, IRQF_ONESHOT,
+ spi->dev.driver->name, ts);
+
+ if (!ts) {
+ dev_dbg(&spi->dev, "irq %d busy?\n", spi->irq);
+ goto err_free_mem;
+ }
+
+ err = alloc_chrdev_region(&ts->character_device, 0, 1, "adis16505");
+ if (err < 0) {
+ dev_dbg(&spi->dev, "alloc_chrdev_region error %i", err);
+ goto err_free_irq;
+ }
+
+ // create device class
+ if ((ts->device_class = class_create(THIS_MODULE, "adis16505_class")) ==
+ NULL) {
+ dev_dbg(&spi->dev, "class_create error");
+ goto error_classCreate;
+ }
+
+ if (NULL == device_create(ts->device_class, NULL, ts->character_device, NULL,
+ "adis16505")) {
+ dev_dbg(&spi->dev, "device_create error");
+ goto error_deviceCreate;
+ }
+
+ cdev_init(&ts->handle_cdev, &adis16505_cdev_opps);
+ err = cdev_add(&ts->handle_cdev, ts->character_device, 1);
+ if (-1 == err) {
+ dev_dbg(&spi->dev, "cdev_add error %i", err);
+ goto error_device_add;
+ return -1;
+ }
+
+ dev_dbg(&spi->dev, "Probed adis16505\n");
+
+ if (err < 0) {
+ goto err_free_mem;
+ }
+
+ return 0;
+
+error_device_add:
+ device_destroy(ts->device_class, ts->character_device);
+error_deviceCreate:
+ class_destroy(ts->device_class);
+error_classCreate:
+ unregister_chrdev_region(ts->character_device, 1);
+err_free_irq:
+ free_irq(spi->irq, ts);
+
+err_free_mem:
+ kfree(ts);
+ return err;
+}
+
+static int adis16505_remove(struct spi_device *spi) {
+ struct adis16505_state *ts = spi_get_drvdata(spi);
+
+ device_destroy(ts->device_class, ts->character_device);
+
+ class_destroy(ts->device_class);
+
+ unregister_chrdev_region(ts->character_device, 1);
+
+ free_irq(spi->irq, ts);
+
+ kfree(ts);
+
+ dev_dbg(&spi->dev, "unregistered adis16505\n");
+ return 0;
+}
+
+static struct spi_driver adis16505_driver = {
+ .driver =
+ {
+ .name = "adis16505",
+ .of_match_table = of_match_ptr(adis16505_match),
+ },
+ .probe = adis16505_probe,
+ .remove = adis16505_remove,
+};
+
+module_spi_driver(adis16505_driver);
diff --git a/y2022/wpilib_interface.cc b/y2022/wpilib_interface.cc
index 2fc5f6b..032b686 100644
--- a/y2022/wpilib_interface.cc
+++ b/y2022/wpilib_interface.cc
@@ -193,6 +193,10 @@
position_builder.add_intake_front(intake_offset_front);
position_builder.add_intake_back(intake_offset_back);
position_builder.add_turret(turret_offset);
+ position_builder.add_intake_beambreak_front(
+ intake_beambreak_front_->Get());
+ position_builder.add_intake_beambreak_back(intake_beambreak_back_->Get());
+ position_builder.add_turret_beambreak(turret_beambreak_->Get());
builder.CheckOk(builder.Send(position_builder.Finish()));
}
@@ -294,6 +298,16 @@
turret_encoder_.set_potentiometer(::std::move(potentiometer));
}
+ void set_intake_beambreak_front(::std::unique_ptr<frc::DigitalInput> sensor) {
+ intake_beambreak_front_ = ::std::move(sensor);
+ }
+ void set_intake_beambreak_back(::std::unique_ptr<frc::DigitalInput> sensor) {
+ intake_beambreak_back_ = ::std::move(sensor);
+ }
+ void set_turret_beambreak(::std::unique_ptr<frc::DigitalInput> sensor) {
+ turret_beambreak_ = ::std::move(sensor);
+ }
+
private:
std::shared_ptr<const Values> values_;
@@ -304,6 +318,9 @@
std::array<std::unique_ptr<frc::DigitalInput>, 2> autonomous_modes_;
+ std::unique_ptr<frc::DigitalInput> intake_beambreak_front_,
+ intake_beambreak_back_, turret_beambreak_;
+
std::unique_ptr<frc::AnalogInput> climber_potentiometer_,
flipper_arm_right_potentiometer_, flipper_arm_left_potentiometer_;
frc971::wpilib::AbsoluteEncoderAndPotentiometer intake_encoder_front_,
@@ -483,6 +500,10 @@
sensor_reader.set_turret_absolute_pwm(make_unique<frc::DigitalInput>(4));
sensor_reader.set_turret_potentiometer(make_unique<frc::AnalogInput>(4));
+ sensor_reader.set_intake_beambreak_front(make_unique<frc::DigitalInput>(5));
+ sensor_reader.set_intake_beambreak_back(make_unique<frc::DigitalInput>(6));
+ sensor_reader.set_turret_beambreak(make_unique<frc::DigitalInput>(7));
+
sensor_reader.set_climber_potentiometer(make_unique<frc::AnalogInput>(5));
sensor_reader.set_flipper_arm_left_potentiometer(