Move PDP measurement fetching to a separate thread
Stupid CAN messages are too slow for being inline with the normal
path...
Change-Id: I160a24eb3216fc54083df576555b5bcad062ceb8
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
new file mode 100644
index 0000000..1c87107
--- /dev/null
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -0,0 +1,52 @@
+#include "frc971/wpilib/pdp_fetcher.h"
+
+#include "aos/common/logging/queue_logging.h"
+#include "aos/linux_code/init.h"
+
+namespace frc971 {
+namespace wpilib {
+
+PDPFetcher::PDPFetcher() : pdp_(new PowerDistributionPanel()) {
+ pdp_values_.Zero();
+}
+
+void PDPFetcher::GetValues(::aos::PDPValues *pdp_values) {
+ ::aos::MutexLocker locker(&values_lock_);
+ *pdp_values = pdp_values_;
+}
+
+void PDPFetcher::operator()() {
+ ::aos::SetCurrentThreadName("PDPFetcher");
+ // Something in WPILib blocks for long periods of time in here, so it's not
+ // actually a busy loop like it looks. It seems to somehow be related to
+ // joystick packets.
+ while (true) {
+ {
+ const double voltage = pdp_->GetVoltage();
+ ::aos::MutexLocker locker(&values_lock_);
+ pdp_values_.voltage = voltage;
+ }
+ {
+ const double temperature = pdp_->GetTemperature();
+ ::aos::MutexLocker locker(&values_lock_);
+ pdp_values_.temperature = temperature;
+ }
+ {
+ const double power = pdp_->GetTotalPower();
+ ::aos::MutexLocker locker(&values_lock_);
+ pdp_values_.power = power;
+ }
+ for (int i = 0; i < 16; ++i) {
+ const double current = pdp_->GetCurrent(i);
+ ::aos::MutexLocker locker(&values_lock_);
+ pdp_values_.currents[i] = current;
+ }
+ {
+ ::aos::MutexLocker locker(&values_lock_);
+ LOG_STRUCT(DEBUG, "finished fetching", pdp_values_);
+ }
+ }
+}
+
+} // namespace wpilib
+} // namespace frc971