Make the Teensy actually send useful camera commands
Change-Id: Ie4081c1e63cb94ecb81c96832938918e18ca9712
diff --git a/motors/print/BUILD b/motors/print/BUILD
index 26ee976..26d8e45 100644
--- a/motors/print/BUILD
+++ b/motors/print/BUILD
@@ -8,6 +8,7 @@
restricted_to = mcu_cpus,
visibility = ["//visibility:public"],
deps = [
+ "//aos/containers:sized_array",
"//motors/core",
"//third_party/GSL",
],
diff --git a/motors/print/print.h b/motors/print/print.h
index d51a2c3..89da913 100644
--- a/motors/print/print.h
+++ b/motors/print/print.h
@@ -3,6 +3,7 @@
#include <memory>
+#include "aos/containers/sized_array.h"
#include "motors/core/kinetis.h"
#include "third_party/GSL/include/gsl/gsl"
@@ -29,6 +30,11 @@
// Writes something to a separate debug stream. Some implementations will
// always ignore this, and others will ignore it under some conditions.
virtual int WriteDebug(gsl::span<const char> buffer) { return buffer.size(); }
+
+ // Reads any characters which are available (never blocks).
+ //
+ // The default never returns any data.
+ virtual aos::SizedArray<char, 4> ReadStdin() { return {}; }
};
// A trivial printing "implementation" which simply does nothing. This is used
diff --git a/motors/print/usb.h b/motors/print/usb.h
index 7e63184..981c5d3 100644
--- a/motors/print/usb.h
+++ b/motors/print/usb.h
@@ -28,6 +28,12 @@
return debug_tty_->Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_->Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::AcmTty *const stdout_tty_;
teensy::AcmTty *const debug_tty_;
@@ -50,6 +56,12 @@
return debug_tty_.Write(buffer.data(), buffer.size());
}
+ aos::SizedArray<char, 4> ReadStdin() override {
+ aos::SizedArray<char, 4> result;
+ result.set_size(stdout_tty_.Read(result.data(), result.max_size()));
+ return result;
+ }
+
private:
teensy::UsbDevice usb_device_;
teensy::AcmTty stdout_tty_;
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 3df3758..a3b93d2 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -577,7 +577,8 @@
// claiming that it's impossible to queue up the first byte for the slave end of
// an SPI connection properly. Instead, we just accept there will be a garbage
// byte and the other end ignores it.
-__attribute__((unused)) void TransferData() {
+__attribute__((unused)) void TransferData(
+ frc971::motors::PrintingImplementation *printing) {
Uarts *const uarts = Uarts::global_instance;
std::array<CobsPacketizer<uart_to_teensy_size()>, 5> packetizers;
std::array<TransmitBuffer, 5> transmit_buffers{
@@ -585,13 +586,18 @@
FrameQueue frame_queue;
aos::monotonic_clock::time_point last_camera_send =
aos::monotonic_clock::min_time;
+ CameraCommand stdin_camera_command = CameraCommand::kNormal;
+ CameraCommand last_roborio_camera_command = CameraCommand::kNormal;
+
bool first = true;
while (true) {
{
const auto received_transfer = SpiQueue::global_instance->Tick();
if (received_transfer) {
const auto unpacked = SpiUnpackToTeensy(*received_transfer);
- if (!unpacked) {
+ if (unpacked) {
+ last_roborio_camera_command = unpacked->camera_command;
+ } else {
printf("UART decode error\n");
}
}
@@ -639,7 +645,11 @@
CameraCalibration calibration{};
calibration.teensy_now = aos::monotonic_clock::now();
calibration.realtime_now = aos::realtime_clock::min_time;
- calibration.camera_command = CameraCommand::kNormal;
+ if (last_roborio_camera_command != CameraCommand::kNormal) {
+ calibration.camera_command = last_roborio_camera_command;
+ } else {
+ calibration.camera_command = stdin_camera_command;
+ }
// TODO(Brian): Actually fill out the calibration field.
transmit_buffers[0].MaybeWritePacket(calibration);
transmit_buffers[1].MaybeWritePacket(calibration);
@@ -652,6 +662,29 @@
}
}
+ {
+ const auto stdin_data = printing->ReadStdin();
+ if (!stdin_data.empty()) {
+ switch (stdin_data.back()) {
+ case 'p':
+ printf("Entering passthrough mode\n");
+ stdin_camera_command = CameraCommand::kCameraPassthrough;
+ break;
+ case 'u':
+ printf("Entering USB mode\n");
+ stdin_camera_command = CameraCommand::kUsb;
+ break;
+ case 'n':
+ printf("Entering normal mode\n");
+ stdin_camera_command = CameraCommand::kNormal;
+ break;
+ default:
+ printf("Unrecognized character\n");
+ break;
+ }
+ }
+ }
+
first = false;
}
}
@@ -817,7 +850,7 @@
NVIC_ENABLE_IRQ(IRQ_SPI0);
NVIC_ENABLE_IRQ(IRQ_PORTA);
- TransferData();
+ TransferData(printing.get());
while (true) {
}