Provide utilities for convenient plotting of channels
Implement a set of utilities (along with basic examples) to
make it so that we can readily make plots of individual channels
from logfiles (or live on the robot).
Change-Id: Ic648c9ccb9dcb73419dc2c8c4c395fdea0536110
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index e7c52bf..087c413 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -1,6 +1,7 @@
package(default_visibility = ["//visibility:public"])
-load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library")
+load("@npm_bazel_typescript//:defs.bzl", "ts_library")
+load("@com_github_google_flatbuffers//:build_defs.bzl", "flatbuffer_cc_library", "flatbuffer_ts_library")
load("//aos:config.bzl", "aos_config")
flatbuffer_cc_library(
@@ -286,6 +287,15 @@
target_compatible_with = ["@platforms//os:linux"],
)
+flatbuffer_ts_library(
+ name = "imu_batch_ts_fbs",
+ srcs = ["imu_batch.fbs"],
+ includes = [
+ ":imu_fbs_includes",
+ ],
+ target_compatible_with = ["@platforms//os:linux"],
+)
+
cc_library(
name = "ADIS16470",
srcs = [
@@ -422,3 +432,27 @@
"@com_github_google_glog//:glog",
],
)
+
+ts_library(
+ name = "imu_plot_utils",
+ srcs = ["imu_plot_utils.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":imu_batch_ts_fbs",
+ "//aos:configuration_ts_fbs",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:reflection_ts",
+ "@com_github_google_flatbuffers//ts:flatbuffers_ts",
+ ],
+)
+
+ts_library(
+ name = "imu_plotter",
+ srcs = ["imu_plotter.ts"],
+ target_compatible_with = ["@platforms//os:linux"],
+ deps = [
+ ":imu_plot_utils",
+ "//aos/network/www:aos_plotter",
+ "//aos/network/www:proxy",
+ ],
+)
diff --git a/frc971/wpilib/imu_plot_utils.ts b/frc971/wpilib/imu_plot_utils.ts
new file mode 100644
index 0000000..4e6c371
--- /dev/null
+++ b/frc971/wpilib/imu_plot_utils.ts
@@ -0,0 +1,34 @@
+// This script provides a basic utility for de-batching the IMUValues
+// message. See imu_plotter.ts for usage.
+import * as configuration from 'org_frc971/aos/configuration_generated';
+import * as imu from 'org_frc971/frc971/wpilib/imu_batch_generated';
+import {MessageHandler, TimestampedMessage} from 'org_frc971/aos/network/www/aos_plotter';
+import {Table} from 'org_frc971/aos/network/www/reflection';
+import {ByteBuffer} from 'org_frc971/external/com_github_google_flatbuffers/ts/byte-buffer';
+
+import Schema = configuration.reflection.Schema;
+import IMUValuesBatch = imu.frc971.IMUValuesBatch;
+import IMUValues = imu.frc971.IMUValues;
+
+export class ImuMessageHandler extends MessageHandler {
+ constructor(private readonly schema: Schema) {
+ super(schema);
+ }
+ addMessage(data: Uint8Array, time: number): void {
+ const batch = IMUValuesBatch.getRootAsIMUValuesBatch(
+ new ByteBuffer(data) as unknown as flatbuffers.ByteBuffer);
+ for (let ii = 0; ii < batch.readingsLength(); ++ii) {
+ const message = batch.readings(ii);
+ const table = Table.getNamedTable(
+ message.bb as unknown as ByteBuffer, this.schema, 'frc971.IMUValues',
+ message.bb_pos);
+ if (this.parser.readScalar(table, "monotonic_timestamp_ns") == null) {
+ console.log('Ignoring unpopulated IMU values: ');
+ console.log(this.parser.toObject(table));
+ continue;
+ }
+ this.messages.push(new TimestampedMessage(
+ table, message.monotonicTimestampNs().toFloat64() * 1e-9));
+ }
+ }
+}
diff --git a/frc971/wpilib/imu_plotter.ts b/frc971/wpilib/imu_plotter.ts
new file mode 100644
index 0000000..af23ed9
--- /dev/null
+++ b/frc971/wpilib/imu_plotter.ts
@@ -0,0 +1,80 @@
+// Provides a basic plot for debugging IMU-related issues on a robot.
+import {AosPlotter} from 'org_frc971/aos/network/www/aos_plotter';
+import {ImuMessageHandler} from 'org_frc971/frc971/wpilib/imu_plot_utils';
+import * as proxy from 'org_frc971/aos/network/www/proxy';
+
+import Connection = proxy.Connection;
+
+export function plotImu(conn: Connection, element: Element): void {
+ const width = 900;
+ const height = 400;
+ const aosPlotter = new AosPlotter(conn);
+
+ const accelPlot = aosPlotter.addPlot(element, [0, 0], [width, height]);
+ accelPlot.plot.getAxisLabels().setTitle('Accelerometer Readings');
+ accelPlot.plot.getAxisLabels().setYLabel('Acceleration (g)');
+ accelPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const drivetrainStatus = aosPlotter.addMessageSource(
+ '/drivetrain', 'frc971.control_loops.drivetrain.Status');
+
+ const imu = aosPlotter.addRawMessageSource(
+ '/drivetrain', 'frc971.IMUValuesBatch',
+ new ImuMessageHandler(conn.getSchema('frc971.IMUValuesBatch')));
+
+ const accelX = accelPlot.addMessageLine(imu, ['accelerometer_x']);
+ accelX.setColor([1, 0, 0]);
+ const accelY = accelPlot.addMessageLine(imu, ['accelerometer_y']);
+ accelY.setColor([0, 1, 0]);
+ const accelZ = accelPlot.addMessageLine(imu, ['accelerometer_z']);
+ accelZ.setColor([0, 0, 1]);
+
+ const gyroPlot = aosPlotter.addPlot(element, [0, height], [width, height]);
+ gyroPlot.plot.getAxisLabels().setTitle('Gyro Readings');
+ gyroPlot.plot.getAxisLabels().setYLabel('Angular Velocity (rad / sec)');
+ gyroPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ const gyroZeroX =
+ gyroPlot.addMessageLine(drivetrainStatus, ['zeroing', 'gyro_x_average']);
+ gyroZeroX.setColor([1, 0, 0]);
+ gyroZeroX.setPointSize(0);
+ gyroZeroX.setLabel('Gyro X Zero');
+ const gyroZeroY =
+ gyroPlot.addMessageLine(drivetrainStatus, ['zeroing', 'gyro_y_average']);
+ gyroZeroY.setColor([0, 1, 0]);
+ gyroZeroY.setPointSize(0);
+ gyroZeroY.setLabel('Gyro Y Zero');
+ const gyroZeroZ =
+ gyroPlot.addMessageLine(drivetrainStatus, ['zeroing', 'gyro_z_average']);
+ gyroZeroZ.setColor([0, 0, 1]);
+ gyroZeroZ.setPointSize(0);
+ gyroZeroZ.setLabel('Gyro Z Zero');
+
+ const gyroX = gyroPlot.addMessageLine(imu, ['gyro_x']);
+ gyroX.setColor([1, 0, 0]);
+ const gyroY = gyroPlot.addMessageLine(imu, ['gyro_y']);
+ gyroY.setColor([0, 1, 0]);
+ const gyroZ = gyroPlot.addMessageLine(imu, ['gyro_z']);
+ gyroZ.setColor([0, 0, 1]);
+
+ const tempPlot = aosPlotter.addPlot(element, [0, height * 2], [width, height / 2]);
+ tempPlot.plot.getAxisLabels().setTitle('IMU Temperature');
+ tempPlot.plot.getAxisLabels().setYLabel('Temperature (deg C)');
+ tempPlot.plot.getAxisLabels().setXLabel('Monotonic Reading Time (sec)');
+
+ tempPlot.addMessageLine(imu, ['temperature']);
+
+ const statePlot = aosPlotter.addPlot(element, [0, height * 2.5], [width, height / 2]);
+ statePlot.plot.getAxisLabels().setTitle('IMU State');
+ statePlot.plot.getAxisLabels().setXLabel('Monotonic Sent Time (sec)');
+ statePlot.plot.setDefaultYRange([-0.1, 1.1]);
+
+ const zeroedLine =
+ statePlot.addMessageLine(drivetrainStatus, ['zeroing', 'zeroed']);
+ zeroedLine.setColor([1, 0, 0]);
+ zeroedLine.setDrawLine(false);
+ const faultedLine =
+ statePlot.addMessageLine(drivetrainStatus, ['zeroing', 'faulted']);
+ faultedLine.setColor([0, 1, 0]);
+ faultedLine.setPointSize(0);
+}