Add debug_serial which can be used to read jevois serial messages with a PC.
Change-Id: I5d20bd4c5d55d7f0d04120731c6aad59e8c53dd5
diff --git a/y2019/vision/debug_serial.cc b/y2019/vision/debug_serial.cc
new file mode 100644
index 0000000..a095b14
--- /dev/null
+++ b/y2019/vision/debug_serial.cc
@@ -0,0 +1,96 @@
+#include "y2019/jevois/serial.h"
+
+#include "aos/logging/implementations.h"
+#include "aos/logging/logging.h"
+#include "y2019/jevois/cobs.h"
+#include "y2019/jevois/serial.h"
+#include "y2019/jevois/structures.h"
+#include "y2019/jevois/uart.h"
+
+#include <fcntl.h>
+#include <unistd.h>
+#include <chrono>
+#include <fstream>
+#include <iostream>
+#include <thread>
+
+namespace y2019 {
+namespace vision {
+
+void main(int argc, char **argv) {
+ (void)argc;
+ (void)argv;
+ using namespace y2019::vision;
+ using namespace frc971::jevois;
+ // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ ::aos::logging::Init();
+ ::aos::logging::AddImplementation(
+ new ::aos::logging::StreamLogImplementation(stderr));
+
+ int flags = fcntl(0, F_GETFL, 0);
+ fcntl(0, F_SETFL, flags | O_NONBLOCK);
+
+ int itsDev = ::y2019::jevois::open_via_terminos("/dev/ttyUSB0");
+
+ CobsPacketizer<uart_to_teensy_size()> cobs;
+ while (true) {
+ {
+ constexpr size_t kBufferSize = uart_to_teensy_size();
+ char data[kBufferSize];
+ ssize_t n = read(itsDev, &data[0], kBufferSize);
+ if (n >= 1) {
+ cobs.ParseData(gsl::span<const char>(&data[0], n));
+ auto packet = cobs.received_packet();
+ if (!packet.empty()) {
+ // One we read data from the serial, Teensy code will return an
+ // optional with success if there is a new frame. unwrap that
+ // and print out any frames from inside.
+ auto frame_optional = UartUnpackToTeensy(packet);
+ if (frame_optional) {
+ printf("----------\n");
+ const auto &frame = *frame_optional;
+ for (const auto &target : frame.targets) {
+ printf("z: %g y: %g, r1: %g, r2: %g\n", target.distance / 0.0254,
+ target.height / 0.0254, target.skew, target.heading);
+ }
+ } else {
+ printf("bad frame\n");
+ }
+ cobs.clear_received_packet();
+ }
+ }
+ }
+ {
+ constexpr size_t kBufferSize = 1024;
+ char data[kBufferSize];
+ // read command char from stdin. 'p' = passthrough else usb.
+ ssize_t n = read(0, &data[0], kBufferSize);
+ if (n >= 1) {
+ CameraCalibration calibration{};
+ if (data[0] == 'p') {
+ calibration.camera_command =
+ CameraCalibration::CameraCommand::kCameraPassthrough;
+ } else {
+ calibration.camera_command = CameraCalibration::CameraCommand::kUsb;
+ }
+ if (write(itsDev, "\0", 1) == 1) {
+ const auto out_data = frc971::jevois::UartPackToCamera(calibration);
+ // We don't really care if this succeeds or not. If it fails for some
+ // reason, we'll just try again with the next frame, and the other end
+ // will find the new packet just fine.
+ if (static_cast<int>(
+ write(itsDev, out_data.data(), out_data.size())) !=
+ static_cast<int>(out_data.size())) {
+ printf("ERROR!!! START OVER FROM FIRST PRINCIPLES!!\n");
+ }
+ }
+ }
+ }
+ std::this_thread::sleep_for(std::chrono::milliseconds(2));
+ }
+}
+
+} // namespace y2019
+} // namespace vision
+
+int main(int argc, char **argv) { y2019::vision::main(argc, argv); }