Add fbs files and config modifications for localizer
Creates a LocalizerStatus channel for the status, a LocalizerOutput for
the minimal output to be forwarded to the roborio for it to correct its
localizer, and adds an IMUValuesBatch channel on an imu pi.
Does not yet remove the old IMUValuesBatch channel in y2022.
Change-Id: I217f6a9c1f482c6db1a36b8a37eabd0e7bb7e2a4
Signed-off-by: James Kuszmaul <jabukuszmaul+collab@gmail.com>
diff --git a/y2022/BUILD b/y2022/BUILD
index 78cb86c..9c2d150 100644
--- a/y2022/BUILD
+++ b/y2022/BUILD
@@ -58,6 +58,7 @@
target_compatible_with = ["@platforms//os:linux"],
visibility = ["//visibility:public"],
deps = [
+ ":config_imu",
":config_logger",
":config_pi1",
":config_pi2",
@@ -99,6 +100,25 @@
]
aos_config(
+ name = "config_imu",
+ src = "y2022_imu.json",
+ flatbuffers = [
+ "//aos/network:message_bridge_client_fbs",
+ "//aos/network:message_bridge_server_fbs",
+ "//aos/network:timestamp_fbs",
+ "//aos/network:remote_message_fbs",
+ "//y2022/control_loops/localizer:localizer_status_fbs",
+ "//y2022/control_loops/localizer:localizer_output_fbs",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+ deps = [
+ "//aos/events:config",
+ "//frc971/control_loops/drivetrain:config",
+ ],
+)
+
+aos_config(
name = "config_logger",
src = "y2022_logger.json",
flatbuffers = [
diff --git a/y2022/control_loops/localizer/BUILD b/y2022/control_loops/localizer/BUILD
new file mode 100644
index 0000000..d5e642f
--- /dev/null
+++ b/y2022/control_loops/localizer/BUILD
@@ -0,0 +1,33 @@
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("//aos:flatbuffers.bzl", "cc_static_flatbuffer")
+
+flatbuffer_cc_library(
+ name = "localizer_output_fbs",
+ srcs = [
+ "localizer_output.fbs",
+ ],
+ gen_reflections = True,
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+flatbuffer_cc_library(
+ name = "localizer_status_fbs",
+ srcs = [
+ "localizer_status.fbs",
+ ],
+ gen_reflections = True,
+ includes = [
+ "//frc971/control_loops:control_loops_fbs_includes",
+ "//frc971/control_loops/drivetrain:drivetrain_status_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+ visibility = ["//visibility:public"],
+)
+
+cc_static_flatbuffer(
+ name = "localizer_schema",
+ function = "frc971::controls::LocalizerStatusSchema",
+ target = ":localizer_status_fbs_reflection_out",
+ visibility = ["//visibility:public"],
+)
diff --git a/y2022/control_loops/localizer/localizer_output.fbs b/y2022/control_loops/localizer/localizer_output.fbs
new file mode 100644
index 0000000..078d723
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_output.fbs
@@ -0,0 +1,17 @@
+namespace frc971.controls;
+
+// This provides a minimal output from the localizer that can be forwarded to
+// the roborio and used for corrections to its (simpler) localizer.
+
+table LocalizerOutput {
+ // Timestamp (on the source node) that this sample corresponds with. This
+ // may be older than the sent time to account for delays in sensor readings.
+ monotonic_timestamp_ns:int64 (id: 0);
+ // Current x/y position estimate, in meters.
+ x:double (id: 1);
+ y:double (id: 2);
+ // Current heading, in radians.
+ theta:double (id: 3);
+}
+
+root_type LocalizerOutput;
diff --git a/y2022/control_loops/localizer/localizer_status.fbs b/y2022/control_loops/localizer/localizer_status.fbs
new file mode 100644
index 0000000..6771c5f
--- /dev/null
+++ b/y2022/control_loops/localizer/localizer_status.fbs
@@ -0,0 +1,87 @@
+include "frc971/control_loops/drivetrain/drivetrain_status.fbs";
+
+namespace frc971.controls;
+
+// Stores the state associated with the acceleration-based modelling.
+table AccelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Velocity in X/Y directions, in m/s.
+ velocity_x:double (id: 3);
+ velocity_y:double (id: 4);
+}
+
+// Stores the state associated with the drivetrain model-based state.
+// This model assumes zero lateral motion of the drivetrain.
+table ModelBasedState {
+ // x/y position, in meters.
+ x:double (id: 0);
+ y:double (id: 1);
+ // heading, in radians.
+ theta:double (id: 2);
+ // Expected encoder reading for the left side of the drivetrain, in meters.
+ left_encoder:double (id: 3);
+ // Modelled velocity of the left side of the drivetrain, in meters / second.
+ left_velocity:double (id: 4);
+ // Estimated voltage error, in volts.
+ left_voltage_error:double (id: 5);
+ // Same as the left_* fields, but for the right side of the drivetrain.
+ right_encoder:double (id: 6);
+ right_velocity:double (id: 7);
+ right_voltage_error:double (id: 8);
+}
+
+table ModelBasedStatus {
+ // Current acceleration and model-based states. Depending on using_model,
+ // one of these will be the ground-truth and the other will be calculated
+ // based on it. E.g. if using_model is true, then model_state will be
+ // populated as you'd expect, while accel_state will be populated to be
+ // consistent with model_state (e.g., no lateral motion).
+ accel_state:AccelBasedState (id: 0);
+ model_state:ModelBasedState (id: 1);
+ // using_model indicates whether we are currently in in model-based or
+ // accelerometer-based estimation.
+ using_model:bool (id: 2);
+ // Current residual associated with the amount of inconsistency between
+ // the two models. Will be zero if the drivetrain model is perfectly
+ // consistent with the IMU readings.
+ residual:double (id: 3);
+ // Status from the down estimator.
+ down_estimator:frc971.control_loops.drivetrain.DownEstimatorState (id: 4);
+ // Current ground-truth for x/y/theta. Should match those present in *_state.
+ x:double (id: 5);
+ y:double (id: 6);
+ theta:double (id: 7);
+ // Current accelerations implied by the current accelerometer + down estimator
+ // + yaw readings.
+ implied_accel_x:double (id: 8);
+ implied_accel_y:double (id: 9);
+ implied_accel_z:double (id: 10);
+ // oldest_* are the oldest surviving branches of the model that have just been
+ // running purely on one model.
+ oldest_accel_state:AccelBasedState (id: 11);
+ oldest_model_state:ModelBasedState (id: 12);
+ // Filtered version of the residual field--this is what is actually used by
+ // the code for determining when to swap between modes.
+ filtered_residual:double (id: 13);
+ // Components of the residual. Useful for debugging.
+ velocity_residual:double (id: 14);
+ accel_residual:double (id: 15);
+ theta_rate_residual:double (id: 16);
+ // Number of times we have missed an IMU reading. Should never increase except
+ // *maybe* during startup.
+ clock_resets:int (id: 17);
+}
+
+table LocalizerStatus {
+ model_based:ModelBasedStatus (id: 0);
+ // Whether the IMU is zeroed or not.
+ zeroed:bool (id: 1);
+ // Whether the IMU zeroing is faulted or not.
+ faulted_zero:bool (id: 2);
+}
+
+root_type LocalizerStatus;
diff --git a/y2022/y2022.json b/y2022/y2022.json
index 010c675..4fafa8e 100644
--- a/y2022/y2022.json
+++ b/y2022/y2022.json
@@ -18,6 +18,7 @@
"y2022_pi3.json",
"y2022_pi4.json",
"y2022_pi5.json",
+ "y2022_imu.json",
"y2022_logger.json"
]
}
diff --git a/y2022/y2022_imu.json b/y2022/y2022_imu.json
new file mode 100644
index 0000000..da0e4cb
--- /dev/null
+++ b/y2022/y2022_imu.json
@@ -0,0 +1,381 @@
+{
+ "channels": [
+ {
+ "name": "/imu/aos",
+ "type": "aos.timing.Report",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "max_size": 4096
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.logging.LogMessageFbs",
+ "source_node": "imu",
+ "frequency": 200,
+ "num_senders": 20
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.Status",
+ "source_node": "imu",
+ "frequency": 50,
+ "num_senders": 20,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ServerStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.ClientStatistics",
+ "source_node": "imu",
+ "frequency": 10,
+ "num_senders": 2
+ },
+ {
+ "name": "/imu/aos",
+ "type": "aos.message_bridge.Timestamp",
+ "source_node": "imu",
+ "frequency": 15,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "logger_nodes": [
+ "roborio",
+ "logger"
+ ],
+ "max_size": 200,
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 1,
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 1,
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "logger",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/logger/aos",
+ "type": "aos.starter.Status",
+ "source_node": "logger",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-starter-Status",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.StarterRpc",
+ "source_node": "roborio",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-StarterRpc",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/roborio/aos",
+ "type": "aos.starter.Status",
+ "source_node": "roborio",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-starter-Status",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerStatus",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 2000,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerStatus",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 2200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.controls.LocalizerOutput",
+ "source_node": "imu",
+ "frequency": 200,
+ "max_size": 200,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "roborio",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ },
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/roborio/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-controls-LocalizerOutput",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.IMUValuesBatch",
+ "source_node": "imu",
+ "frequency": 2200,
+ "max_size": 1000,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-IMUValuesBatch",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 2200,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
+ "name": "/localizer",
+ "type": "frc971.control_loops.drivetrain.Position",
+ "source_node": "imu",
+ "frequency": 200,
+ "max_size": 200,
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "logger",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/imu/aos/remote_timestamps/logger/localizer/frc971-control_loops-drivetrain-Position",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "imu",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
+ }
+ ],
+ "applications": [
+ {
+ "name": "message_bridge_client",
+ "executable_name": "message_bridge_client",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "message_bridge_server",
+ "executable_name": "message_bridge_server",
+ "nodes": [
+ "imu"
+ ]
+ },
+ {
+ "name": "web_proxy",
+ "executable_name": "web_proxy_main",
+ "nodes": [
+ "imu"
+ ]
+ }
+ ],
+ "maps": [
+ {
+ "match": {
+ "name": "/aos*",
+ "source_node": "imu"
+ },
+ "rename": {
+ "name": "/imu/aos"
+ }
+ }
+ ],
+ "nodes": [
+ {
+ "name": "imu",
+ "hostname": "imu",
+ "hostnames": [
+ "pi-971-7",
+ "pi-7971-7",
+ "pi-8971-7",
+ "pi-9971-7"
+ ],
+ "port": 9971
+ },
+ {
+ "name": "logger"
+ },
+ {
+ "name": "roborio"
+ }
+ ]
+}
diff --git a/y2022/y2022_logger.json b/y2022/y2022_logger.json
index 1c71234..67f2822 100644
--- a/y2022/y2022_logger.json
+++ b/y2022/y2022_logger.json
@@ -268,6 +268,15 @@
]
},
{
+ "name": "imu",
+ "priority": 1,
+ "time_to_live": 5000000,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "logger"
+ ]
+ },
+ {
"name": "roborio",
"priority": 1,
"time_to_live": 5000000,
@@ -288,6 +297,15 @@
"max_size": 200
},
{
+ "name": "/logger/aos/remote_timestamps/imu/logger/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "logger",
+ "logger": "NOT_LOGGED",
+ "frequency": 20,
+ "num_senders": 2,
+ "max_size": 200
+ },
+ {
"name": "/logger/aos/remote_timestamps/pi1/logger/aos/aos-message_bridge-Timestamp",
"type": "aos.message_bridge.RemoteMessage",
"source_node": "logger",
@@ -474,6 +492,9 @@
"name": "roborio"
},
{
+ "name": "imu"
+ },
+ {
"name": "pi4"
},
{
diff --git a/y2022/y2022_roborio.json b/y2022/y2022_roborio.json
index 24144a8..41f5baa 100644
--- a/y2022/y2022_roborio.json
+++ b/y2022/y2022_roborio.json
@@ -138,12 +138,19 @@
"max_size": 208
},
{
+ "name": "/roborio/aos/remote_timestamps/imu/roborio/aos/aos-message_bridge-Timestamp",
+ "type": "aos.message_bridge.RemoteMessage",
+ "frequency": 20,
+ "source_node": "roborio",
+ "max_size": 208
+ },
+ {
"name": "/roborio/aos",
"type": "aos.message_bridge.Timestamp",
"source_node": "roborio",
"frequency": 15,
"num_senders": 2,
- "max_size": 304,
+ "max_size": 512,
"destination_nodes": [
{
"name": "pi1",
@@ -189,6 +196,15 @@
"roborio"
],
"time_to_live": 5000000
+ },
+ {
+ "name": "imu",
+ "priority": 1,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "roborio"
+ ],
+ "time_to_live": 5000000
}
]
},
@@ -282,7 +298,28 @@
"source_node": "roborio",
"frequency": 200,
"max_size": 80,
- "num_senders": 2
+ "num_senders": 2,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-Output",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
},
{
"name": "/drivetrain",
@@ -297,7 +334,28 @@
"type": "frc971.control_loops.drivetrain.LocalizerControl",
"source_node": "roborio",
"frequency": 200,
- "max_size": 96
+ "max_size": 96,
+ "logger": "LOCAL_AND_REMOTE_LOGGER",
+ "destination_nodes": [
+ {
+ "name": "imu",
+ "priority": 5,
+ "timestamp_logger": "LOCAL_AND_REMOTE_LOGGER",
+ "timestamp_logger_nodes": [
+ "imu"
+ ],
+ "time_to_live": 5000000
+ }
+ ]
+ },
+ {
+ "name": "/roborio/aos/remote_timestamps/imu/drivetrain/frc971-control_loops-drivetrain-LocalizerControl",
+ "type": "aos.message_bridge.RemoteMessage",
+ "source_node": "roborio",
+ "logger": "NOT_LOGGED",
+ "frequency": 200,
+ "num_senders": 2,
+ "max_size": 200
},
{
"name": "/drivetrain",
@@ -435,6 +493,9 @@
"port": 9971
},
{
+ "name": "imu"
+ },
+ {
"name": "logger"
},
{