Now detecting reset using the sensor_generation queue.
diff --git a/aos/common/control_loop/ControlLoop-tmpl.h b/aos/common/control_loop/ControlLoop-tmpl.h
index b5714d5..4e1999c 100644
--- a/aos/common/control_loop/ControlLoop-tmpl.h
+++ b/aos/common/control_loop/ControlLoop-tmpl.h
@@ -5,6 +5,8 @@
#include "aos/common/messages/RobotState.q.h"
#include "aos/common/logging/queue_logging.h"
+#include "bbb/sensor_generation.q.h"
+
namespace aos {
namespace control_loops {
@@ -37,6 +39,21 @@
ZeroOutputs();
return;
}
+
+ ::bbb::sensor_generation.FetchLatest();
+ if (::bbb::sensor_generation.get() == nullptr) {
+ ZeroOutputs();
+ return;
+ }
+ if (!has_sensor_reset_counters_ ||
+ ::bbb::sensor_generation->reader_pid != reader_pid_ ||
+ ::bbb::sensor_generation->cape_resets != cape_resets_) {
+ reader_pid_ = ::bbb::sensor_generation->reader_pid;
+ cape_resets_ = ::bbb::sensor_generation->cape_resets;
+ has_sensor_reset_counters_ = true;
+ reset_ = true;
+ }
+
LOG_STRUCT(DEBUG, "goal", *goal);
// Only pass in a position if we got one this cycle.
@@ -75,12 +92,12 @@
bool outputs_enabled = false;
// Check to see if we got a driver station packet recently.
- if (aos::robot_state.FetchLatest()) {
+ if (::aos::robot_state.FetchLatest()) {
outputs_enabled = true;
- } else if (aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+ } else if (::aos::robot_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
outputs_enabled = true;
} else {
- if (aos::robot_state.get()) {
+ if (::aos::robot_state.get()) {
LOG_INTERVAL(driver_station_old_);
} else {
LOG_INTERVAL(no_driver_station_);
diff --git a/aos/common/control_loop/ControlLoop.h b/aos/common/control_loop/ControlLoop.h
index e48d259..c9e3381 100644
--- a/aos/common/control_loop/ControlLoop.h
+++ b/aos/common/control_loop/ControlLoop.h
@@ -60,8 +60,10 @@
// Maximum age of driver station packets before the loop will be disabled.
static const int kDSPacketTimeoutMs = 500;
- ControlLoop(T *control_loop) : control_loop_(control_loop) {}
-
+ ControlLoop(T *control_loop)
+ : control_loop_(control_loop),
+ reset_(true),
+ has_sensor_reset_counters_(false) {}
// Create some convenient typedefs to reference the Goal, Position, Status,
// and Output structures.
typedef typename std::remove_reference<
@@ -81,6 +83,14 @@
// motors. Calls Zero to clear all the state.
void ZeroOutputs();
+ // Returns true if the device reading the sensors reset and potentially lost
+ // track of encoder counts. Calling this read method clears the flag.
+ bool reset() {
+ bool ans = reset_;
+ reset_ = false;
+ return ans;
+ }
+
// Sets the output to zero.
// Over-ride if a value of zero is not "off" for this subsystem.
virtual void Zero(OutputType *output) { output->Zero(); }
@@ -131,6 +141,11 @@
// Pointer to the queue group
T *control_loop_;
+ bool reset_;
+ bool has_sensor_reset_counters_;
+ int32_t reader_pid_;
+ uint32_t cape_resets_;
+
typedef ::aos::util::SimpleLogInterval SimpleLogInterval;
static constexpr ::aos::time::Time kStaleLogInterval =
::aos::time::Time::InSeconds(0.1);