Merge "Tune setpoints"
diff --git a/y2019/BUILD b/y2019/BUILD
index f00f5ba..3fae9e5 100644
--- a/y2019/BUILD
+++ b/y2019/BUILD
@@ -72,6 +72,7 @@
"//frc971/wpilib:wpilib_robot_base",
"//third_party:phoenix",
"//third_party:wpilib",
+ "//y2019/control_loops/drivetrain:camera_queue",
"//y2019/control_loops/superstructure:superstructure_queue",
"//y2019/jevois:spi",
],
diff --git a/y2019/constants.cc b/y2019/constants.cc
index 375e6e7..915c53c 100644
--- a/y2019/constants.cc
+++ b/y2019/constants.cc
@@ -129,6 +129,7 @@
.nominal_skew_noise = 0.1,
.nominal_height_noise = 0.01};
+ constexpr double kInchesToMeters = 0.0254;
switch (team) {
// A set of constants for tests.
case 1:
@@ -192,6 +193,24 @@
stilts_params->zeroing_constants.measured_absolute_position = 0.0;
stilts->potentiometer_offset = 0.0;
+
+ // Deliberately make FOV a bit large so that we are overly conservative in
+ // our EKF.
+ for (auto &camera : r->cameras) {
+ camera.fov = M_PI_2 * 1.1;
+ }
+ // 0 - 2 ar ecurrently unpopulated:
+ r->cameras[0].pose.set_theta(M_PI);
+ r->cameras[1].pose.set_theta(0.26);
+ r->cameras[2].pose.set_theta(-0.26);
+ // 3 is front right
+ r->cameras[3].pose.set_theta(-12.2377 / 180.0 * M_PI);
+ *r->cameras[3].pose.mutable_pos() << 4.98126 * kInchesToMeters,
+ 1.96988 * kInchesToMeters, 33.4276 * kInchesToMeters;
+ // 4 is back
+ r->cameras[4].pose.set_theta(M_PI + 2.581 * M_PI / 180.0);
+ *r->cameras[4].pose.mutable_pos() << -6.93309 * kInchesToMeters,
+ 2.64735 * kInchesToMeters, 32.8758 * kInchesToMeters;
break;
default:
diff --git a/y2019/control_loops/drivetrain/BUILD b/y2019/control_loops/drivetrain/BUILD
index 677726e..de52215 100644
--- a/y2019/control_loops/drivetrain/BUILD
+++ b/y2019/control_loops/drivetrain/BUILD
@@ -88,6 +88,7 @@
srcs = [
"camera.q",
],
+ visibility = ["//visibility:public"],
)
cc_library(
diff --git a/y2019/control_loops/drivetrain/camera.h b/y2019/control_loops/drivetrain/camera.h
index 8015772..04e4157 100644
--- a/y2019/control_loops/drivetrain/camera.h
+++ b/y2019/control_loops/drivetrain/camera.h
@@ -108,7 +108,7 @@
Scalar heading; // radians
// The distance from the camera to the target.
Scalar distance; // meters
- // Height of the target from the floor.
+ // Height of the target from the camera.
Scalar height; // meters
// The angle of the target relative to the frame of the camera.
Scalar skew; // radians
@@ -292,7 +292,7 @@
view.reading.heading = heading;
view.reading.distance = distance;
view.reading.skew = skew;
- view.reading.height = target.pose().abs_pos().z();
+ view.reading.height = relative_pose.rel_pos().z();
view.target = ⌖
view.camera_pose = camera_abs_pose;
diff --git a/y2019/control_loops/drivetrain/camera.q b/y2019/control_loops/drivetrain/camera.q
index 5add759..6ef6f49 100644
--- a/y2019/control_loops/drivetrain/camera.q
+++ b/y2019/control_loops/drivetrain/camera.q
@@ -1,7 +1,6 @@
package y2019.control_loops.drivetrain;
-// These structures have a nearly one-to-one correspondence to those in
-// //y2019/jevois:structures.h. Please refer to that file for details.
+// See the Target structure in //y2019/jevois:structures.h for documentation.
struct CameraTarget {
float distance;
float height;
@@ -10,17 +9,17 @@
};
message CameraFrame {
- // monotonic time in nanoseconds at which frame was taken (note structure.h
- // uses age).
+ // Number of nanoseconds since the aos::monotonic_clock epoch at which this
+ // frame was captured.
int64_t timestamp;
// Number of targets actually in this frame.
uint8_t num_targets;
- // Buffer for the targets
+ // Buffer for the targets.
CameraTarget[3] targets;
- // Index of the camera with which this frame was taken:
+ // Index of the camera position (not serial number) which this frame is from.
uint8_t camera;
};
diff --git a/y2019/control_loops/drivetrain/localizer.h b/y2019/control_loops/drivetrain/localizer.h
index f011720..3e20a1e 100644
--- a/y2019/control_loops/drivetrain/localizer.h
+++ b/y2019/control_loops/drivetrain/localizer.h
@@ -56,7 +56,7 @@
if (t > HybridEkf::latest_t()) {
LOG(ERROR,
"target observations must be older than most recent encoder/gyro "
- "update.");
+ "update.\n");
return;
}
diff --git a/y2019/jevois/BUILD b/y2019/jevois/BUILD
index 3f5890a..afca4ef 100644
--- a/y2019/jevois/BUILD
+++ b/y2019/jevois/BUILD
@@ -246,6 +246,7 @@
"//motors/peripheral:uart",
"//motors/print:usb",
"//third_party/GSL",
+ "//y2019/vision:constants",
],
)
diff --git a/y2019/jevois/spi.cc b/y2019/jevois/spi.cc
index b6e6632..c695c89 100644
--- a/y2019/jevois/spi.cc
+++ b/y2019/jevois/spi.cc
@@ -32,9 +32,10 @@
// 2. 8 bits distance
// 3. 6 bits skew
// 4. 6 bits height
-// 5. 1 bit target valid (a present frame has all-valid targets)
-// 6. 1 bit target present (a present frame can have from 0 to 3
-// targets, depending on how many were found)
+// 5. 2 bits of quantity+index
+// The 6 bits of quantity+index (between all three targets) are:
+// 1. 4 bits camera index + 1 (0 means this isn't a valid frame)
+// 2. 2 bits target count
// Note that empty frames are still sent to indicate that the camera is
// still working even though it doesn't see any targets.
@@ -60,10 +61,7 @@
}
constexpr float distance_min() { return 0; }
-constexpr float distance_max() {
- // The field is 18.4m diagonally.
- return 18.4;
-}
+constexpr float distance_max() { return 10.0; }
constexpr int distance_bits() { return 8; }
constexpr int distance_offset() { return heading_offset() + heading_bits(); }
void distance_pack(float distance, gsl::span<char> destination) {
@@ -110,26 +108,29 @@
integer);
}
-constexpr int valid_bits() { return 1; }
-constexpr int valid_offset() { return height_offset() + height_bits(); }
-void valid_pack(bool valid, gsl::span<char> destination) {
- aos::PackBits<uint32_t, valid_bits(), valid_offset()>(valid, destination);
+constexpr int quantity_index_offset() { return height_offset() + height_bits(); }
+void camera_index_pack(int camera_index, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, 2, quantity_index_offset()>(camera_index & 3,
+ destination);
+ aos::PackBits<uint32_t, 2, quantity_index_offset() + 32>(
+ (camera_index >> 2) & 3, destination);
}
-bool valid_unpack(gsl::span<const char> source) {
- return aos::UnpackBits<uint32_t, valid_bits(), valid_offset()>(source);
+int camera_index_unpack(gsl::span<const char> source) {
+ int result = 0;
+ result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset()>(source);
+ result |= aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32>(source)
+ << 2;
+ return result;
+}
+void target_count_pack(int target_count, gsl::span<char> destination) {
+ aos::PackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(target_count,
+ destination);
+}
+int target_count_unpack(gsl::span<const char> source) {
+ return aos::UnpackBits<uint32_t, 2, quantity_index_offset() + 32 * 2>(source);
}
-constexpr int present_bits() { return 1; }
-constexpr int present_offset() { return valid_offset() + valid_bits(); }
-void present_pack(bool present, gsl::span<char> destination) {
- aos::PackBits<uint32_t, present_bits(), present_offset()>(present,
- destination);
-}
-bool present_unpack(gsl::span<const char> source) {
- return aos::UnpackBits<uint32_t, present_bits(), present_offset()>(source);
-}
-
-constexpr int next_offset() { return present_offset() + present_bits(); }
+constexpr int next_offset() { return quantity_index_offset() + 2; }
static_assert(next_offset() <= 32, "Target is too big");
} // namespace
@@ -138,14 +139,17 @@
SpiTransfer transfer;
gsl::span<char> remaining_space = transfer;
for (int frame = 0; frame < 3; ++frame) {
- for (int target = 0; target < 3; ++target) {
- remaining_space[0] = 0;
- remaining_space[1] = 0;
- remaining_space[2] = 0;
- remaining_space[3] = 0;
+ // Zero out all three targets and the age.
+ for (int i = 0; i < 3 * 4 + 1; ++i) {
+ remaining_space[i] = 0;
+ }
- if (static_cast<int>(message.frames.size()) > frame) {
- valid_pack(true, remaining_space);
+ if (static_cast<int>(message.frames.size()) > frame) {
+ camera_index_pack(message.frames[frame].camera_index + 1,
+ remaining_space);
+ target_count_pack(message.frames[frame].targets.size(), remaining_space);
+
+ for (int target = 0; target < 3; ++target) {
if (static_cast<int>(message.frames[frame].targets.size()) > target) {
heading_pack(message.frames[frame].targets[target].heading,
remaining_space);
@@ -155,23 +159,16 @@
remaining_space);
height_pack(message.frames[frame].targets[target].height,
remaining_space);
- present_pack(true, remaining_space);
- } else {
- present_pack(false, remaining_space);
}
- } else {
- valid_pack(false, remaining_space);
+ remaining_space = remaining_space.subspan(4);
}
- remaining_space = remaining_space.subspan(4);
- }
- if (static_cast<int>(message.frames.size()) > frame) {
const uint8_t age_count = message.frames[frame].age.count();
memcpy(&remaining_space[0], &age_count, 1);
+ remaining_space = remaining_space.subspan(1);
} else {
- remaining_space[0] = 0;
+ remaining_space = remaining_space.subspan(4 * 3 + 1);
}
- remaining_space = remaining_space.subspan(1);
}
{
uint16_t crc = jevois_crc_init();
@@ -191,13 +188,14 @@
TeensyToRoborio message;
gsl::span<const char> remaining_input = transfer;
for (int frame = 0; frame < 3; ++frame) {
- const bool have_frame = valid_unpack(remaining_input);
- if (have_frame) {
+ const int camera_index_plus = camera_index_unpack(remaining_input);
+ if (camera_index_plus > 0) {
message.frames.push_back({});
- }
- for (int target = 0; target < 3; ++target) {
- if (present_unpack(remaining_input)) {
- if (have_frame) {
+ message.frames.back().camera_index = camera_index_plus - 1;
+
+ const int target_count = target_count_unpack(remaining_input);
+ for (int target = 0; target < 3; ++target) {
+ if (target < target_count) {
message.frames.back().targets.push_back({});
message.frames.back().targets.back().heading =
heading_unpack(remaining_input);
@@ -208,16 +206,18 @@
message.frames.back().targets.back().height =
height_unpack(remaining_input);
}
+ remaining_input = remaining_input.subspan(4);
}
- remaining_input = remaining_input.subspan(4);
+ {
+ uint8_t age_count;
+ memcpy(&age_count, &remaining_input[0], 1);
+ message.frames.back().age = camera_duration(age_count);
+ }
+ remaining_input = remaining_input.subspan(1);
+ } else {
+ remaining_input = remaining_input.subspan(4 * 3 + 1);
}
- if (have_frame) {
- uint8_t age_count;
- memcpy(&age_count, &remaining_input[0], 1);
- message.frames.back().age = camera_duration(age_count);
- }
- remaining_input = remaining_input.subspan(1);
}
{
uint16_t calculated_crc = jevois_crc_init();
diff --git a/y2019/jevois/spi_test.cc b/y2019/jevois/spi_test.cc
index 32551db..84938f0 100644
--- a/y2019/jevois/spi_test.cc
+++ b/y2019/jevois/spi_test.cc
@@ -60,10 +60,13 @@
TeensyToRoborio input_message;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(9);
+ input_message.frames.back().camera_index = 2;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(7);
+ input_message.frames.back().camera_index = 5;
input_message.frames.push_back({});
input_message.frames.back().age = camera_duration(1);
+ input_message.frames.back().camera_index = 4;
const SpiTransfer transfer = SpiPackToRoborio(input_message);
const auto output_message = SpiUnpackToRoborio(transfer);
@@ -80,13 +83,16 @@
input_message.frames.back().targets.back().height = 1;
input_message.frames.back().targets.back().heading = 0.5;
input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().camera_index = 0;
input_message.frames.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
+ input_message.frames.back().camera_index = 2;
input_message.frames.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
input_message.frames.back().targets.push_back({});
+ input_message.frames.back().camera_index = 3;
const SpiTransfer transfer = SpiPackToRoborio(input_message);
const auto output_message = SpiUnpackToRoborio(transfer);
@@ -103,6 +109,93 @@
output_message->frames.back().targets.back().heading, 0.1);
EXPECT_NEAR(input_message.frames.back().targets.back().skew,
output_message->frames.back().targets.back().skew, 0.1);
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_EQ(input_message.frames[i].camera_index,
+ output_message->frames[i].camera_index);
+ }
+}
+
+// Tests that packing and unpacking two targets results in the same number on
+// the other side.
+TEST(SpiToRoborioPackTest, TwoTargets) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 1;
+ input_message.frames.back().targets.back().height = 0.9;
+ input_message.frames.back().targets.back().heading = 0.4;
+ input_message.frames.back().targets.back().skew = -0.4;
+ input_message.frames.back().age = camera_duration(9);
+ input_message.frames.back().camera_index = 2;
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(1u, output_message->frames.size());
+ ASSERT_EQ(2u, output_message->frames[0].targets.size());
+ for (int i = 0; i < 2; ++i) {
+ EXPECT_NEAR(input_message.frames.back().targets[i].distance,
+ output_message->frames.back().targets[i].distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].height,
+ output_message->frames.back().targets[i].height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].heading,
+ output_message->frames.back().targets[i].heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].skew,
+ output_message->frames.back().targets[i].skew, 0.1);
+ EXPECT_EQ(input_message.frames.back().age,
+ output_message->frames.back().age);
+ EXPECT_EQ(input_message.frames.back().camera_index,
+ output_message->frames.back().camera_index);
+ }
+}
+
+// Tests that packing and unpacking three targets results in the same number on
+// the other side.
+TEST(SpiToRoborioPackTest, ThreeTargets) {
+ TeensyToRoborio input_message;
+ input_message.frames.push_back({});
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 9;
+ input_message.frames.back().targets.back().height = 1;
+ input_message.frames.back().targets.back().heading = 0.5;
+ input_message.frames.back().targets.back().skew = -0.5;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 1;
+ input_message.frames.back().targets.back().height = 0.9;
+ input_message.frames.back().targets.back().heading = 0.4;
+ input_message.frames.back().targets.back().skew = -0.4;
+ input_message.frames.back().targets.push_back({});
+ input_message.frames.back().targets.back().distance = 2;
+ input_message.frames.back().targets.back().height = 0.7;
+ input_message.frames.back().targets.back().heading = 0.3;
+ input_message.frames.back().targets.back().skew = -0.3;
+ input_message.frames.back().age = camera_duration(1);
+ input_message.frames.back().camera_index = 1;
+
+ const SpiTransfer transfer = SpiPackToRoborio(input_message);
+ const auto output_message = SpiUnpackToRoborio(transfer);
+ ASSERT_TRUE(output_message);
+ ASSERT_EQ(1u, output_message->frames.size());
+ ASSERT_EQ(3u, output_message->frames[0].targets.size());
+ for (int i = 0; i < 3; ++i) {
+ EXPECT_NEAR(input_message.frames.back().targets[i].distance,
+ output_message->frames.back().targets[i].distance, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].height,
+ output_message->frames.back().targets[i].height, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].heading,
+ output_message->frames.back().targets[i].heading, 0.1);
+ EXPECT_NEAR(input_message.frames.back().targets[i].skew,
+ output_message->frames.back().targets[i].skew, 0.1);
+ EXPECT_EQ(input_message.frames.back().age,
+ output_message->frames.back().age);
+ EXPECT_EQ(input_message.frames.back().camera_index,
+ output_message->frames.back().camera_index);
+ }
}
// Tests packing and then unpacking an empty message.
diff --git a/y2019/jevois/structures.h b/y2019/jevois/structures.h
index 1f10dc0..9c74b11 100644
--- a/y2019/jevois/structures.h
+++ b/y2019/jevois/structures.h
@@ -66,7 +66,7 @@
// center of the camera's image plane to the center of the target.
float distance;
- // Height of the target in meters. Specifically, the distance from the floor
+ // Height of the target in meters. Specifically, the distance from the camera
// to the center of the target.
float height;
@@ -85,8 +85,8 @@
// The information extracted from a single camera frame.
//
// This is all the information sent from each camera to the Teensy.
-struct Frame {
- bool operator==(const Frame &other) const {
+struct CameraFrame {
+ bool operator==(const CameraFrame &other) const {
if (other.targets != targets) {
return false;
}
@@ -95,7 +95,7 @@
}
return true;
}
- bool operator!=(const Frame &other) const {
+ bool operator!=(const CameraFrame &other) const {
return !(*this == other);
}
@@ -106,6 +106,34 @@
camera_duration age;
};
+// The information extracted from a single camera frame, from a given camera.
+struct RoborioFrame {
+ bool operator==(const RoborioFrame &other) const {
+ if (other.targets != targets) {
+ return false;
+ }
+ if (other.age != age) {
+ return false;
+ }
+ if (other.camera_index != camera_index) {
+ return false;
+ }
+ return true;
+ }
+ bool operator!=(const RoborioFrame &other) const {
+ return !(*this == other);
+ }
+
+ // The top most interesting targets found in this frame.
+ aos::SizedArray<Target, 3> targets;
+
+ // How long ago from the current time this frame was captured.
+ camera_duration age;
+ // Which camera this is from (which position on the robot, not a serial
+ // number).
+ int camera_index;
+};
+
enum class CameraCommand : char {
// Stay in normal mode.
kNormal,
@@ -113,6 +141,9 @@
kCameraPassthrough,
// Go to being a useful USB device.
kUsb,
+ // Send As, which triggers the bootstrap script to drop directly into USB
+ // mode.
+ kAs,
};
// This is all the information sent from the Teensy to each camera.
@@ -170,7 +201,7 @@
// The newest frames received from up to three cameras. These will be the
// three earliest-received of all buffered frames.
- aos::SizedArray<Frame, 3> frames;
+ aos::SizedArray<RoborioFrame, 3> frames;
};
// This is all the information the RoboRIO sends to the Teensy.
@@ -182,6 +213,12 @@
if (other.light_rings != light_rings) {
return false;
}
+ if (other.realtime_now != realtime_now) {
+ return false;
+ }
+ if (other.camera_command != camera_command) {
+ return false;
+ }
return true;
}
bool operator!=(const RoborioToTeensy &other) const {
diff --git a/y2019/jevois/teensy.cc b/y2019/jevois/teensy.cc
index 539d7b6..7a0eb45 100644
--- a/y2019/jevois/teensy.cc
+++ b/y2019/jevois/teensy.cc
@@ -1,3 +1,6 @@
+#include <inttypes.h>
+#include <stdio.h>
+
#include "aos/time/time.h"
#include "motors/core/kinetis.h"
#include "motors/core/time.h"
@@ -10,6 +13,7 @@
#include "y2019/jevois/cobs.h"
#include "y2019/jevois/spi.h"
#include "y2019/jevois/uart.h"
+#include "y2019/vision/constants.h"
using frc971::teensy::InterruptBufferedUart;
using frc971::teensy::InterruptBufferedSpi;
@@ -87,6 +91,12 @@
buffer.PushSingle(0);
}
}
+
+ void FillAs() {
+ while (!buffer.full()) {
+ buffer.PushSingle('a');
+ }
+ }
};
InterruptBufferedSpi *global_spi_instance = nullptr;
@@ -269,9 +279,10 @@
FrameQueue(const FrameQueue &) = delete;
FrameQueue &operator=(const FrameQueue &) = delete;
- void UpdateFrame(int camera, const Frame &frame) {
+ void UpdateFrame(int camera, const CameraFrame &frame) {
frames_[camera].targets = frame.targets;
frames_[camera].capture_time = aos::monotonic_clock::now() - frame.age;
+ frames_[camera].camera_index = camera;
const aos::SizedArray<int, 3> old_last_frames = last_frames_;
last_frames_.clear();
for (int index : old_last_frames) {
@@ -302,6 +313,7 @@
aos::SizedArray<Target, 3> targets;
aos::monotonic_clock::time_point capture_time =
aos::monotonic_clock::min_time;
+ int camera_index;
};
std::array<FrameData, 5> frames_;
@@ -327,7 +339,7 @@
const FrameData &frame = frames_[index];
const auto age = aos::monotonic_clock::now() - frame.capture_time;
const auto rounded_age = aos::time::round<camera_duration>(age);
- message.frames.push_back({frame.targets, rounded_age});
+ message.frames.push_back({frame.targets, rounded_age, frame.camera_index});
last_frames_.push_back(index);
}
return SpiPackToRoborio(message);
@@ -346,8 +358,7 @@
next_off_time_ = next_off_time;
}
- void Tick() {
- const auto now = aos::monotonic_clock::now();
+ bool Tick(aos::monotonic_clock::time_point now) {
if (last_cycle_start_ == aos::monotonic_clock::min_time) {
last_cycle_start_ = now;
current_off_point_ = last_cycle_start_ + next_off_time_;
@@ -355,22 +366,38 @@
last_cycle_start_ += period();
current_off_point_ = last_cycle_start_ + next_off_time_;
}
- if (now > current_off_point_) {
- GPIOC_PCOR = 1 << 5;
- } else {
- GPIOC_PSOR = 1 << 5;
- }
+ return now > current_off_point_;
}
private:
aos::monotonic_clock::time_point last_cycle_start_ =
aos::monotonic_clock::min_time;
- aos::monotonic_clock::duration next_off_time_ = std::chrono::milliseconds(100);
+ aos::monotonic_clock::duration next_off_time_ =
+ std::chrono::milliseconds(100);
aos::monotonic_clock::time_point current_off_point_ =
aos::monotonic_clock::min_time;
};
+// Returns an identifier for the processor we're running on.
+uint32_t ProcessorIdentifier() {
+ uint32_t r = 0;
+ r |= SIM_UIDH << 24;
+ r |= SIM_UIDMH << 16;
+ r |= SIM_UIDML << 8;
+ r |= SIM_UIDL << 0;
+ return r;
+}
+
+std::array<int, 5> CameraSerialNumbers() {
+ switch (ProcessorIdentifier()) {
+ case 0xffff322e: // CODE bot
+ return {{0, 0, 0, 16, 19}};
+ default:
+ return {{0, 0, 0, 0, 0}};
+ }
+}
+
extern "C" {
void *__stack_chk_guard = (void *)0x67111971;
@@ -609,6 +636,23 @@
}
}
+struct LightRingState {
+ DebugLight debug_light;
+ aos::monotonic_clock::time_point last_frame = aos::monotonic_clock::max_time;
+
+ bool Tick(aos::monotonic_clock::time_point now) {
+ if (last_frame == aos::monotonic_clock::max_time) {
+ last_frame = now;
+ }
+ if (now > last_frame + std::chrono::seconds(1)) {
+ debug_light.set_next_off_time(std::chrono::milliseconds(500));
+ } else {
+ debug_light.set_next_off_time(std::chrono::seconds(0));
+ }
+ return debug_light.Tick(now);
+ }
+};
+
// Does the normal work of transferring data in all directions.
//
// https://community.nxp.com/thread/466937#comment-983881 is a post from NXP
@@ -621,16 +665,25 @@
std::array<CobsPacketizer<uart_to_teensy_size()>, 5> packetizers;
std::array<TransmitBuffer, 5> transmit_buffers{
{&uarts->cam0, &uarts->cam1, &uarts->cam2, &uarts->cam3, &uarts->cam4}};
+ std::array<LightRingState, 5> light_rings;
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;
- DebugLight debug_light;
+ DebugLight teensy_debug_light;
bool first = true;
while (true) {
- debug_light.Tick();
+ {
+ const auto now = aos::monotonic_clock::now();
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 5) = !teensy_debug_light.Tick(now);
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 11) = light_rings[0].Tick(now);
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 10) = light_rings[1].Tick(now);
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 8) = light_rings[2].Tick(now);
+ PERIPHERAL_BITBAND(GPIOC_PDOR, 9) = light_rings[3].Tick(now);
+ PERIPHERAL_BITBAND(GPIOB_PDOR, 18) = light_rings[4].Tick(now);
+ }
{
const auto received_transfer = SpiQueue::global_instance->Tick();
@@ -639,7 +692,7 @@
if (unpacked) {
last_roborio_camera_command = unpacked->camera_command;
} else {
- printf("UART decode error\n");
+ printf("SPI decode error\n");
}
}
}
@@ -658,8 +711,10 @@
UartUnpackToTeensy(packetizers[i].received_packet());
packetizers[i].clear_received_packet();
if (decoded) {
- printf("got one with %d\n", (int)decoded->targets.size());
frame_queue.UpdateFrame(i, *decoded);
+ light_rings[i].last_frame = aos::monotonic_clock::now();
+ } else {
+ printf("UART decode error\n");
}
}
}
@@ -681,30 +736,42 @@
}
{
const auto now = aos::monotonic_clock::now();
- if (last_camera_send + std::chrono::milliseconds(1000) < now) {
- last_camera_send = now;
- CameraCalibration calibration{};
- calibration.teensy_now = aos::monotonic_clock::now();
- calibration.realtime_now = aos::realtime_clock::min_time;
- if (last_roborio_camera_command != CameraCommand::kNormal) {
- calibration.camera_command = last_roborio_camera_command;
- } else {
- calibration.camera_command = stdin_camera_command;
+ CameraCommand current_camera_command = CameraCommand::kNormal;
+ if (last_roborio_camera_command != CameraCommand::kNormal) {
+ current_camera_command = last_roborio_camera_command;
+ } else {
+ current_camera_command = stdin_camera_command;
+ }
+ if (current_camera_command == CameraCommand::kUsb) {
+ teensy_debug_light.set_next_off_time(std::chrono::milliseconds(900));
+ } else if (current_camera_command == CameraCommand::kCameraPassthrough) {
+ teensy_debug_light.set_next_off_time(std::chrono::milliseconds(500));
+ } else {
+ teensy_debug_light.set_next_off_time(std::chrono::milliseconds(100));
+ }
+
+ if (current_camera_command == CameraCommand::kAs) {
+ for (size_t i = 0; i < transmit_buffers.size(); ++i) {
+ transmit_buffers[i].FillAs();
}
- if (calibration.camera_command == CameraCommand::kUsb) {
- debug_light.set_next_off_time(std::chrono::milliseconds(900));
- } else if (calibration.camera_command ==
- CameraCommand::kCameraPassthrough) {
- debug_light.set_next_off_time(std::chrono::milliseconds(500));
- } else {
- debug_light.set_next_off_time(std::chrono::milliseconds(100));
+ } else {
+ if (last_camera_send + std::chrono::milliseconds(1000) < now) {
+ last_camera_send = now;
+ CameraCalibration calibration{};
+ calibration.teensy_now = aos::monotonic_clock::now();
+ calibration.realtime_now = aos::realtime_clock::min_time;
+ calibration.camera_command = current_camera_command;
+
+ for (int i = 0; i < 5; ++i) {
+ const y2019::vision::CameraCalibration *const constants =
+ y2019::vision::GetCamera(CameraSerialNumbers()[i]);
+ (void)constants;
+ calibration.calibration(0, 0) = constants->intrinsics.mount_angle;
+ calibration.calibration(0, 1) = constants->intrinsics.focal_length;
+ calibration.calibration(0, 2) = constants->intrinsics.barrel_mount;
+ transmit_buffers[i].MaybeWritePacket(calibration);
+ }
}
- // TODO(Brian): Actually fill out the calibration field.
- transmit_buffers[0].MaybeWritePacket(calibration);
- transmit_buffers[1].MaybeWritePacket(calibration);
- transmit_buffers[2].MaybeWritePacket(calibration);
- transmit_buffers[3].MaybeWritePacket(calibration);
- transmit_buffers[4].MaybeWritePacket(calibration);
}
for (TransmitBuffer &transmit_buffer : transmit_buffers) {
transmit_buffer.Tick(now);
@@ -716,17 +783,36 @@
if (!stdin_data.empty()) {
switch (stdin_data.back()) {
case 'p':
- printf("Entering passthrough mode\n");
+ printf("Sending passthrough mode\n");
stdin_camera_command = CameraCommand::kCameraPassthrough;
break;
case 'u':
- printf("Entering USB mode\n");
+ printf("Sending USB mode\n");
stdin_camera_command = CameraCommand::kUsb;
break;
case 'n':
- printf("Entering normal mode\n");
+ printf("Sending normal mode\n");
stdin_camera_command = CameraCommand::kNormal;
break;
+ case 'a':
+ printf("Sending all 'a's\n");
+ stdin_camera_command = CameraCommand::kAs;
+ break;
+ case 'c':
+ printf("This UART board is 0x%" PRIx32 "\n", ProcessorIdentifier());
+ for (int i = 0; i < 5; ++i) {
+ printf("Camera slot %d's serial number is %d\n", i,
+ CameraSerialNumbers()[i]);
+ }
+ break;
+ case 'h':
+ printf("UART board commands:\n");
+ printf(" p: Send passthrough mode\n");
+ printf(" u: Send USB mode\n");
+ printf(" n: Send normal mode\n");
+ printf(" a: Send all-'a' mode\n");
+ printf(" c: Dump camera configuration\n");
+ break;
default:
printf("Unrecognized character\n");
break;
diff --git a/y2019/jevois/uart.cc b/y2019/jevois/uart.cc
index 3621da8..63138a8 100644
--- a/y2019/jevois/uart.cc
+++ b/y2019/jevois/uart.cc
@@ -15,7 +15,7 @@
namespace frc971 {
namespace jevois {
-UartToTeensyBuffer UartPackToTeensy(const Frame &message) {
+UartToTeensyBuffer UartPackToTeensy(const CameraFrame &message) {
std::array<char, uart_to_teensy_size()> buffer;
gsl::span<char> remaining_space = buffer;
remaining_space[0] = message.targets.size();
@@ -55,7 +55,7 @@
return result;
}
-tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> encoded_buffer) {
+tl::optional<CameraFrame> UartUnpackToTeensy(gsl::span<const char> encoded_buffer) {
std::array<char, uart_to_teensy_size()> buffer;
if (static_cast<size_t>(
CobsDecode<uart_to_teensy_size()>(encoded_buffer, &buffer).size()) !=
@@ -63,7 +63,7 @@
return tl::nullopt;
}
- Frame message;
+ CameraFrame message;
gsl::span<const char> remaining_input = buffer;
const int number_targets = remaining_input[0];
remaining_input = remaining_input.subspan(1);
diff --git a/y2019/jevois/uart.h b/y2019/jevois/uart.h
index 2a4acec..b9e784b 100644
--- a/y2019/jevois/uart.h
+++ b/y2019/jevois/uart.h
@@ -29,8 +29,8 @@
using UartToCameraBuffer =
aos::SizedArray<char, CobsMaxEncodedSize(uart_to_camera_size())>;
-UartToTeensyBuffer UartPackToTeensy(const Frame &message);
-tl::optional<Frame> UartUnpackToTeensy(gsl::span<const char> buffer);
+UartToTeensyBuffer UartPackToTeensy(const CameraFrame &message);
+tl::optional<CameraFrame> UartUnpackToTeensy(gsl::span<const char> buffer);
UartToCameraBuffer UartPackToCamera(const CameraCalibration &message);
tl::optional<CameraCalibration> UartUnpackToCamera(
diff --git a/y2019/jevois/uart_test.cc b/y2019/jevois/uart_test.cc
index d669688..b8f25c1 100644
--- a/y2019/jevois/uart_test.cc
+++ b/y2019/jevois/uart_test.cc
@@ -10,7 +10,7 @@
// Tests packing and then unpacking a message with arbitrary values.
TEST(UartToTeensyTest, Basic) {
- Frame input_message;
+ CameraFrame input_message;
for (int i = 0; i < 3; ++i) {
input_message.targets.push_back({});
Target *const target = &input_message.targets.back();
@@ -29,7 +29,7 @@
// Tests packing and then unpacking a message with arbitrary values and no
// frames.
TEST(UartToTeensyTest, NoFrames) {
- Frame input_message;
+ CameraFrame input_message;
input_message.age = camera_duration(123);
const UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
const auto output_message = UartUnpackToTeensy(buffer);
@@ -39,7 +39,7 @@
// Tests packing and then unpacking a message with just one frame.
TEST(UartToTeensyTest, OneFrame) {
- Frame input_message;
+ CameraFrame input_message;
{
input_message.targets.push_back({});
Target *const target = &input_message.targets.back();
@@ -75,7 +75,7 @@
// Tests that corrupting the data in various ways is handled properly.
TEST(UartToTeensyTest, CorruptData) {
- Frame input_message{};
+ CameraFrame input_message{};
{
UartToTeensyBuffer buffer = UartPackToTeensy(input_message);
buffer[0]++;
diff --git a/y2019/vision/BUILD b/y2019/vision/BUILD
index 0221254..2bdfd9c 100644
--- a/y2019/vision/BUILD
+++ b/y2019/vision/BUILD
@@ -1,6 +1,7 @@
load("//aos/build:queues.bzl", "queue_library")
load("//tools/build_rules:gtk_dependent.bzl", "gtk_dependent_cc_binary", "gtk_dependent_cc_library")
load("@com_google_protobuf//:protobuf.bzl", "cc_proto_library")
+load("//tools:environments.bzl", "mcu_cpus")
package(default_visibility = ["//visibility:public"])
@@ -13,6 +14,8 @@
name = "constants",
srcs = ["constants.cc"],
hdrs = ["constants.h"],
+ compatible_with = mcu_cpus,
+ visibility = ["//visibility:public"],
)
cc_library(
diff --git a/y2019/vision/constants.cc b/y2019/vision/constants.cc
index ea47dd7..fa7c130 100644
--- a/y2019/vision/constants.cc
+++ b/y2019/vision/constants.cc
@@ -7,48 +7,85 @@
CameraCalibration camera_4 = {
{
- 3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
+ 3.73623 / 180.0 * M_PI, 588.1, 0.269291 / 180.0 * M_PI,
},
{
- {{6.02674 * kInchesToMeters, 4.57805 * kInchesToMeters,
- 33.3849 * kInchesToMeters}},
- 22.4535 / 180.0 * M_PI,
+ {{6.02674 * kInchesToMeters, 4.57805 * kInchesToMeters,
+ 33.3849 * kInchesToMeters}},
+ 22.4535 / 180.0 * M_PI,
},
{
- 4,
- {{12.5 * kInchesToMeters, 12 * kInchesToMeters}},
- {{1 * kInchesToMeters, 0.0}},
- 26,
- "cam4_0/debug_viewer_jpeg_",
- 52,
+ 4,
+ {{12.5 * kInchesToMeters, 12 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 26,
+ "cam4_0/debug_viewer_jpeg_",
+ 52,
}};
CameraCalibration camera_5 = {
{
- 1.00774 / 180.0 * M_PI, 658.554, 2.43864 / 180.0 * M_PI,
+ 1.00774 / 180.0 * M_PI, 658.554, 2.43864 / 180.0 * M_PI,
},
{
- {{5.51248 * kInchesToMeters, 2.04087 * kInchesToMeters,
- 33.2555 * kInchesToMeters}},
- -13.1396 / 180.0 * M_PI,
+ {{5.51248 * kInchesToMeters, 2.04087 * kInchesToMeters,
+ 33.2555 * kInchesToMeters}},
+ -13.1396 / 180.0 * M_PI,
},
{
- 5,
- {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
- {{1 * kInchesToMeters, 0.0}},
- 26,
- "cam5_0/debug_viewer_jpeg_",
- 59,
+ 5,
+ {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 26,
+ "cam5_0/debug_viewer_jpeg_",
+ 59,
+ }};
+
+CameraCalibration camera_16 = {
+ {
+ -1.30906 / 180.0 * M_PI, 347.372, 2.18486 / 180.0 * M_PI,
+ },
+ {
+ {{4.98126 * kInchesToMeters, 1.96988 * kInchesToMeters,
+ 33.4276 * kInchesToMeters}},
+ -12.2377 / 180.0 * M_PI,
+ },
+ {
+ 16,
+ {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 16,
+ "cam16/debug_viewer_jpeg_",
+ 55,
+ }};
+
+// Note: x/y should be negated and heading should be offset by 180 deg to
+// account for this being calibrated on the rear of the robot.
+CameraCalibration camera_19 = {
+ {
+ -0.341036 / 180.0 * M_PI, 324.626, 1.2545 / 180.0 * M_PI,
+ },
+ {
+ {{6.93309 * kInchesToMeters, -2.64735 * kInchesToMeters,
+ 32.8758 * kInchesToMeters}},
+ 2.58102 / 180.0 * M_PI,
+ },
+ {
+ 19,
+ {{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
+ {{1 * kInchesToMeters, 0.0}},
+ 16,
+ "cam19/debug_viewer_jpeg_",
+ 68,
}};
const CameraCalibration *GetCamera(int camera_id) {
switch (camera_id) {
- case 4:
- return &camera_4;
- case 5:
- return &camera_5;
- default:
- return nullptr;
+ case 4: return &camera_4;
+ case 5: return &camera_5;
+ case 16: return &camera_16;
+ case 19: return &camera_19;
+ default: return nullptr;
}
}
diff --git a/y2019/vision/debug_viewer.cc b/y2019/vision/debug_viewer.cc
index 533dcf6..1db8e33 100644
--- a/y2019/vision/debug_viewer.cc
+++ b/y2019/vision/debug_viewer.cc
@@ -150,15 +150,14 @@
// Use the solver to generate an intermediate version of our results.
std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
- results.emplace_back(finder_.ProcessTargetToResult(target, true));
+ results.emplace_back(finder_.ProcessTargetToResult(target, draw_raw_IR_));
if (draw_raw_IR_) DrawResult(results.back(), {255, 128, 0});
}
// Check that our current results match possible solutions.
- results = finder_.FilterResults(results);
+ results = finder_.FilterResults(results, 0);
if (draw_results_) {
for (const IntermediateResult &res : results) {
- DrawResult(res, {0, 255, 0});
DrawTarget(res, {0, 255, 0});
}
}
@@ -194,7 +193,7 @@
printf(" n: Toggle drawing countours before and after warping.\n");
printf(" m: Toggle drawing raw blob data (may need to change image to toggle a redraw).\n");
printf(" h: Print this message.\n");
- printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv"
+ printf(" a: May log camera image to /tmp/debug_viewer_jpeg_<#>.yuyv\n");
printf(" q: Exit the application.\n");
} else if (key == 'q') {
printf("User requested shutdown.\n");
diff --git a/y2019/vision/global_calibration.cc b/y2019/vision/global_calibration.cc
index bf64310..94048ad 100644
--- a/y2019/vision/global_calibration.cc
+++ b/y2019/vision/global_calibration.cc
@@ -17,6 +17,8 @@
// CERES Clashes with logging symbols...
#include "ceres/ceres.h"
+DEFINE_int32(camera_id, -1, "The camera ID to calibrate");
+
using ::aos::events::DataSocket;
using ::aos::events::RXUdpSocket;
using ::aos::events::TCPServer;
@@ -74,16 +76,13 @@
}
void main(int argc, char **argv) {
- (void)argc;
- (void)argv;
using namespace y2019::vision;
- // gflags::ParseCommandLineFlags(&argc, &argv, false);
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
- int camera_id = 5;
const char *base_directory = "/home/parker/data/frc/2019_calibration/";
DatasetInfo info = {
- camera_id,
+ FLAGS_camera_id,
{{12.5 * kInchesToMeters, 0.5 * kInchesToMeters}},
{{kInchesToMeters, 0.0}},
26,
@@ -248,7 +247,7 @@
results.dataset = info;
results.intrinsics = IntrinsicParams::get(&intrinsics[0]);
results.geometry = CameraGeometry::get(&geometry[0]);
- DumpCameraConstants(camera_id, results);
+ DumpCameraConstants(info.camera_id, results);
}
} // namespace y2019
diff --git a/y2019/vision/target_finder.cc b/y2019/vision/target_finder.cc
index b46d802..be2e262 100644
--- a/y2019/vision/target_finder.cc
+++ b/y2019/vision/target_finder.cc
@@ -419,13 +419,24 @@
}
std::vector<IntermediateResult> TargetFinder::FilterResults(
- const std::vector<IntermediateResult> &results) {
+ const std::vector<IntermediateResult> &results, uint64_t print_rate) {
std::vector<IntermediateResult> filtered;
for (const IntermediateResult &res : results) {
if (res.solver_error < 75.0) {
filtered.emplace_back(res);
}
}
+ frame_count_++;
+ if (!filtered.empty()) {
+ valid_result_count_++;
+ }
+ if (print_rate > 0 && frame_count_ > print_rate) {
+ LOG(INFO, "Found (%zu / %zu)(%.2f) targets.\n", valid_result_count_,
+ frame_count_, (double)valid_result_count_ / (double)frame_count_);
+ frame_count_ = 0;
+ valid_result_count_ = 0;
+ }
+
return filtered;
}
diff --git a/y2019/vision/target_finder.h b/y2019/vision/target_finder.h
index bdf67f2..4583690 100644
--- a/y2019/vision/target_finder.h
+++ b/y2019/vision/target_finder.h
@@ -49,7 +49,7 @@
IntermediateResult ProcessTargetToResult(const Target &target, bool verbose);
std::vector<IntermediateResult> FilterResults(
- const std::vector<IntermediateResult> &results);
+ const std::vector<IntermediateResult> &results, uint64_t print_rate);
// Get the local overlay for debug if we are doing that.
aos::vision::PixelLinesOverlay *GetOverlay() { return &overlay_; }
@@ -78,6 +78,10 @@
IntrinsicParams intrinsics_;
ExtrinsicParams default_extrinsics_;
+
+ // Counts for logging.
+ size_t frame_count_;
+ size_t valid_result_count_;
};
} // namespace vision
diff --git a/y2019/vision/target_sender.cc b/y2019/vision/target_sender.cc
index 2c7d8e2..06235ff 100644
--- a/y2019/vision/target_sender.cc
+++ b/y2019/vision/target_sender.cc
@@ -289,12 +289,11 @@
TargetFinder finder_;
- // Check that our current results match possible solutions.
aos::vision::CameraParams params0;
- params0.set_exposure(400);
+ params0.set_exposure(50);
params0.set_brightness(40);
params0.set_width(640);
- // params0.set_fps(10);
+ params0.set_fps(15);
params0.set_height(480);
::std::unique_ptr<CameraStream> camera0(
@@ -302,9 +301,11 @@
camera0->set_on_frame([&](DataRef data,
monotonic_clock::time_point monotonic_now) {
aos::vision::ImageFormat fmt{640, 480};
+ // Use threshold from aos::vision. This will run at 15 FPS.
aos::vision::BlobList imgs =
- aos::vision::FindBlobs(::DoThresholdYUYV(fmt, data.data(), 120));
+ aos::vision::FindBlobs(aos::vision::DoThresholdYUYV(fmt, data.data(), 120));
finder_.PreFilter(&imgs);
+ LOG(INFO, "Blobs: (%zu).\n", imgs.size());
bool verbose = false;
std::vector<std::vector<Segment<2>>> raw_polys;
@@ -317,26 +318,31 @@
raw_polys.push_back(polygon);
}
}
+ LOG(INFO, "Polygons: (%zu).\n", raw_polys.size());
// Calculate each component side of a possible target.
std::vector<TargetComponent> target_component_list =
finder_.FillTargetComponentList(raw_polys);
+ LOG(INFO, "Components: (%zu).\n", target_component_list.size());
// Put the compenents together into targets.
std::vector<Target> target_list =
finder_.FindTargetsFromComponents(target_component_list, verbose);
+ LOG(INFO, "Potential Target: (%zu).\n", target_list.size());
// Use the solver to generate an intermediate version of our results.
std::vector<IntermediateResult> results;
for (const Target &target : target_list) {
results.emplace_back(finder_.ProcessTargetToResult(target, verbose));
}
+ LOG(INFO, "Raw Results: (%zu).\n", results.size());
- results = finder_.FilterResults(results);
+ results = finder_.FilterResults(results, 30);
+ LOG(INFO, "Results: (%zu).\n", results.size());
// TODO: Select top 3 (randomly?)
- frc971::jevois::Frame frame{};
+ frc971::jevois::CameraFrame frame{};
for (size_t i = 0; i < results.size() && i < frame.targets.max_size();
++i) {
@@ -390,6 +396,7 @@
switch (calibration.camera_command) {
case CameraCommand::kNormal:
+ case CameraCommand::kAs:
break;
case CameraCommand::kUsb:
return 0;
diff --git a/y2019/vision/tools/deploy.sh b/y2019/vision/tools/deploy.sh
index ff3ee01..3c16b03 100755
--- a/y2019/vision/tools/deploy.sh
+++ b/y2019/vision/tools/deploy.sh
@@ -1,32 +1,38 @@
#!/bin/sh
+set -e
+
echo "Building executables"
readonly BAZEL_OPTIONS="-c opt --cpu=armhf-debian"
readonly BAZEL_BIN="$(bazel info ${BAZEL_OPTIONS} bazel-bin)"
+readonly TARGET_DIR=/media/$USER/JEVOIS
bazel build ${BAZEL_OPTIONS} \
//y2019/vision:target_sender \
//y2019/vision:serial_waiter
-echo "Mount jevois ..."
-./jevois-cmd usbsd
+if [ ! -d "${TARGET_DIR}" ]
+then
+ echo "Mount jevois at ${TARGET_DIR} ..."
+ ./jevois-cmd usbsd
+fi
echo "Waiting for fs ..."
-while [ ! -d /media/$USER/JEVOIS ]
+while [ ! -d "${TARGET_DIR}" ]
do
sleep 1
done
echo "OK"
echo "Copying files ..."
-cp ./austin_cam.sh /media/$USER/JEVOIS/
-cp ./launch.sh /media/$USER/JEVOIS/deploy/
+cp ./austin_cam.sh "${TARGET_DIR}"/
+cp ./launch.sh "${TARGET_DIR}"/deploy/
cp "${BAZEL_BIN}/y2019/vision/target_sender" \
"${BAZEL_BIN}/y2019/vision/serial_waiter" \
- /media/$USER/JEVOIS/deploy/
+ "${TARGET_DIR}"/deploy/
echo "Unmount sd card ..."
-umount /media/$USER/JEVOIS
+umount "${TARGET_DIR}"
echo "OK"
echo "Rebooting Jevois."
diff --git a/y2019/vision/tools/flash.sh b/y2019/vision/tools/flash.sh
index 5daad73..d3c55ae 100755
--- a/y2019/vision/tools/flash.sh
+++ b/y2019/vision/tools/flash.sh
@@ -6,6 +6,19 @@
exit
fi
+echo "Pulling Binaries ..."
+if [ ! -f "./libjevoisbase.so" ]
+then
+ wget http://www.frc971.org/Build-Dependencies/libjevoisbase.so
+fi
+echo "Got libjevoisbase.so"
+
+if [ ! -f "./PassThrough.so" ]
+then
+ wget http://www.frc971.org/Build-Dependencies/PassThrough.so
+fi
+echo "Got PassThrough.so"
+
DEV_BASE=/dev/mmcblk0p
echo "Please check that ${DEV_BASE}3 is the correct drive to format. This is a destructive command so please be sure."
@@ -18,7 +31,7 @@
fi
# need to disable some new features on 17.x
-sudo mkfs.ext4 -L JEVOIS -O ^64bit,uninit_bg,^metadata_csum ${DEV_BASE}3
+sudo mkfs.ext3 -L JEVOIS ${DEV_BASE}3
echo "Mounting JEVOIS."
mkdir -p /tmp/JEVOIS
@@ -30,15 +43,20 @@
echo "Make JEVOIS directories."
mkdir -p /tmp/JEVOIS/packages
-mkdir -p /tmp/JEVOIS/modules
+mkdir -p /tmp/JEVOIS/modules/JeVois/PassThrough/
mkdir -p /tmp/JEVOIS/config
-mkdir -p /tmp/JEVOIS/lib
+mkdir -p /tmp/JEVOIS/lib/JeVois/
+mkdir -p /tmp/JEVOIS/deploy/
+mkdir -p /tmp/JEVOIS/data/
echo "Copy configs."
cp ./austin_cam.sh /tmp/JEVOIS
cp ./videomappings.cfg /tmp/JEVOIS/config/
cp ./rcS_script.txt /tmp/LINUX/etc/init.d/rcS
cp Jevois_fstab /tmp/LINUX/etc/fstab
+cp ./PassThrough.so /tmp/JEVOIS/modules/JeVois/PassThrough/
+cp ./libjevoisbase.so /tmp/JEVOIS/lib/JeVois/
+cp ./launch.sh /tmp/JEVOIS/deploy/
echo "Un-mounting JEVOIS"
sudo umount /tmp/JEVOIS
diff --git a/y2019/wpilib_interface.cc b/y2019/wpilib_interface.cc
index 02269eb..2b21eb8 100644
--- a/y2019/wpilib_interface.cc
+++ b/y2019/wpilib_interface.cc
@@ -28,6 +28,7 @@
#include "aos/util/log_interval.h"
#include "aos/util/phased_loop.h"
#include "aos/util/wrapping_counter.h"
+#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "frc971/autonomous/auto.q.h"
#include "frc971/control_loops/drivetrain/drivetrain.q.h"
#include "frc971/wpilib/ADIS16448.h"
@@ -42,8 +43,8 @@
#include "frc971/wpilib/pdp_fetcher.h"
#include "frc971/wpilib/sensor_reader.h"
#include "frc971/wpilib/wpilib_robot_base.h"
-#include "ctre/phoenix/motorcontrol/can/TalonSRX.h"
#include "y2019/constants.h"
+#include "y2019/control_loops/drivetrain/camera.q.h"
#include "y2019/control_loops/superstructure/superstructure.q.h"
#include "y2019/jevois/spi.h"
@@ -319,7 +320,23 @@
return;
}
- // TODO(Brian): Do something useful with the targets.
+ const auto now = aos::monotonic_clock::now();
+ for (const auto &received : unpacked->frames) {
+ auto to_send = control_loops::drivetrain::camera_frames.MakeMessage();
+ to_send->timestamp =
+ std::chrono::nanoseconds((now - received.age).time_since_epoch())
+ .count();
+ to_send->num_targets = received.targets.size();
+ for (size_t i = 0; i < received.targets.size(); ++i) {
+ to_send->targets[i].distance = received.targets[i].distance;
+ to_send->targets[i].height = received.targets[i].height;
+ to_send->targets[i].heading = received.targets[i].heading;
+ to_send->targets[i].skew = received.targets[i].skew;
+ }
+ to_send->camera = received.camera_index;
+ LOG_STRUCT(DEBUG, "camera_frames", *to_send);
+ to_send.Send();
+ }
if (dummy_spi_) {
uint8_t dummy_send, dummy_receive;