Move PDP values out to a separate queue so the timestamps make sense etc
Change-Id: Iad5303c3446dc29f339f02af9c9c99dd7c75d8e0
diff --git a/frc971/wpilib/pdp_fetcher.cc b/frc971/wpilib/pdp_fetcher.cc
index 735036e..4e6be32 100644
--- a/frc971/wpilib/pdp_fetcher.cc
+++ b/frc971/wpilib/pdp_fetcher.cc
@@ -3,21 +3,14 @@
#include "aos/common/logging/queue_logging.h"
#include "aos/linux_code/init.h"
#include "aos/common/util/phased_loop.h"
+#include "frc971/wpilib/pdp_values.q.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");
+ ::std::unique_ptr<PowerDistributionPanel> pdp(new PowerDistributionPanel());
::aos::time::PhasedLoop phased_loop(::aos::time::Time::InMS(20),
::aos::time::Time::InMS(4));
@@ -29,29 +22,16 @@
LOG(DEBUG, "PDPFetcher skipped %d iterations\n", iterations - 1);
}
}
- {
- 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;
- }
+ auto message = pdp_values.MakeMessage();
+ message->voltage = pdp->GetVoltage();
+ message->temperature = pdp->GetTemperature();
+ message->power = pdp->GetTotalPower();
for (int i = 0; i < 16; ++i) {
- const double current = pdp_->GetCurrent(i);
- ::aos::MutexLocker locker(&values_lock_);
- pdp_values_.currents[i] = current;
+ message->currents[i] = pdp->GetCurrent(i);
}
- {
- ::aos::MutexLocker locker(&values_lock_);
- LOG_STRUCT(DEBUG, "finished fetching", pdp_values_);
+ LOG_STRUCT(DEBUG, "got", *message);
+ if (!message.Send()) {
+ LOG(WARNING, "sending pdp values failed\n");
}
}
}