Merge "Added intake joint fourbar loop and python"
diff --git a/aos/BUILD b/aos/BUILD
index 2ae0fec..f0c12b9 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -463,6 +463,7 @@
"@com_github_google_glog//:glog",
"@com_google_absl//absl/strings",
"@com_google_absl//absl/types:span",
+ "//aos:macros",
],
)
diff --git a/aos/flatbuffers.h b/aos/flatbuffers.h
index e556c0f..15740a1 100644
--- a/aos/flatbuffers.h
+++ b/aos/flatbuffers.h
@@ -5,6 +5,7 @@
#include <string_view>
#include "absl/types/span.h"
+#include "aos/macros.h"
#include "flatbuffers/flatbuffers.h"
#include "glog/logging.h"
@@ -30,12 +31,14 @@
void Reset() { is_allocated_ = false; }
bool is_allocated() const { return is_allocated_; }
+ bool allocated() { return is_allocated_; }
+
private:
bool is_allocated_ = false;
};
// This class is a fixed memory allocator which holds the data for a flatbuffer
-// in an array.
+// in a vector.
class FixedAllocator : public FixedAllocatorBase {
public:
FixedAllocator(size_t size) : buffer_(size, 0) {}
@@ -55,7 +58,7 @@
class PreallocatedAllocator : public FixedAllocatorBase {
public:
PreallocatedAllocator(void *data, size_t size) : data_(data), size_(size) {}
- PreallocatedAllocator(const PreallocatedAllocator&) = delete;
+ PreallocatedAllocator(const PreallocatedAllocator &) = delete;
PreallocatedAllocator(PreallocatedAllocator &&other)
: data_(other.data_), size_(other.size_) {
CHECK(!is_allocated());
@@ -229,6 +232,50 @@
flatbuffers::DetachedBuffer buffer_;
};
+// Array backed flatbuffer which manages building of the flatbuffer.
+template <typename T, size_t Size>
+class FlatbufferFixedAllocatorArray final : public Flatbuffer<T> {
+ public:
+ FlatbufferFixedAllocatorArray() : buffer_(), allocator_(&buffer_[0], Size) {
+ builder_ = flatbuffers::FlatBufferBuilder(Size, &allocator_);
+ }
+
+ flatbuffers::FlatBufferBuilder *Builder() {
+ if (allocator_.allocated()) {
+ LOG(FATAL) << "Array backed flatbuffer can only be built once";
+ }
+ return &builder_;
+ }
+
+ void Finish(flatbuffers::Offset<T> root) {
+ if (!allocator_.allocated()) {
+ LOG(FATAL) << "Cannot finish if never building";
+ }
+ builder_.Finish(root);
+ data_ = builder_.GetBufferPointer();
+ size_ = builder_.GetSize();
+ }
+
+ const uint8_t *data() const override {
+ CHECK_NOTNULL(data_);
+ return data_;
+ }
+ uint8_t *data() override {
+ CHECK_NOTNULL(data_);
+ return data_;
+ }
+ size_t size() const override { return size_; }
+
+ private:
+ std::array<uint8_t, Size> buffer_;
+ PreallocatedAllocator allocator_;
+ flatbuffers::FlatBufferBuilder builder_;
+ uint8_t *data_ = nullptr;
+ size_t size_ = 0;
+
+ DISALLOW_COPY_AND_ASSIGN(FlatbufferFixedAllocatorArray);
+};
+
// This object associates the message type with the memory storing the
// flatbuffer. This only stores root tables.
//
diff --git a/y2020/vision/v4l2_reader.cc b/y2020/vision/v4l2_reader.cc
index 727f8ba..f43a2ac 100644
--- a/y2020/vision/v4l2_reader.cc
+++ b/y2020/vision/v4l2_reader.cc
@@ -15,10 +15,7 @@
PCHECK(fd_.get() != -1);
// First, clean up after anybody else who left the device streaming.
- {
- int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
- PCHECK(Ioctl(VIDIOC_STREAMOFF, &type) == 0);
- }
+ StreamOff();
{
struct v4l2_format format;
@@ -130,5 +127,19 @@
PCHECK(Ioctl(VIDIOC_QBUF, &buffer) == 0);
}
+void V4L2Reader::StreamOff() {
+ int type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
+ const int result = Ioctl(VIDIOC_STREAMOFF, &type);
+ 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.
+ if (errno == EBUSY) {
+ return;
+ }
+ PLOG(FATAL) << "VIDIOC_STREAMOFF failed";
+}
+
} // namespace vision
} // namespace frc971
diff --git a/y2020/vision/v4l2_reader.h b/y2020/vision/v4l2_reader.h
index 969f4a8..bdf4a8e 100644
--- a/y2020/vision/v4l2_reader.h
+++ b/y2020/vision/v4l2_reader.h
@@ -88,6 +88,8 @@
int Ioctl(unsigned long number, void *arg);
+ void StreamOff();
+
// The mmaped V4L2 buffers.
std::array<Buffer, kNumberBuffers> buffers_;