Move PDP values out to a separate queue so the timestamps make sense etc
Change-Id: Iad5303c3446dc29f339f02af9c9c99dd7c75d8e0
diff --git a/frc971/wpilib/BUILD b/frc971/wpilib/BUILD
index afa9a4e..0e31d83 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -170,7 +170,13 @@
'//aos/common/messages:robot_state',
'//aos/externals:wpilib',
'//aos/common/logging:queue_logging',
- ':pdp_fetcher',
+ ],
+)
+
+queue_library(
+ name = 'pdp_values',
+ srcs = [
+ 'pdp_values.q',
],
)
@@ -183,7 +189,7 @@
'pdp_fetcher.h',
],
deps = [
- '//aos/common/messages:robot_state',
+ ':pdp_values',
'//aos/externals:wpilib',
'//aos/common/logging:queue_logging',
'//aos/linux_code:init',
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");
}
}
}
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
index 8271f2b..5979f96 100644
--- a/frc971/wpilib/pdp_fetcher.h
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -4,9 +4,6 @@
#include <memory>
#include <atomic>
-#include "aos/common/messages/robot_state.q.h"
-#include "aos/common/mutex.h"
-
#include "PowerDistributionPanel.h"
namespace frc971 {
@@ -16,23 +13,12 @@
// separate thread.
class PDPFetcher {
public:
- PDPFetcher();
-
void Quit() { run_ = false; }
- // Retrieves the latest set of values and stores it in *pdp_values.
- // This is safe to call from any thread.
- void GetValues(::aos::PDPValues *pdp_values);
-
// To be called by a ::std::thread.
void operator()();
private:
- const ::std::unique_ptr<PowerDistributionPanel> pdp_;
-
- ::aos::PDPValues pdp_values_;
- ::aos::Mutex values_lock_;
-
::std::atomic<bool> run_{true};
};
diff --git a/frc971/wpilib/pdp_values.q b/frc971/wpilib/pdp_values.q
new file mode 100644
index 0000000..bda1c49
--- /dev/null
+++ b/frc971/wpilib/pdp_values.q
@@ -0,0 +1,11 @@
+package frc971;
+
+// Values retrieved from the PDP.
+message PDPValues {
+ double voltage;
+ double temperature;
+ double power;
+ double[16] currents;
+};
+
+queue PDPValues pdp_values;
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 8d67cf0..016f52f 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -3,8 +3,6 @@
#include "aos/common/messages/robot_state.q.h"
#include "aos/common/logging/queue_logging.h"
-#include "frc971/wpilib/pdp_fetcher.h"
-
#include "DriverStation.h"
#include "ControllerPower.h"
#undef ERROR
@@ -12,8 +10,7 @@
namespace frc971 {
namespace wpilib {
-void SendRobotState(int32_t my_pid, DriverStation *ds,
- PDPFetcher *pdp_fetcher) {
+void SendRobotState(int32_t my_pid, DriverStation *ds) {
auto new_state = ::aos::robot_state.MakeMessage();
new_state->reader_pid = my_pid;
@@ -28,10 +25,6 @@
new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
new_state->voltage_battery = ds->GetBatteryVoltage();
- if (pdp_fetcher) {
- pdp_fetcher->GetValues(&new_state->pdp);
- }
-
LOG_STRUCT(DEBUG, "robot_state", *new_state);
new_state.Send();
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 82936ec..216bf09 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -8,11 +8,8 @@
namespace frc971 {
namespace wpilib {
-class PDPFetcher;
-
// Sends out a message on ::aos::robot_state.
-void SendRobotState(int32_t my_pid, DriverStation *ds,
- PDPFetcher *pdp_fetcher);
+void SendRobotState(int32_t my_pid, DriverStation *ds);
} // namespace wpilib
} // namespace frc971