Directly log gyro angular velocity.
Change-Id: Ia45bf8c0c54986919a33495eda48284bdacf48b1
diff --git a/frc971/queues/gyro.q b/frc971/queues/gyro.q
index 7b234a1..9179d5d 100644
--- a/frc971/queues/gyro.q
+++ b/frc971/queues/gyro.q
@@ -3,7 +3,10 @@
message GyroReading {
// Positive is counter-clockwise (Austin says "it's Positive").
// Right-hand coordinate system around the Z-axis going up.
+ // The angle is measured in radians.
double angle;
+ // The angular velocity in radians/sec
+ double velocity;
};
queue GyroReading gyro_reading;
diff --git a/frc971/wpilib/gyro_interface.h b/frc971/wpilib/gyro_interface.h
index baba248..05b9bff 100644
--- a/frc971/wpilib/gyro_interface.h
+++ b/frc971/wpilib/gyro_interface.h
@@ -27,7 +27,7 @@
// Returns all of the error bits in the "footer" from value.
uint8_t ExtractErrors(uint32_t value) { return (value >> 1) & 0x7F; }
- // Returns the anglular rate contained in value.
+ // Returns the anglular rate contained in value in radians/sec.
double ExtractAngle(uint32_t value);
// Performs a transaction with the gyro.
diff --git a/frc971/wpilib/gyro_sender.cc b/frc971/wpilib/gyro_sender.cc
index d3b9500..d136b9c 100644
--- a/frc971/wpilib/gyro_sender.cc
+++ b/frc971/wpilib/gyro_sender.cc
@@ -91,13 +91,14 @@
continue;
}
- const double new_angle =
- gyro_.ExtractAngle(result) / static_cast<double>(kReadingRate);
+ const double angle_rate = gyro_.ExtractAngle(result);
+ const double new_angle = angle_rate / static_cast<double>(kReadingRate);
auto message = ::frc971::sensors::gyro_reading.MakeMessage();
if (zeroed) {
angle += new_angle;
angle += zero_offset;
message->angle = angle;
+ message->velocity = angle_rate + zero_offset * kReadingRate;
LOG_STRUCT(DEBUG, "sending", *message);
message.Send();
} else {
@@ -107,8 +108,10 @@
// Some kind of indicator light?
{
message->angle = new_angle;
+ message->velocity = angle_rate;
LOG_STRUCT(DEBUG, "collected while zeroing", *message);
message->angle = 0.0;
+ message->velocity = 0.0;
message.Send();
}
zeroing_data[zeroing_index] = new_angle;