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/aos/common/messages/robot_state.q b/aos/common/messages/robot_state.q
index a99627e..ab269bd 100644
--- a/aos/common/messages/robot_state.q
+++ b/aos/common/messages/robot_state.q
@@ -33,6 +33,14 @@
// joystick code hasn't died.
queue JoystickState joystick_state;
+// Values retrieved from the PDP.
+struct PDPValues {
+ double voltage;
+ double temperature;
+ double power;
+ double[16] currents;
+};
+
message RobotState {
// The PID of the process reading sensors.
// This is here so control loops can tell when it changes.
@@ -60,11 +68,7 @@
// actual driver's station.
double voltage_battery;
- // From the PDP directly.
- double pdp_voltage;
- double pdp_temperature;
- double pdp_power;
- double[16] pdp_currents;
+ PDPValues pdp;
};
// Messages are sent out on this queue along with reading sensors. It contains
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
diff --git a/y2014/wpilib/BUILD b/y2014/wpilib/BUILD
index c9aca05..499049d 100644
--- a/y2014/wpilib/BUILD
+++ b/y2014/wpilib/BUILD
@@ -32,5 +32,6 @@
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
+ '//frc971/wpilib:pdp_fetcher',
],
)
diff --git a/y2014/wpilib/wpilib_interface.cc b/y2014/wpilib/wpilib_interface.cc
index 77e478e..fdd9b5c 100644
--- a/y2014/wpilib/wpilib_interface.cc
+++ b/y2014/wpilib/wpilib_interface.cc
@@ -18,7 +18,6 @@
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
#endif
-#include "PowerDistributionPanel.h"
#undef ERROR
#include "aos/common/logging/logging.h"
@@ -49,6 +48,7 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/pdp_fetcher.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -122,7 +122,8 @@
class SensorReader {
public:
- SensorReader() {
+ SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
+ : pdp_fetcher_(pdp_fetcher) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
encoder_filter_.SetPeriodNanoSeconds(
@@ -255,7 +256,6 @@
#else
&DriverStation::GetInstance();
#endif
- pdp_.reset(new PowerDistributionPanel());
top_reader_.Start();
bottom_reader_.Start();
@@ -280,7 +280,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_.get());
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
const auto &values = constants::GetValues();
@@ -443,7 +443,7 @@
int32_t my_pid_;
DriverStation *ds_;
- ::std::unique_ptr<PowerDistributionPanel> pdp_;
+ ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<::frc971::wpilib::DMASynchronizer> dma_synchronizer_;
@@ -699,7 +699,9 @@
::frc971::wpilib::JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
- SensorReader reader;
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+ SensorReader reader(&pdp_fetcher);
// Create this first to make sure it ends up in one of the lower-numbered
// FPGA slots so we can use it with DMA.
@@ -775,6 +777,8 @@
joystick_sender.Quit();
joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
reader.Quit();
reader_thread.join();
gyro_sender.Quit();
diff --git a/y2014_bot3/wpilib/BUILD b/y2014_bot3/wpilib/BUILD
index 5c3f0d1..ea8409a 100644
--- a/y2014_bot3/wpilib/BUILD
+++ b/y2014_bot3/wpilib/BUILD
@@ -25,6 +25,7 @@
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
+ '//frc971/wpilib:pdp_fetcher',
'//y2014_bot3/autonomous:auto_queue',
'//y2014_bot3/control_loops/drivetrain:drivetrain_lib',
'//y2014_bot3/control_loops/rollers:rollers_lib',
diff --git a/y2014_bot3/wpilib/wpilib_interface.cc b/y2014_bot3/wpilib/wpilib_interface.cc
index 4fde0f5..3b6c8b1 100644
--- a/y2014_bot3/wpilib/wpilib_interface.cc
+++ b/y2014_bot3/wpilib/wpilib_interface.cc
@@ -16,7 +16,6 @@
#include "RobotBase.h"
#include "dma.h"
#include "DigitalInput.h"
-#include "PowerDistributionPanel.h"
#undef ERROR
#include "aos/common/logging/logging.h"
@@ -42,6 +41,7 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/pdp_fetcher.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -74,7 +74,8 @@
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
- SensorReader() {}
+ SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
+ : pdp_fetcher_(pdp_fetcher) {}
void set_drivetrain_left_encoder(::std::unique_ptr<Encoder> encoder) {
drivetrain_left_encoder_ = ::std::move(encoder);
@@ -96,7 +97,6 @@
#else
&DriverStation::GetInstance();
#endif
- pdp_.reset(new PowerDistributionPanel());
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
@@ -106,7 +106,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_.get());
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
// Drivetrain
{
@@ -138,7 +138,7 @@
int32_t my_pid_;
DriverStation *ds_;
- ::std::unique_ptr<PowerDistributionPanel> pdp_;
+ ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<Encoder> drivetrain_left_encoder_;
::std::unique_ptr<Encoder> drivetrain_right_encoder_;
@@ -343,11 +343,14 @@
JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+
//TODO(comran): IO ports are placeholders at the moment, so match them to
// the robot before turning on.
// Sensors
- SensorReader reader;
+ SensorReader reader(&pdp_fetcher);
reader.set_drivetrain_left_encoder(make_encoder(4));
reader.set_drivetrain_right_encoder(make_encoder(5));
@@ -393,6 +396,8 @@
joystick_sender.Quit();
joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
reader.Quit();
reader_thread.join();
gyro_sender.Quit();
diff --git a/y2015/wpilib/BUILD b/y2015/wpilib/BUILD
index c5c4202..6966c9f 100644
--- a/y2015/wpilib/BUILD
+++ b/y2015/wpilib/BUILD
@@ -33,5 +33,6 @@
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
'//frc971/wpilib:wpilib_interface',
+ '//frc971/wpilib:pdp_fetcher',
],
)
diff --git a/y2015/wpilib/wpilib_interface.cc b/y2015/wpilib/wpilib_interface.cc
index 0ea4b8d..4d49b41 100644
--- a/y2015/wpilib/wpilib_interface.cc
+++ b/y2015/wpilib/wpilib_interface.cc
@@ -18,7 +18,6 @@
#ifndef WPILIB2015
#include "DigitalGlitchFilter.h"
#endif
-#include "PowerDistributionPanel.h"
#undef ERROR
#include "aos/common/logging/logging.h"
@@ -49,6 +48,7 @@
#include "frc971/wpilib/encoder_and_potentiometer.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/pdp_fetcher.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -126,7 +126,8 @@
class SensorReader {
public:
- SensorReader() {
+ SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
+ : pdp_fetcher_(pdp_fetcher) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
filter_.SetPeriodNanoSeconds(
@@ -237,7 +238,6 @@
#else
&DriverStation::GetInstance();
#endif
- pdp_.reset(new PowerDistributionPanel());
wrist_encoder_.Start();
dma_synchronizer_->Start();
@@ -252,7 +252,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_.get());
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
{
auto drivetrain_message = drivetrain_queue.position.MakeMessage();
@@ -309,7 +309,7 @@
int32_t my_pid_;
DriverStation *ds_;
- ::std::unique_ptr<PowerDistributionPanel> pdp_;
+ ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
void CopyPotAndIndexPosition(
const DMAEncoderAndPotentiometer &encoder, PotAndIndexPosition *position,
@@ -647,7 +647,10 @@
::std::thread joystick_thread(::std::ref(joystick_sender));
// TODO(austin): Compressor needs to use a spike.
- SensorReader reader;
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+
+ SensorReader reader(&pdp_fetcher);
reader.set_arm_left_encoder(encoder(1));
reader.set_arm_left_index(make_unique<DigitalInput>(1));
reader.set_arm_left_potentiometer(make_unique<AnalogInput>(1));
@@ -730,6 +733,8 @@
joystick_sender.Quit();
joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
reader.Quit();
reader_thread.join();
gyro_sender.Quit();
diff --git a/y2015_bot3/wpilib/BUILD b/y2015_bot3/wpilib/BUILD
index f89a2b4..6fae2e4 100644
--- a/y2015_bot3/wpilib/BUILD
+++ b/y2015_bot3/wpilib/BUILD
@@ -25,6 +25,7 @@
'//frc971/wpilib:wpilib_interface',
'//frc971/control_loops:queues',
'//frc971/wpilib:logging_queue',
+ '//frc971/wpilib:pdp_fetcher',
'//y2015_bot3/autonomous:auto_queue',
'//y2015_bot3/control_loops/drivetrain:drivetrain_lib',
'//y2015_bot3/control_loops/elevator:elevator_lib',
diff --git a/y2015_bot3/wpilib/wpilib_interface.cc b/y2015_bot3/wpilib/wpilib_interface.cc
index 6b5c3db..de694ab 100644
--- a/y2015_bot3/wpilib/wpilib_interface.cc
+++ b/y2015_bot3/wpilib/wpilib_interface.cc
@@ -19,7 +19,6 @@
#include "DigitalGlitchFilter.h"
#endif
#include "DigitalInput.h"
-#include "PowerDistributionPanel.h"
#undef ERROR
#include "aos/common/logging/logging.h"
@@ -49,6 +48,7 @@
#include "frc971/wpilib/gyro_sender.h"
#include "frc971/wpilib/logging.q.h"
#include "frc971/wpilib/wpilib_interface.h"
+#include "frc971/wpilib/pdp_fetcher.h"
#ifndef M_PI
#define M_PI 3.14159265358979323846
@@ -94,7 +94,8 @@
// Reads in our inputs. (sensors, voltages, etc.)
class SensorReader {
public:
- SensorReader() {
+ SensorReader(::frc971::wpilib::PDPFetcher *pdp_fetcher)
+ : pdp_fetcher_(pdp_fetcher) {
// Set it to filter out anything shorter than 1/4 of the minimum pulse width
// we should ever see.
filter_.SetPeriodNanoSeconds(
@@ -136,7 +137,6 @@
#else
&DriverStation::GetInstance();
#endif
- pdp_.reset(new PowerDistributionPanel());
::aos::SetCurrentThreadRealtimePriority(kPriority);
while (run_) {
@@ -146,7 +146,7 @@
}
void RunIteration() {
- ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_.get());
+ ::frc971::wpilib::SendRobotState(my_pid_, ds_, pdp_fetcher_);
// Drivetrain
{
@@ -190,7 +190,7 @@
int32_t my_pid_;
DriverStation *ds_;
- ::std::unique_ptr<PowerDistributionPanel> pdp_;
+ ::frc971::wpilib::PDPFetcher *const pdp_fetcher_;
::std::unique_ptr<Encoder> left_encoder_, right_encoder_, elevator_encoder_;
::std::unique_ptr<DigitalInput> zeroing_hall_effect_;
@@ -463,7 +463,9 @@
JoystickSender joystick_sender;
::std::thread joystick_thread(::std::ref(joystick_sender));
- SensorReader reader;
+ ::frc971::wpilib::PDPFetcher pdp_fetcher;
+ ::std::thread pdp_fetcher_thread(::std::ref(pdp_fetcher));
+ SensorReader reader(&pdp_fetcher);
reader.set_elevator_encoder(encoder(6));
reader.set_elevator_zeroing_hall_effect(make_unique<DigitalInput>(6));
@@ -518,6 +520,8 @@
joystick_sender.Quit();
joystick_thread.join();
+ pdp_fetcher.Quit();
+ pdp_fetcher_thread.join();
reader.Quit();
reader_thread.join();
gyro_sender.Quit();