Merge changes If3498bf9,Ieae86dce,I2c2f5bc0,I452994fb
* changes:
Make -v1 print out a stripped message header
Support globs in maps
Switch to a batch IMU message
Make logger and timesamp_logger config lists
diff --git a/aos/BUILD b/aos/BUILD
index 1d6d388..da558a4 100644
--- a/aos/BUILD
+++ b/aos/BUILD
@@ -481,6 +481,9 @@
"testdata/expected_multinode.json",
"testdata/good_multinode.json",
"testdata/good_multinode_hostnames.json",
+ "testdata/invalid_channel_name1.json",
+ "testdata/invalid_channel_name2.json",
+ "testdata/invalid_channel_name3.json",
"testdata/invalid_destination_node.json",
"testdata/invalid_nodes.json",
"testdata/invalid_source_node.json",
diff --git a/aos/configuration.cc b/aos/configuration.cc
index d6d276e..0a34b02 100644
--- a/aos/configuration.cc
+++ b/aos/configuration.cc
@@ -1,11 +1,11 @@
#include "aos/configuration.h"
-#include <string.h>
-#include <stdlib.h>
-#include <sys/types.h>
-#include <netinet/in.h>
#include <arpa/inet.h>
#include <ifaddrs.h>
+#include <netinet/in.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
#include <unistd.h>
#include <set>
@@ -80,8 +80,7 @@
namespace {
// Extracts the folder part of a path. Returns ./ if there is no path.
-std::string_view ExtractFolder(
- const std::string_view filename) {
+std::string_view ExtractFolder(const std::string_view filename) {
auto last_slash_pos = filename.find_last_of("/\\");
return last_slash_pos == std::string_view::npos
@@ -178,7 +177,7 @@
// Compares for equality (c == p) a channel, and a name, type tuple.
bool EqualsChannels(const Channel *c,
- ::std::pair<std::string_view, std::string_view> p) {
+ ::std::pair<std::string_view, std::string_view> p) {
return c->name()->string_view() == p.first &&
c->type()->string_view() == p.second;
}
@@ -195,8 +194,7 @@
// Maps name for the provided maps. Modifies name.
void HandleMaps(const flatbuffers::Vector<flatbuffers::Offset<aos::Map>> *maps,
- std::string_view *name, std::string_view type,
- const Node *node) {
+ std::string *name, std::string_view type, const Node *node) {
// For the same reason we merge configs in reverse order, we want to process
// maps in reverse order. That lets the outer config overwrite channels from
// the inner configs.
@@ -210,8 +208,16 @@
// Handle normal maps (now that we know that match and rename are filled
// out).
- if (i->match()->name()->string_view() != *name) {
- continue;
+ const std::string_view match_name = i->match()->name()->string_view();
+ if (match_name != *name) {
+ if (match_name.back() == '*' &&
+ std::string_view(*name).substr(
+ 0, std::min(name->size(), match_name.size() - 1)) ==
+ match_name.substr(0, match_name.size() - 1)) {
+ CHECK_EQ(match_name.find('*'), match_name.size() - 1);
+ } else {
+ continue;
+ }
}
// Handle type specific maps.
@@ -219,15 +225,19 @@
continue;
}
+ // Now handle node specific maps.
if (node != nullptr && i->match()->has_source_node() &&
i->match()->source_node()->string_view() !=
node->name()->string_view()) {
continue;
}
- VLOG(1) << "Renamed \"" << *name << "\" to \""
- << i->rename()->name()->string_view() << "\"";
- *name = i->rename()->name()->string_view();
+ std::string new_name(i->rename()->name()->string_view());
+ if (match_name.back() == '*') {
+ new_name += std::string(name->substr(match_name.size() - 1));
+ }
+ VLOG(1) << "Renamed \"" << *name << "\" to \"" << new_name << "\"";
+ *name = std::move(new_name);
}
}
@@ -298,8 +308,7 @@
{
::std::vector<flatbuffers::Offset<Channel>> channel_offsets;
for (const FlatbufferDetachedBuffer<Channel> &c : channels) {
- channel_offsets.emplace_back(
- CopyFlatBuffer<Channel>(&c.message(), &fbb));
+ channel_offsets.emplace_back(CopyFlatBuffer<Channel>(&c.message(), &fbb));
}
channels_offset = fbb.CreateVector(channel_offsets);
}
@@ -362,8 +371,36 @@
// Check that if there is a node list, all the source nodes are filled out and
// valid, and all the destination nodes are valid (and not the source). This
// is a basic consistency check.
- if (result.message().has_nodes() && config.message().has_channels()) {
- for (const Channel *c : *config.message().channels()) {
+ if (result.message().has_channels()) {
+ for (const Channel *c : *result.message().channels()) {
+ if (c->name()->string_view().back() == '/') {
+ LOG(FATAL) << "Channel names can't end with '/'";
+ }
+ if(c->name()->string_view().find("//")!= std::string_view::npos) {
+ LOG(FATAL) << ": Invalid channel name " << c->name()->string_view()
+ << ", can't use //.";
+ }
+ for (const char data : c->name()->string_view()) {
+ if (data >= '0' && data <= '9') {
+ continue;
+ }
+ if (data >= 'a' && data <= 'z') {
+ continue;
+ }
+ if (data >= 'A' && data <= 'Z') {
+ continue;
+ }
+ if (data == '-' || data == '_' || data == '/') {
+ continue;
+ }
+ LOG(FATAL) << "Invalid channel name " << c->name()->string_view()
+ << ", can only use [-a-zA-Z0-9_/]";
+ }
+ }
+ }
+
+ if (result.message().has_nodes() && result.message().has_channels()) {
+ for (const Channel *c : *result.message().channels()) {
CHECK(c->has_source_node()) << ": Channel " << FlatbufferToJson(c)
<< " is missing \"source_node\"";
CHECK(GetNode(&result.message(), c->source_node()->string_view()) !=
@@ -383,18 +420,20 @@
switch (connection->timestamp_logger()) {
case LoggerConfig::LOCAL_LOGGER:
case LoggerConfig::NOT_LOGGED:
- CHECK(!connection->has_timestamp_logger_node());
+ CHECK(!connection->has_timestamp_logger_nodes());
break;
case LoggerConfig::REMOTE_LOGGER:
case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
- CHECK(connection->has_timestamp_logger_node());
- CHECK(
- GetNode(&result.message(),
- connection->timestamp_logger_node()->string_view()) !=
- nullptr)
- << ": Channel " << FlatbufferToJson(c)
- << " has an unknown \"timestamp_logger_node\""
- << connection->name()->string_view();
+ CHECK(connection->has_timestamp_logger_nodes());
+ CHECK_GT(connection->timestamp_logger_nodes()->size(), 0u);
+ for (const flatbuffers::String *timestamp_logger_node :
+ *connection->timestamp_logger_nodes()) {
+ CHECK(GetNode(&result.message(),
+ timestamp_logger_node->string_view()) != nullptr)
+ << ": Channel " << FlatbufferToJson(c)
+ << " has an unknown \"timestamp_logger_node\""
+ << connection->name()->string_view();
+ }
break;
}
@@ -429,6 +468,7 @@
std::string_view type,
std::string_view application_name, const Node *node) {
const std::string_view original_name = name;
+ std::string mutable_name;
VLOG(1) << "Looking up { \"name\": \"" << name << "\", \"type\": \"" << type
<< "\" }";
@@ -441,14 +481,18 @@
if (application_iterator != config->applications()->cend() &&
EqualsApplications(*application_iterator, application_name)) {
if (application_iterator->has_maps()) {
- HandleMaps(application_iterator->maps(), &name, type, node);
+ mutable_name = std::string(name);
+ HandleMaps(application_iterator->maps(), &mutable_name, type, node);
+ name = std::string_view(mutable_name);
}
}
}
// Now do global maps.
if (config->has_maps()) {
- HandleMaps(config->maps(), &name, type, node);
+ mutable_name = std::string(name);
+ HandleMaps(config->maps(), &mutable_name, type, node);
+ name = std::string_view(mutable_name);
}
if (original_name != name) {
@@ -458,8 +502,7 @@
// Then look for the channel.
auto channel_iterator =
- std::lower_bound(config->channels()->cbegin(),
- config->channels()->cend(),
+ std::lower_bound(config->channels()->cbegin(), config->channels()->cend(),
std::make_pair(name, type), CompareChannels);
// Make sure we actually found it, and it matches.
@@ -512,7 +555,7 @@
// Search for a schema with a matching type.
const aos::FlatbufferString<reflection::Schema> *found_schema = nullptr;
- for (const aos::FlatbufferString<reflection::Schema> &schema: schemas) {
+ for (const aos::FlatbufferString<reflection::Schema> &schema : schemas) {
if (schema.message().root_table() != nullptr) {
if (schema.message().root_table()->name()->string_view() ==
c->type()->string_view()) {
@@ -769,7 +812,7 @@
}
bool ChannelMessageIsLoggedOnNode(const Channel *channel, const Node *node) {
- switch(channel->logger()) {
+ switch (channel->logger()) {
case LoggerConfig::LOCAL_LOGGER:
if (node == nullptr) {
// Single node world. If there is a local logger, then we want to use
@@ -778,20 +821,24 @@
}
return channel->source_node()->string_view() ==
node->name()->string_view();
- case LoggerConfig::REMOTE_LOGGER:
- CHECK(channel->has_logger_node());
-
- return channel->logger_node()->string_view() ==
- CHECK_NOTNULL(node)->name()->string_view();
case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
- CHECK(channel->has_logger_node());
+ CHECK(channel->has_logger_nodes());
+ CHECK_GT(channel->logger_nodes()->size(), 0u);
if (channel->source_node()->string_view() ==
CHECK_NOTNULL(node)->name()->string_view()) {
return true;
}
- if (channel->logger_node()->string_view() == node->name()->string_view()) {
- return true;
+
+ [[fallthrough]];
+ case LoggerConfig::REMOTE_LOGGER:
+ CHECK(channel->has_logger_nodes());
+ CHECK_GT(channel->logger_nodes()->size(), 0u);
+ for (const flatbuffers::String *logger_node : *channel->logger_nodes()) {
+ if (logger_node->string_view() ==
+ CHECK_NOTNULL(node)->name()->string_view()) {
+ return true;
+ }
}
return false;
@@ -828,24 +875,27 @@
const Node *node) {
switch (connection->timestamp_logger()) {
case LoggerConfig::LOCAL_AND_REMOTE_LOGGER:
- CHECK(connection->has_timestamp_logger_node());
+ CHECK(connection->has_timestamp_logger_nodes());
+ CHECK_GT(connection->timestamp_logger_nodes()->size(), 0u);
if (connection->name()->string_view() == node->name()->string_view()) {
return true;
}
- if (connection->timestamp_logger_node()->string_view() ==
- node->name()->string_view()) {
- return true;
+ [[fallthrough]];
+ case LoggerConfig::REMOTE_LOGGER:
+ CHECK(connection->has_timestamp_logger_nodes());
+ CHECK_GT(connection->timestamp_logger_nodes()->size(), 0u);
+ for (const flatbuffers::String *timestamp_logger_node :
+ *connection->timestamp_logger_nodes()) {
+ if (timestamp_logger_node->string_view() ==
+ node->name()->string_view()) {
+ return true;
+ }
}
return false;
case LoggerConfig::LOCAL_LOGGER:
return connection->name()->string_view() == node->name()->string_view();
- case LoggerConfig::REMOTE_LOGGER:
- CHECK(connection->has_timestamp_logger_node());
-
- return connection->timestamp_logger_node()->string_view() ==
- node->name()->string_view();
case LoggerConfig::NOT_LOGGED:
return false;
}
diff --git a/aos/configuration.fbs b/aos/configuration.fbs
index 576616e..31d89e7 100644
--- a/aos/configuration.fbs
+++ b/aos/configuration.fbs
@@ -29,7 +29,7 @@
// remotely, which node should be responsible for logging the data. Note:
// for now, this can only be the source node. Empty implies the node this
// connection is connecting to (i.e. name).
- timestamp_logger_node:string;
+ timestamp_logger_nodes:[string];
// Priority to forward data with.
priority:ushort = 100;
@@ -77,13 +77,15 @@
// logging the data. Note: this requires that the data is forwarded to the
// node responsible for logging it. Empty implies the node this connection
// is connecting to (i.e. name).
- logger_node:string;
+ logger_nodes:[string];
}
// Table to support renaming channel names.
table Map {
// Channel to match with. If the name in here matches, the name is replaced
- // with the name in rename.
+ // with the name in rename. If the name ends in *, it will be treated like a
+ // wildcard. Anything with the same prefix will match, and anything matching
+ // the * will get preserved on rename. This supports moving subfolders.
// Node specific matches are also supported.
match:Channel;
// The channel to merge in.
diff --git a/aos/configuration_test.cc b/aos/configuration_test.cc
index f069277..aa6de8e 100644
--- a/aos/configuration_test.cc
+++ b/aos/configuration_test.cc
@@ -71,9 +71,9 @@
LOG(INFO) << "Read: " << FlatbufferToJson(config, true);
- EXPECT_EQ(FlatbufferToJson(GetChannel(config, ".aos.robot_state",
+ EXPECT_EQ(FlatbufferToJson(GetChannel(config, "/aos/robot_state",
"aos.RobotState", "app1", nullptr)),
- "{ \"name\": \".aos.robot_state\", \"type\": \"aos.RobotState\", "
+ "{ \"name\": \"/aos/robot_state\", \"type\": \"aos.RobotState\", "
"\"max_size\": 5 }");
}
@@ -87,6 +87,30 @@
kConfigPrefix + "config1_bad.json");
}
+// Tests that we reject invalid channel names. This means any channels with //
+// in their name, a trailing /, or regex characters.
+TEST_F(ConfigurationDeathTest, InvalidChannelName) {
+ EXPECT_DEATH(
+ {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig(kConfigPrefix + "invalid_channel_name1.json");
+ },
+ "Channel names can't end with '/'");
+ EXPECT_DEATH(
+ {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig(kConfigPrefix + "invalid_channel_name2.json");
+ },
+ "Invalid channel name");
+ EXPECT_DEATH(
+ {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig(kConfigPrefix + "invalid_channel_name3.json");
+ LOG(FATAL) << "Foo";
+ },
+ "Invalid channel name");
+}
+
// Tests that we can modify a config with a json snippet.
TEST_F(ConfigurationTest, MergeWithConfig) {
FlatbufferDetachedBuffer<Configuration> config =
@@ -203,6 +227,30 @@
kExpectedBazMultinodeLocation);
}
+// Tests that we can lookup a location with a glob
+TEST_F(ConfigurationTest, GetChannelGlob) {
+ FlatbufferDetachedBuffer<Configuration> config =
+ ReadConfig(kConfigPrefix + "good_multinode.json");
+ const Node *pi1 = GetNode(&config.message(), "pi1");
+
+ // Confirm that a glob with nothing after it matches.
+ const char *kExpectedMultinodeLocation =
+ "{ \"name\": \"/foo\", \"type\": \".aos.bar\", \"max_size\": 5, "
+ "\"source_node\": \"pi1\" }";
+ EXPECT_EQ(FlatbufferToJson(
+ GetChannel(config, "/magic/string", ".aos.bar", "app7", pi1)),
+ kExpectedMultinodeLocation);
+
+ // Now confirm that glob with something following it matches and renames
+ // correctly.
+ const char *kExpectedSubfolderMultinodeLocation =
+ "{ \"name\": \"/foo/subfolder\", \"type\": \".aos.bar\", \"max_size\": "
+ "5, \"source_node\": \"pi1\" }";
+ EXPECT_EQ(FlatbufferToJson(GetChannel(config, "/magic/string/subfolder",
+ ".aos.bar", "app7", pi1)),
+ kExpectedSubfolderMultinodeLocation);
+}
+
// Tests that we reject a configuration which has a nodes list, but has channels
// withoout source_node filled out.
TEST_F(ConfigurationDeathTest, InvalidSourceNode) {
@@ -354,7 +402,7 @@
"type": "aos.examples.Ping",
"source_node": "bar",
"logger": "REMOTE_LOGGER",
- "logger_node": "baz",
+ "logger_nodes": ["baz"],
"destination_nodes": [
{
"name": "baz"
@@ -370,7 +418,7 @@
"type": "aos.examples.Ping",
"source_node": "bar",
"logger": "REMOTE_LOGGER",
- "logger_node": "foo",
+ "logger_nodes": ["foo"],
"destination_nodes": [
{
"name": "baz"
@@ -386,7 +434,7 @@
"type": "aos.examples.Ping",
"source_node": "bar",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "baz",
+ "logger_nodes": ["baz"],
"destination_nodes": [
{
"name": "baz"
@@ -462,7 +510,7 @@
"type": "aos.examples.Ping",
"source_node": "bar",
"logger": "REMOTE_LOGGER",
- "logger_node": "baz",
+ "logger_nodes": ["baz"],
"destination_nodes": [
{
"name": "baz"
@@ -495,7 +543,7 @@
{
"name": "baz",
"timestamp_logger": "REMOTE_LOGGER",
- "timestamp_logger_node": "bar"
+ "timestamp_logger_nodes": ["bar"]
}
]
})channel",
@@ -508,12 +556,12 @@
"type": "aos.examples.Ping",
"source_node": "bar",
"logger": "REMOTE_LOGGER",
- "logger_node": "foo",
+ "logger_nodes": ["foo"],
"destination_nodes": [
{
"name": "baz",
"timestamp_logger": "REMOTE_LOGGER",
- "timestamp_logger_node": "foo"
+ "timestamp_logger_nodes": ["foo"]
}
]
})channel",
@@ -529,7 +577,7 @@
{
"name": "baz",
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_node": "bar"
+ "timestamp_logger_nodes": ["bar"]
}
]
})channel",
diff --git a/aos/events/logging/logfile_utils.cc b/aos/events/logging/logfile_utils.cc
index a7238ba..5388df7 100644
--- a/aos/events/logging/logfile_utils.cc
+++ b/aos/events/logging/logfile_utils.cc
@@ -432,10 +432,19 @@
const monotonic_clock::time_point timestamp = monotonic_clock::time_point(
chrono::nanoseconds(header.monotonic_sent_time()));
- VLOG(1) << "Queued " << this << " " << filename()
- << " ttq: " << time_to_queue_ << " now "
- << newest_timestamp() << " start time "
- << monotonic_start_time() << " " << FlatbufferToJson(&header);
+ if (VLOG_IS_ON(2)) {
+ LOG(INFO) << "Queued " << this << " " << filename()
+ << " ttq: " << time_to_queue_ << " now " << newest_timestamp()
+ << " start time " << monotonic_start_time() << " "
+ << FlatbufferToJson(&header);
+ } else if (VLOG_IS_ON(1)) {
+ FlatbufferVector<MessageHeader> copy = msg.value();
+ copy.mutable_message()->clear_data();
+ LOG(INFO) << "Queued " << this << " " << filename()
+ << " ttq: " << time_to_queue_ << " now " << newest_timestamp()
+ << " start time " << monotonic_start_time() << " "
+ << FlatbufferToJson(copy);
+ }
const int channel_index = header.channel_index();
was_emplaced = channels_to_write_[channel_index]->emplace_back(
diff --git a/aos/events/logging/multinode_pingpong.json b/aos/events/logging/multinode_pingpong.json
index 957cc2b..f1bcc54 100644
--- a/aos/events/logging/multinode_pingpong.json
+++ b/aos/events/logging/multinode_pingpong.json
@@ -43,7 +43,7 @@
"name": "pi2",
"priority": 1,
"timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
- "timestamp_logger_node": "pi1",
+ "timestamp_logger_nodes": ["pi1"],
"time_to_live": 5000000
}
]
@@ -59,7 +59,7 @@
"type": "aos.examples.Pong",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "pi1",
+ "logger_nodes": ["pi1"],
"destination_nodes": [
{
"name": "pi1",
diff --git a/aos/network/message_bridge_test_common.json b/aos/network/message_bridge_test_common.json
index 1e5dfce..1767c4c 100644
--- a/aos/network/message_bridge_test_common.json
+++ b/aos/network/message_bridge_test_common.json
@@ -105,7 +105,7 @@
"name": "pi2",
"priority": 1,
"timestamp_logger": "REMOTE_LOGGER",
- "timestamp_logger_node": "pi1"
+ "timestamp_logger_nodes": ["pi1"]
}
]
},
@@ -119,13 +119,13 @@
"type": "aos.examples.Pong",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "pi1",
+ "logger_nodes": ["pi1"],
"destination_nodes": [
{
"name": "pi1",
"priority": 1,
"timestamp_logger": "REMOTE_LOGGER",
- "timestamp_logger_node": "pi1"
+ "timestamp_logger_nodes": ["pi1"]
}
]
}
diff --git a/aos/testdata/backwards.json b/aos/testdata/backwards.json
index 192005e..a39d5c7 100644
--- a/aos/testdata/backwards.json
+++ b/aos/testdata/backwards.json
@@ -1,12 +1,12 @@
{
"channels": [
{
- "name": ".aos.robot_state",
+ "name": "/aos/robot_state",
"type": "aos.RobotState",
"max_size": 5
},
{
- "name": ".aos.joystick_state",
+ "name": "/aos/joystick_state",
"type": "aos.JoystickState"
}
]
diff --git a/aos/testdata/good_multinode.json b/aos/testdata/good_multinode.json
index 94db924..52a53b0 100644
--- a/aos/testdata/good_multinode.json
+++ b/aos/testdata/good_multinode.json
@@ -7,6 +7,12 @@
"source_node": "pi1"
},
{
+ "name": "/foo/subfolder",
+ "type": ".aos.bar",
+ "max_size": 5,
+ "source_node": "pi1"
+ },
+ {
"name": "/batman",
"type": ".aos.baz",
"max_size": 5,
@@ -23,6 +29,14 @@
"rename": {
"name": "/foo"
}
+ },
+ {
+ "match": {
+ "name": "/magic/string*"
+ },
+ "rename": {
+ "name": "/foo"
+ }
}
],
"applications": [
diff --git a/aos/testdata/invalid_channel_name1.json b/aos/testdata/invalid_channel_name1.json
new file mode 100644
index 0000000..90c6d24
--- /dev/null
+++ b/aos/testdata/invalid_channel_name1.json
@@ -0,0 +1,9 @@
+{
+ "channels": [
+ {
+ "name": "/foo/",
+ "type": ".aos.bar",
+ "max_size": 5
+ }
+ ]
+}
diff --git a/aos/testdata/invalid_channel_name2.json b/aos/testdata/invalid_channel_name2.json
new file mode 100644
index 0000000..070c124
--- /dev/null
+++ b/aos/testdata/invalid_channel_name2.json
@@ -0,0 +1,9 @@
+{
+ "channels": [
+ {
+ "name": "/f*oo",
+ "type": ".aos.bar",
+ "max_size": 5
+ }
+ ]
+}
diff --git a/aos/testdata/invalid_channel_name3.json b/aos/testdata/invalid_channel_name3.json
new file mode 100644
index 0000000..efe8e1b
--- /dev/null
+++ b/aos/testdata/invalid_channel_name3.json
@@ -0,0 +1,9 @@
+{
+ "channels": [
+ {
+ "name": "/foo//bar",
+ "type": ".aos.bar",
+ "max_size": 5
+ }
+ ]
+}
diff --git a/frc971/control_loops/drivetrain/BUILD b/frc971/control_loops/drivetrain/BUILD
index 8c81440..ac2a2e8 100644
--- a/frc971/control_loops/drivetrain/BUILD
+++ b/frc971/control_loops/drivetrain/BUILD
@@ -134,6 +134,7 @@
"//frc971/queues:gyro_fbs",
"//frc971/queues:gyro_uid_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
],
visibility = ["//visibility:public"],
deps = [
@@ -413,6 +414,7 @@
"//aos/util:log_interval",
"//frc971/control_loops:runge_kutta",
"//frc971/queues:gyro_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
"//frc971/wpilib:imu_fbs",
"//frc971/zeroing:imu_zeroer",
],
@@ -443,6 +445,7 @@
"//frc971/control_loops:state_feedback_loop",
"//frc971/queues:gyro_fbs",
"//frc971/wpilib:imu_fbs",
+ "//frc971/wpilib:imu_batch_fbs",
"//y2016:constants",
"//y2016/control_loops/drivetrain:polydrivetrain_plants",
] + cpu_select({
diff --git a/frc971/control_loops/drivetrain/drivetrain.cc b/frc971/control_loops/drivetrain/drivetrain.cc
index 1c0ac4f..a822ac4 100644
--- a/frc971/control_loops/drivetrain/drivetrain.cc
+++ b/frc971/control_loops/drivetrain/drivetrain.cc
@@ -18,7 +18,7 @@
#include "frc971/control_loops/runge_kutta.h"
#include "frc971/queues/gyro_generated.h"
#include "frc971/shifter_hall_effect.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
using ::aos::monotonic_clock;
namespace chrono = ::std::chrono;
@@ -37,7 +37,7 @@
localizer_control_fetcher_(
event_loop->MakeFetcher<LocalizerControl>("/drivetrain")),
imu_values_fetcher_(
- event_loop->MakeFetcher<::frc971::IMUValues>("/drivetrain")),
+ event_loop->MakeFetcher<::frc971::IMUValuesBatch>("/drivetrain")),
gyro_reading_fetcher_(
event_loop->MakeFetcher<::frc971::sensors::GyroReading>(
"/drivetrain")),
@@ -149,37 +149,44 @@
}
while (imu_values_fetcher_.FetchNext()) {
- imu_zeroer_.InsertMeasurement(*imu_values_fetcher_);
+ CHECK(imu_values_fetcher_->has_readings());
last_gyro_time_ = monotonic_now;
- if (!imu_zeroer_.Zeroed()) {
- continue;
- }
- aos::monotonic_clock::time_point reading_time(std::chrono::nanoseconds(
- imu_values_fetcher_->monotonic_timestamp_ns()));
- if (last_imu_update_ == aos::monotonic_clock::min_time) {
+ for (const IMUValues *value : *imu_values_fetcher_->readings()) {
+ imu_zeroer_.InsertMeasurement(*value);
+ if (!imu_zeroer_.Zeroed()) {
+ continue;
+ }
+ const aos::monotonic_clock::time_point reading_time(
+ std::chrono::nanoseconds(value->monotonic_timestamp_ns()));
+ if (last_imu_update_ == aos::monotonic_clock::min_time) {
+ last_imu_update_ = reading_time;
+ }
+ down_estimator_.Predict(imu_zeroer_.ZeroedGyro(),
+ imu_zeroer_.ZeroedAccel(),
+ reading_time - last_imu_update_);
last_imu_update_ = reading_time;
}
- down_estimator_.Predict(imu_zeroer_.ZeroedGyro(), imu_zeroer_.ZeroedAccel(),
- reading_time - last_imu_update_);
- last_imu_update_ = reading_time;
}
bool got_imu_reading = false;
if (imu_values_fetcher_.get() != nullptr) {
imu_zeroer_.ProcessMeasurements();
got_imu_reading = true;
+ CHECK(imu_values_fetcher_->has_readings());
+ const IMUValues *value = imu_values_fetcher_->readings()->Get(
+ imu_values_fetcher_->readings()->size() - 1);
switch (dt_config_.imu_type) {
case IMUType::IMU_X:
- last_accel_ = -imu_values_fetcher_->accelerometer_x();
+ last_accel_ = -value->accelerometer_x();
break;
case IMUType::IMU_FLIPPED_X:
- last_accel_ = imu_values_fetcher_->accelerometer_x();
+ last_accel_ = value->accelerometer_x();
break;
case IMUType::IMU_Y:
- last_accel_ = -imu_values_fetcher_->accelerometer_y();
+ last_accel_ = -value->accelerometer_y();
break;
case IMUType::IMU_Z:
- last_accel_ = imu_values_fetcher_->accelerometer_z();
+ last_accel_ = value->accelerometer_z();
break;
}
}
diff --git a/frc971/control_loops/drivetrain/drivetrain.h b/frc971/control_loops/drivetrain/drivetrain.h
index b766db8..d350321 100644
--- a/frc971/control_loops/drivetrain/drivetrain.h
+++ b/frc971/control_loops/drivetrain/drivetrain.h
@@ -21,7 +21,7 @@
#include "frc971/control_loops/drivetrain/splinedrivetrain.h"
#include "frc971/control_loops/drivetrain/ssdrivetrain.h"
#include "frc971/queues/gyro_generated.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/zeroing/imu_zeroer.h"
namespace frc971 {
@@ -59,7 +59,7 @@
const DrivetrainConfig<double> dt_config_;
::aos::Fetcher<LocalizerControl> localizer_control_fetcher_;
- ::aos::Fetcher<::frc971::IMUValues> imu_values_fetcher_;
+ ::aos::Fetcher<::frc971::IMUValuesBatch> imu_values_fetcher_;
::aos::Fetcher<::frc971::sensors::GyroReading> gyro_reading_fetcher_;
zeroing::ImuZeroer imu_zeroer_;
diff --git a/frc971/control_loops/drivetrain/drivetrain_config.json b/frc971/control_loops/drivetrain/drivetrain_config.json
index f452bc6..bafedff 100644
--- a/frc971/control_loops/drivetrain/drivetrain_config.json
+++ b/frc971/control_loops/drivetrain/drivetrain_config.json
@@ -3,8 +3,9 @@
[
{
"name": "/drivetrain",
- "type": "frc971.IMUValues",
- "frequency": 2000
+ "type": "frc971.IMUValuesBatch",
+ "max_size": 2000,
+ "frequency": 200
},
{
"name": "/drivetrain",
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
index 6a90122..3ed8d00 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.cc
@@ -10,6 +10,7 @@
#if defined(SUPPORT_PLOT)
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
#endif
+#include "frc971/wpilib/imu_batch_generated.h"
#include "y2016/constants.h"
#include "y2016/control_loops/drivetrain/drivetrain_dog_motor_plant.h"
#include "y2016/control_loops/drivetrain/hybrid_velocity_drivetrain.h"
@@ -107,7 +108,7 @@
drivetrain_status_fetcher_(
event_loop_->MakeFetcher<::frc971::control_loops::drivetrain::Status>(
"/drivetrain")),
- imu_sender_(event_loop->MakeSender<::frc971::IMUValues>("/drivetrain")),
+ imu_sender_(event_loop->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
dt_config_(dt_config),
drivetrain_plant_(MakePlantFromConfig(dt_config_)),
velocity_drivetrain_(
@@ -211,7 +212,16 @@
std::chrono::duration_cast<std::chrono::nanoseconds>(
event_loop_->monotonic_now().time_since_epoch())
.count());
- builder.Send(imu_builder.Finish());
+ flatbuffers::Offset<frc971::IMUValues> imu_values_offsets =
+ imu_builder.Finish();
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
+ imu_values_offset = builder.fbb()->CreateVector(&imu_values_offsets, 1);
+
+ frc971::IMUValuesBatch::Builder imu_values_batch_builder =
+ builder.MakeBuilder<frc971::IMUValuesBatch>();
+ imu_values_batch_builder.add_readings(imu_values_offset);
+ builder.Send(imu_values_batch_builder.Finish());
}
// Simulates the drivetrain moving for one timestep.
diff --git a/frc971/control_loops/drivetrain/drivetrain_test_lib.h b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
index e8b513c..f749626 100644
--- a/frc971/control_loops/drivetrain/drivetrain_test_lib.h
+++ b/frc971/control_loops/drivetrain/drivetrain_test_lib.h
@@ -9,7 +9,7 @@
#include "frc971/control_loops/drivetrain/drivetrain_position_generated.h"
#include "frc971/control_loops/drivetrain/drivetrain_status_generated.h"
#include "frc971/control_loops/state_feedback_loop.h"
-#include "frc971/wpilib/imu_generated.h"
+#include "frc971/wpilib/imu_batch_generated.h"
namespace frc971 {
namespace control_loops {
@@ -95,7 +95,7 @@
drivetrain_output_fetcher_;
::aos::Fetcher<::frc971::control_loops::drivetrain::Status>
drivetrain_status_fetcher_;
- ::aos::Sender<::frc971::IMUValues> imu_sender_;
+ ::aos::Sender<::frc971::IMUValuesBatch> imu_sender_;
DrivetrainConfig<double> dt_config_;
diff --git a/frc971/wpilib/ADIS16470.cc b/frc971/wpilib/ADIS16470.cc
index 28b1a0b..fa61d9b 100644
--- a/frc971/wpilib/ADIS16470.cc
+++ b/frc971/wpilib/ADIS16470.cc
@@ -4,6 +4,7 @@
#include "glog/logging.h"
+#include "aos/containers/sized_array.h"
#include "aos/time/time.h"
#include "hal/HAL.h"
@@ -174,7 +175,7 @@
frc::DigitalInput *data_ready, frc::DigitalOutput *reset)
: event_loop_(event_loop),
imu_values_sender_(
- event_loop_->MakeSender<::frc971::IMUValues>("/drivetrain")),
+ event_loop_->MakeSender<::frc971::IMUValuesBatch>("/drivetrain")),
initialize_timer_(
event_loop_->AddTimer([this]() { DoInitializeStep(); })),
spi_(spi),
@@ -209,8 +210,12 @@
return;
}
+ auto builder = imu_values_sender_.MakeBuilder();
+
int amount_to_read =
spi_->ReadAutoReceivedData(to_read_.data(), 0, 0 /* don't block */);
+
+ aos::SizedArray<flatbuffers::Offset<IMUValues>, 50> readings_offsets;
while (true) {
if (amount_to_read == 0) break;
CHECK(!to_read_.empty());
@@ -223,7 +228,9 @@
amount_to_read -= amount_read_now;
if (to_read_.empty()) {
- ProcessReading();
+ flatbuffers::Offset<IMUValues> reading_offset =
+ ProcessReading(builder.fbb());
+ readings_offsets.push_back(reading_offset);
// Reset for the next reading.
to_read_ = absl::MakeSpan(read_data_);
@@ -232,6 +239,15 @@
break;
}
}
+
+ flatbuffers::Offset<flatbuffers::Vector<flatbuffers::Offset<IMUValues>>>
+ readings_offset = builder.fbb()->CreateVector(readings_offsets.data(),
+ readings_offsets.size());
+
+ IMUValuesBatch::Builder imu_values_batch_builder =
+ builder.MakeBuilder<IMUValuesBatch>();
+ imu_values_batch_builder.add_readings(readings_offset);
+ builder.Send(imu_values_batch_builder.Finish());
}
void ADIS16470::DoInitializeStep() {
@@ -331,7 +347,17 @@
if (!self_test_diag_stat.IsNull()) {
imu_builder.add_self_test_diag_stat(self_test_diag_stat);
}
- builder.Send(imu_builder.Finish());
+
+ const flatbuffers::Offset<IMUValues> readings_offsets =
+ imu_builder.Finish();
+ const flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<IMUValues>>>
+ readings_offset = builder.fbb()->CreateVector(&readings_offsets, 1);
+
+ IMUValuesBatch::Builder imu_batch_builder =
+ builder.MakeBuilder<IMUValuesBatch>();
+ imu_batch_builder.add_readings(readings_offset);
+ builder.Send(imu_batch_builder.Finish());
if (success) {
state_ = State::kRunning;
} else {
@@ -345,13 +371,12 @@
}
}
-void ADIS16470::ProcessReading() {
+flatbuffers::Offset<IMUValues> ADIS16470::ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb) {
// If we ever see this, we'll need to decide how to handle it. Probably reset
// everything and try again.
CHECK_EQ(0, spi_->GetAutoDroppedCount());
- auto builder = imu_values_sender_.MakeBuilder();
-
absl::Span<const uint32_t> to_process = read_data_;
hal::fpga_clock::time_point fpga_time;
{
@@ -365,10 +390,10 @@
const uint16_t diag_stat_value = (static_cast<uint16_t>(to_process[0]) << 8) |
static_cast<uint16_t>(to_process[1]);
- const auto diag_stat = PackDiagStat(builder.fbb(), diag_stat_value);
+ const auto diag_stat = PackDiagStat(fbb, diag_stat_value);
to_process = to_process.subspan(2);
- IMUValues::Builder imu_builder = builder.MakeBuilder<IMUValues>();
+ IMUValues::Builder imu_builder(*fbb);
imu_builder.add_fpga_timestamp(
aos::time::DurationInSeconds(fpga_time.time_since_epoch()));
imu_builder.add_monotonic_timestamp_ns(
@@ -397,7 +422,7 @@
CHECK(to_process.empty()) << "Have leftover bytes: " << to_process.size();
- builder.Send(imu_builder.Finish());
+ return imu_builder.Finish();
}
double ADIS16470::ConvertValue32(absl::Span<const uint32_t> data,
diff --git a/frc971/wpilib/ADIS16470.h b/frc971/wpilib/ADIS16470.h
index 4c9917f..a87ca1b 100644
--- a/frc971/wpilib/ADIS16470.h
+++ b/frc971/wpilib/ADIS16470.h
@@ -9,6 +9,7 @@
#include "frc971/wpilib/ahal/DigitalSource.h"
#include "frc971/wpilib/ahal/SPI.h"
#include "frc971/wpilib/fpga_time_conversion.h"
+#include "frc971/wpilib/imu_batch_generated.h"
#include "frc971/wpilib/imu_generated.h"
namespace frc971 {
@@ -47,7 +48,8 @@
void DoInitializeStep();
// Processes a complete reading in read_data_.
- void ProcessReading();
+ flatbuffers::Offset<IMUValues> ProcessReading(
+ flatbuffers::FlatBufferBuilder *fbb);
// Converts a 32-bit value at data to a scaled output value where a value of 1
// corresponds to lsb_per_output.
@@ -74,7 +76,7 @@
}
aos::EventLoop *const event_loop_;
- aos::Sender<::frc971::IMUValues> imu_values_sender_;
+ aos::Sender<::frc971::IMUValuesBatch> imu_values_sender_;
aos::TimerHandler *const initialize_timer_;
frc::SPI *const spi_;
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index bcdd2ce..734a231 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -267,6 +267,17 @@
gen_reflections = 1,
)
+flatbuffer_cc_library(
+ name = "imu_batch_fbs",
+ srcs = [
+ "imu_batch.fbs",
+ ],
+ gen_reflections = 1,
+ includes = [
+ ":imu_fbs_includes",
+ ],
+)
+
cc_library(
name = "ADIS16470",
srcs = [
@@ -278,7 +289,9 @@
restricted_to = ["//tools:roborio"],
deps = [
":fpga_time_conversion",
+ ":imu_batch_fbs",
":imu_fbs",
+ "//aos/containers:sized_array",
"//aos/events:event_loop",
"//aos/time",
"//third_party:wpilib",
diff --git a/frc971/wpilib/imu_batch.fbs b/frc971/wpilib/imu_batch.fbs
new file mode 100644
index 0000000..8029ced
--- /dev/null
+++ b/frc971/wpilib/imu_batch.fbs
@@ -0,0 +1,9 @@
+include "frc971/wpilib/imu.fbs";
+
+namespace frc971;
+
+table IMUValuesBatch {
+ readings:[IMUValues];
+}
+
+root_type IMUValuesBatch;
diff --git a/y2020/BUILD b/y2020/BUILD
index 8beb799..58a99ce 100644
--- a/y2020/BUILD
+++ b/y2020/BUILD
@@ -169,6 +169,7 @@
visibility = ["//visibility:public"],
deps = [
"//aos/events:config",
+ "//frc971/control_loops/drivetrain:config",
],
)
for pi in [
diff --git a/y2020/control_loops/drivetrain/BUILD b/y2020/control_loops/drivetrain/BUILD
index 1bd760f..0e06057 100644
--- a/y2020/control_loops/drivetrain/BUILD
+++ b/y2020/control_loops/drivetrain/BUILD
@@ -110,6 +110,15 @@
],
)
+aos_config(
+ name = "replay_config",
+ src = "drivetrain_replay_config.json",
+ visibility = ["//visibility:public"],
+ deps = [
+ "//y2020:config",
+ ],
+)
+
cc_test(
name = "localizer_test",
srcs = ["localizer_test.cc"],
@@ -131,7 +140,7 @@
name = "drivetrain_replay_test",
srcs = ["drivetrain_replay_test.cc"],
data = [
- "//y2020:config.json",
+ ":replay_config.json",
"@drivetrain_replay//file:spinning_wheels_while_still.bfbs",
],
deps = [
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_config.json b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
new file mode 100644
index 0000000..987e55b
--- /dev/null
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_config.json
@@ -0,0 +1,13 @@
+{
+ "channels": [
+ {
+ "name": "/drivetrain",
+ "type": "frc971.IMUValues",
+ "frequency": 2000,
+ "source_node": "roborio"
+ }
+ ],
+ "imports": [
+ "../../y2020.json"
+ ]
+}
diff --git a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
index 6033184..4b02923 100644
--- a/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
+++ b/y2020/control_loops/drivetrain/drivetrain_replay_test.cc
@@ -23,7 +23,7 @@
DEFINE_string(
logfile, "external/drivetrain_replay/file/spinning_wheels_while_still.bfbs",
"Name of the logfile to read from.");
-DEFINE_string(config, "y2020/config.json",
+DEFINE_string(config, "y2020/control_loops/drivetrain/replay_config.json",
"Name of the config file to replay using.");
namespace y2020 {
@@ -67,7 +67,27 @@
config, drivetrain_event_loop_.get(), localizer_.get());
test_event_loop_ =
- reader_.event_loop_factory()->MakeEventLoop("drivetrain", roborio_);
+ reader_.event_loop_factory()->MakeEventLoop("drivetrain_test", roborio_);
+
+ // IMU readings used to be published out one at a time, but we now expect
+ // batches. Batch them up to upgrade the data.
+ imu_sender_ =
+ test_event_loop_->MakeSender<frc971::IMUValuesBatch>("/drivetrain");
+ test_event_loop_->MakeWatcher(
+ "/drivetrain", [this](const frc971::IMUValues &values) {
+ aos::Sender<frc971::IMUValuesBatch>::Builder builder =
+ imu_sender_.MakeBuilder();
+ flatbuffers::Offset<frc971::IMUValues> values_offsets =
+ aos::CopyFlatBuffer(&values, builder.fbb());
+ flatbuffers::Offset<
+ flatbuffers::Vector<flatbuffers::Offset<frc971::IMUValues>>>
+ values_offset = builder.fbb()->CreateVector(&values_offsets, 1);
+ frc971::IMUValuesBatch::Builder imu_values_batch_builder =
+ builder.MakeBuilder<frc971::IMUValuesBatch>();
+ imu_values_batch_builder.add_readings(values_offset);
+ builder.Send(imu_values_batch_builder.Finish());
+ });
+
status_fetcher_ = test_event_loop_->MakeFetcher<
frc971::control_loops::drivetrain::Status>("/drivetrain");
}
@@ -81,6 +101,7 @@
std::unique_ptr<frc971::control_loops::drivetrain::DrivetrainLoop>
drivetrain_;
std::unique_ptr<aos::EventLoop> test_event_loop_;
+ aos::Sender<frc971::IMUValuesBatch> imu_sender_;
aos::Fetcher<frc971::control_loops::drivetrain::Status> status_fetcher_;
};
diff --git a/y2020/y2020_pi1.json b/y2020/y2020_pi1.json
index f14996c..93d3ba9 100644
--- a/y2020/y2020_pi1.json
+++ b/y2020/y2020_pi1.json
@@ -58,7 +58,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi1",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "roborio",
+ "logger_nodes": ["roborio"],
"frequency": 25,
"max_size": 10000,
"destination_nodes": [
diff --git a/y2020/y2020_pi2.json b/y2020/y2020_pi2.json
index 82a2bc9..2046666 100644
--- a/y2020/y2020_pi2.json
+++ b/y2020/y2020_pi2.json
@@ -58,7 +58,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi2",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "roborio",
+ "logger_nodes": ["roborio"],
"frequency": 25,
"max_size": 10000,
"destination_nodes": [
diff --git a/y2020/y2020_pi3.json b/y2020/y2020_pi3.json
index 727e41d..fe8f980 100644
--- a/y2020/y2020_pi3.json
+++ b/y2020/y2020_pi3.json
@@ -58,7 +58,7 @@
"type": "frc971.vision.sift.ImageMatchResult",
"source_node": "pi3",
"logger": "LOCAL_AND_REMOTE_LOGGER",
- "logger_node": "roborio",
+ "logger_nodes": ["roborio"],
"frequency": 25,
"max_size": 10000,
"destination_nodes": [
diff --git a/y2020/y2020_roborio.json b/y2020/y2020_roborio.json
index 86d9b7e..0ab7c36 100644
--- a/y2020/y2020_roborio.json
+++ b/y2020/y2020_roborio.json
@@ -122,9 +122,10 @@
},
{
"name": "/drivetrain",
- "type": "frc971.IMUValues",
+ "type": "frc971.IMUValuesBatch",
"source_node": "roborio",
- "frequency": 2000,
+ "frequency": 200,
+ "max_size": 2000,
"num_senders": 2
},
{