Merge branch 'sensor-data-timing' into hardware-interface-integration
Conflicts:
aos/common/time.h
diff --git a/frc971/input/sensor_unpacker.cc b/frc971/input/sensor_unpacker.cc
new file mode 100644
index 0000000..8695785
--- /dev/null
+++ b/frc971/input/sensor_unpacker.cc
@@ -0,0 +1,42 @@
+#include "frc971/input/sensor_unpacker.h"
+
+#include <arpa/inet.h>
+
+#include "aos/common/inttypes.h"
+
+#include "frc971/control_loops/DriveTrain.q.h"
+
+#define M_PI 3.14159265358979323846
+
+using ::frc971::control_loops::drivetrain;
+
+namespace frc971 {
+namespace {
+
+inline double drivetrain_translate(int32_t in) {
+ // TODO(2013) fix the math
+ return static_cast<double>(in) / (256.0 * 4.0 * 44.0 / 32.0) *
+ (3.5 * 2.54 / 100.0 * M_PI);
+}
+
+} // namespace
+
+SensorUnpacker::SensorUnpacker() {}
+
+void SensorUnpacker::UnpackFrom(sensor_values *values) {
+ for (size_t i = 0; i < sizeof(values->encoders) / sizeof(values->encoders[0]); ++i) {
+ values->encoders[i] = ntohl(values->encoders[i]);
+ }
+
+ // TODO(aschuh): Convert to meters.
+ const double left_encoder = drivetrain_translate(
+ values->drive_left_encoder);
+ const double right_encoder = drivetrain_translate(
+ values->drive_right_encoder);
+ drivetrain.position.MakeWithBuilder()
+ .left_encoder(left_encoder)
+ .right_encoder(right_encoder)
+ .Send();
+}
+
+} // namespace frc971