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/BUILD b/frc971/wpilib/BUILD
index c07b7f8..7a7d7bb 100644
--- a/frc971/wpilib/BUILD
+++ b/frc971/wpilib/BUILD
@@ -170,5 +170,22 @@
'//aos/common/messages:robot_state',
'//aos/externals:wpilib',
'//aos/common/logging:queue_logging',
+ ':pdp_fetcher',
+ ],
+)
+
+cc_library(
+ name = 'pdp_fetcher',
+ srcs = [
+ 'pdp_fetcher.cc',
+ ],
+ hdrs = [
+ 'pdp_fetcher.h',
+ ],
+ deps = [
+ '//aos/common/messages:robot_state',
+ '//aos/externals:wpilib',
+ '//aos/common/logging:queue_logging',
+ '//aos/linux_code:init',
],
)
diff --git a/frc971/wpilib/loop_output_handler.h b/frc971/wpilib/loop_output_handler.h
index d7b6283..972d7ef 100644
--- a/frc971/wpilib/loop_output_handler.h
+++ b/frc971/wpilib/loop_output_handler.h
@@ -1,12 +1,12 @@
#ifndef FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
#define FRC971_WPILIB_LOOP_OUTPUT_HANDLER_H_
+#include <atomic>
+
#include "aos/common/scoped_fd.h"
#include "aos/common/time.h"
#include "aos/common/util/log_interval.h"
-#include <atomic>
-
namespace frc971 {
namespace wpilib {
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
diff --git a/frc971/wpilib/pdp_fetcher.h b/frc971/wpilib/pdp_fetcher.h
new file mode 100644
index 0000000..8271f2b
--- /dev/null
+++ b/frc971/wpilib/pdp_fetcher.h
@@ -0,0 +1,42 @@
+#ifndef FRC971_WPILIB_PDP_FETCHER_H_
+#define FRC971_WPILIB_PDP_FETCHER_H_
+
+#include <memory>
+#include <atomic>
+
+#include "aos/common/messages/robot_state.q.h"
+#include "aos/common/mutex.h"
+
+#include "PowerDistributionPanel.h"
+
+namespace frc971 {
+namespace wpilib {
+
+// Handles fetching values from the PDP. This is slow, so it has to happen in a
+// 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};
+};
+
+} // namespace wpilib
+} // namespace frc971
+
+#endif // FRC971_WPILIB_PDP_FETCHER_H_
diff --git a/frc971/wpilib/wpilib_interface.cc b/frc971/wpilib/wpilib_interface.cc
index 2129ec3..5df3c9c 100644
--- a/frc971/wpilib/wpilib_interface.cc
+++ b/frc971/wpilib/wpilib_interface.cc
@@ -3,6 +3,8 @@
#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
@@ -11,7 +13,7 @@
namespace wpilib {
void SendRobotState(int32_t my_pid, DriverStation *ds,
- PowerDistributionPanel *pdp) {
+ PDPFetcher *pdp_fetcher) {
auto new_state = ::aos::robot_state.MakeMessage();
new_state->reader_pid = my_pid;
@@ -26,12 +28,7 @@
new_state->voltage_roborio_in = ControllerPower::GetInputVoltage();
new_state->voltage_battery = ds->GetBatteryVoltage();
- new_state->pdp_voltage = pdp->GetVoltage();
- new_state->pdp_temperature = pdp->GetTemperature();
- new_state->pdp_power = pdp->GetTotalPower();
- for (int i = 0; i < 16; ++i) {
- new_state->pdp_currents[i] = pdp->GetCurrent(i);
- }
+ pdp_fetcher->GetValues(&new_state->pdp);
LOG_STRUCT(DEBUG, "robot_state", *new_state);
diff --git a/frc971/wpilib/wpilib_interface.h b/frc971/wpilib/wpilib_interface.h
index 7d4b92d..82936ec 100644
--- a/frc971/wpilib/wpilib_interface.h
+++ b/frc971/wpilib/wpilib_interface.h
@@ -3,16 +3,16 @@
#include <stdint.h>
-#include "PowerDistributionPanel.h"
-
class DriverStation;
namespace frc971 {
namespace wpilib {
+class PDPFetcher;
+
// Sends out a message on ::aos::robot_state.
void SendRobotState(int32_t my_pid, DriverStation *ds,
- PowerDistributionPanel *pdp);
+ PDPFetcher *pdp_fetcher);
} // namespace wpilib
} // namespace frc971