Now detecting reset using the sensor_generation queue.
diff --git a/aos/common/common.gyp b/aos/common/common.gyp
index 906b5b2..088c882 100644
--- a/aos/common/common.gyp
+++ b/aos/common/common.gyp
@@ -184,6 +184,7 @@
'control_loop_queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/util/util.gyp:log_interval',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_generation',
],
'export_dependent_settings': [
'<(AOS)/common/messages/messages.gyp:aos_queues',
@@ -193,6 +194,7 @@
'control_loop_queues',
'<(AOS)/common/logging/logging.gyp:queue_logging',
'<(AOS)/common/util/util.gyp:log_interval',
+ '<(DEPTH)/bbb_cape/src/bbb/bbb.gyp:sensor_generation',
],
},
{
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);
diff --git a/frc971/control_loops/claw/claw_lib_test.cc b/frc971/control_loops/claw/claw_lib_test.cc
index e663672..db435ad 100644
--- a/frc971/control_loops/claw/claw_lib_test.cc
+++ b/frc971/control_loops/claw/claw_lib_test.cc
@@ -274,6 +274,11 @@
// test.
::aos::robot_state.Clear();
SendDSPacket(true);
+ ::bbb::sensor_generation.Clear();
+ ::bbb::sensor_generation.MakeWithBuilder()
+ .reader_pid(254)
+ .cape_resets(5)
+ .Send();
}
void SendDSPacket(bool enabled) {
@@ -299,6 +304,7 @@
virtual ~ClawTest() {
::aos::robot_state.Clear();
+ ::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
index 0119dd2..eb1ede2 100644
--- a/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
+++ b/frc971/control_loops/drivetrain/drivetrain_lib_test.cc
@@ -3,6 +3,7 @@
#include <memory>
#include "gtest/gtest.h"
+#include "aos/common/network/team_number.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "aos/controls/polytope.h"
@@ -31,6 +32,14 @@
::testing::Environment* const holder_env =
::testing::AddGlobalTestEnvironment(new Environment);
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ // Override this to define how to set up the environment.
+ virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
+};
+
+::testing::Environment* const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
// Class which simulates the drivetrain and sends out queue messages containing the
// position.
@@ -119,6 +128,11 @@
// Flush the robot state queue so we can use clean shared memory for this
// test, also for the gyro.
::aos::robot_state.Clear();
+ ::bbb::sensor_generation.Clear();
+ ::bbb::sensor_generation.MakeWithBuilder()
+ .reader_pid(254)
+ .cape_resets(5)
+ .Send();
::frc971::sensors::gyro.Clear();
SendDSPacket(true);
}
@@ -144,6 +158,7 @@
virtual ~DrivetrainTest() {
::aos::robot_state.Clear();
::frc971::sensors::gyro.Clear();
+ ::bbb::sensor_generation.Clear();
}
};
diff --git a/frc971/control_loops/shooter/shooter_lib_test.cc b/frc971/control_loops/shooter/shooter_lib_test.cc
index b2da13e..f3a8872 100755
--- a/frc971/control_loops/shooter/shooter_lib_test.cc
+++ b/frc971/control_loops/shooter/shooter_lib_test.cc
@@ -3,6 +3,7 @@
#include <memory>
#include "gtest/gtest.h"
+#include "aos/common/network/team_number.h"
#include "aos/common/queue.h"
#include "aos/common/queue_testutils.h"
#include "frc971/control_loops/shooter/shooter.q.h"
@@ -17,6 +18,16 @@
namespace control_loops {
namespace testing {
+class TeamNumberEnvironment : public ::testing::Environment {
+ public:
+ // Override this to define how to set up the environment.
+ virtual void SetUp() { aos::network::OverrideTeamNumber(971); }
+};
+
+::testing::Environment* const team_number_env =
+ ::testing::AddGlobalTestEnvironment(new TeamNumberEnvironment);
+
+
// Class which simulates the shooter and sends out queue messages containing the
// position.
class ShooterSimulation {
@@ -204,6 +215,11 @@
// test.
::aos::robot_state.Clear();
SendDSPacket(true);
+ ::bbb::sensor_generation.Clear();
+ ::bbb::sensor_generation.MakeWithBuilder()
+ .reader_pid(254)
+ .cape_resets(5)
+ .Send();
}
@@ -221,7 +237,10 @@
EXPECT_NEAR(shooter_queue_group_.goal->shot_power, pos, 1e-4);
}
- virtual ~ShooterTest() { ::aos::robot_state.Clear(); }
+ virtual ~ShooterTest() {
+ ::aos::robot_state.Clear();
+ ::bbb::sensor_generation.Clear();
+ }
};