Make python plotting tool handle batched IMU values
Change-Id: If9bc94ec2472b5ba37cdc8254d4bb97f968de463
diff --git a/frc971/analysis/plot.py b/frc971/analysis/plot.py
index ecb23b3..efe86f4 100644
--- a/frc971/analysis/plot.py
+++ b/frc971/analysis/plot.py
@@ -106,51 +106,60 @@
last_1000_gyros = np.zeros((buffer_len, 3))
gyros_next_row = 0
for entry in self.data['IMU']:
- accel_x = 'accelerometer_x'
- accel_y = 'accelerometer_y'
- accel_z = 'accelerometer_z'
- gyro_x = 'gyro_x'
- gyro_y = 'gyro_y'
- gyro_z = 'gyro_z'
- msg = entry[2]
- new_msg = {}
- if accel_x in msg and accel_y in msg and accel_x in msg:
- last_1000_accels[accels_next_row, :] = [
- msg[accel_x], msg[accel_y], msg[accel_z]
- ]
- total_acceleration = np.linalg.norm(
- last_1000_accels[accels_next_row, :])
- new_msg['total_acceleration'] = total_acceleration
+ for msg in entry[2]['readings']:
+ accel_x = 'accelerometer_x'
+ accel_y = 'accelerometer_y'
+ accel_z = 'accelerometer_z'
+ gyro_x = 'gyro_x'
+ gyro_y = 'gyro_y'
+ gyro_z = 'gyro_z'
+ temp = 'temperature'
+ new_msg = {}
+ if temp in msg:
+ new_msg[temp] = msg[temp]
+ if accel_x in msg and accel_y in msg and accel_x in msg:
+ last_1000_accels[accels_next_row, :] = [
+ msg[accel_x], msg[accel_y], msg[accel_z]
+ ]
+ total_acceleration = np.linalg.norm(
+ last_1000_accels[accels_next_row, :])
+ new_msg['total_acceleration'] = total_acceleration
- accels_next_row += 1
- accels_next_row = accels_next_row % buffer_len
- std_accels = np.std(last_1000_accels, axis=0)
- new_msg['accel_x_rolling_std'] = std_accels[0]
- new_msg['accel_y_rolling_std'] = std_accels[1]
- new_msg['accel_z_rolling_std'] = std_accels[2]
- mean_accels = np.mean(last_1000_accels, axis=0)
- new_msg['accel_x_rolling_mean'] = mean_accels[0]
- new_msg['accel_y_rolling_mean'] = mean_accels[1]
- new_msg['accel_z_rolling_mean'] = mean_accels[2]
- if gyro_x in msg and gyro_y in msg and gyro_z in msg:
- last_1000_gyros[gyros_next_row, :] = [
- msg[gyro_x], msg[gyro_y], msg[gyro_z]
- ]
- gyros_next_row += 1
- gyros_next_row = gyros_next_row % buffer_len
- std_gyros = np.std(last_1000_gyros, axis=0)
- new_msg['gyro_x_rolling_std'] = std_gyros[0]
- new_msg['gyro_y_rolling_std'] = std_gyros[1]
- new_msg['gyro_z_rolling_std'] = std_gyros[2]
- mean_gyros = np.mean(last_1000_gyros, axis=0)
- new_msg['gyro_x_rolling_mean'] = mean_gyros[0]
- new_msg['gyro_y_rolling_mean'] = mean_gyros[1]
- new_msg['gyro_z_rolling_mean'] = mean_gyros[2]
- timestamp = 'monotonic_timestamp_ns'
- if timestamp in msg:
- timestamp_sec = msg[timestamp] * 1e-9
- new_msg['monotonic_timestamp_sec'] = timestamp_sec
- entries.append((entry[0], entry[1], new_msg))
+ accels_next_row += 1
+ accels_next_row = accels_next_row % buffer_len
+ std_accels = np.std(last_1000_accels, axis=0)
+ new_msg['accel_x_rolling_std'] = std_accels[0]
+ new_msg['accel_y_rolling_std'] = std_accels[1]
+ new_msg['accel_z_rolling_std'] = std_accels[2]
+ mean_accels = np.mean(last_1000_accels, axis=0)
+ new_msg['accel_x_rolling_mean'] = mean_accels[0]
+ new_msg['accel_y_rolling_mean'] = mean_accels[1]
+ new_msg['accel_z_rolling_mean'] = mean_accels[2]
+ new_msg[accel_x] = msg[accel_x]
+ new_msg[accel_y] = msg[accel_y]
+ new_msg[accel_z] = msg[accel_z]
+ if gyro_x in msg and gyro_y in msg and gyro_z in msg:
+ last_1000_gyros[gyros_next_row, :] = [
+ msg[gyro_x], msg[gyro_y], msg[gyro_z]
+ ]
+ gyros_next_row += 1
+ gyros_next_row = gyros_next_row % buffer_len
+ std_gyros = np.std(last_1000_gyros, axis=0)
+ new_msg['gyro_x_rolling_std'] = std_gyros[0]
+ new_msg['gyro_y_rolling_std'] = std_gyros[1]
+ new_msg['gyro_z_rolling_std'] = std_gyros[2]
+ mean_gyros = np.mean(last_1000_gyros, axis=0)
+ new_msg['gyro_x_rolling_mean'] = mean_gyros[0]
+ new_msg['gyro_y_rolling_mean'] = mean_gyros[1]
+ new_msg['gyro_z_rolling_mean'] = mean_gyros[2]
+ new_msg[gyro_x] = msg[gyro_x]
+ new_msg[gyro_y] = msg[gyro_y]
+ new_msg[gyro_z] = msg[gyro_z]
+ timestamp = 'monotonic_timestamp_ns'
+ if timestamp in msg:
+ timestamp_sec = msg[timestamp] * 1e-9
+ new_msg['monotonic_timestamp_sec'] = timestamp_sec
+ entries.append((entry[0], entry[1], new_msg))
if 'CalcIMU' in self.data:
raise RuntimeError('CalcIMU is already a member of data.')
self.data['CalcIMU'] = entries
diff --git a/frc971/analysis/plot_configs/gyro.pb b/frc971/analysis/plot_configs/gyro.pb
index 790fc55..70c6480 100644
--- a/frc971/analysis/plot_configs/gyro.pb
+++ b/frc971/analysis/plot_configs/gyro.pb
@@ -1,6 +1,6 @@
channel {
name: "/drivetrain"
- type: "frc971.IMUValues"
+ type: "frc971.IMUValuesBatch"
alias: "IMU"
}
@@ -8,7 +8,7 @@
axes {
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "gyro_x"
}
x_signal {
@@ -18,7 +18,7 @@
}
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "gyro_y"
}
x_signal {
@@ -28,7 +28,7 @@
}
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "gyro_z"
}
x_signal {
@@ -51,7 +51,7 @@
}
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "accelerometer_x"
}
x_signal {
@@ -61,7 +61,7 @@
}
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "accelerometer_y"
}
x_signal {
@@ -71,7 +71,7 @@
}
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "accelerometer_z"
}
x_signal {
@@ -225,7 +225,7 @@
axes {
line {
y_signal {
- channel: "IMU"
+ channel: "CalcIMU"
field: "temperature"
}
x_signal {
diff --git a/frc971/analysis/plot_configs/localizer.pb b/frc971/analysis/plot_configs/localizer.pb
index 930d63d..7b8d440 100644
--- a/frc971/analysis/plot_configs/localizer.pb
+++ b/frc971/analysis/plot_configs/localizer.pb
@@ -119,6 +119,18 @@
line {
y_signal {
channel: "DrivetrainStatus"
+ field: "trajectory_logging.x"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "trajectory_logging.y"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
field: "localizer.x"
}
}
@@ -266,6 +278,18 @@
line {
y_signal {
channel: "DrivetrainStatus"
+ field: "left_voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
+ field: "right_voltage_error"
+ }
+ }
+ line {
+ y_signal {
+ channel: "DrivetrainStatus"
field: "localizer.left_voltage_error"
}
}