actually think through enabled/disabled state for control loops
Change-Id: I7c0638932d3d5f8b0fc07c99fcdfbfd9b8e86053
diff --git a/aos/common/controls/control_loop-tmpl.h b/aos/common/controls/control_loop-tmpl.h
index 6903bc2..42702c0 100644
--- a/aos/common/controls/control_loop-tmpl.h
+++ b/aos/common/controls/control_loop-tmpl.h
@@ -71,11 +71,21 @@
// Check to see if we got a driver station packet recently.
if (::aos::joystick_state.FetchLatest()) {
- outputs_enabled = true;
+ if (::aos::joystick_state->enabled) outputs_enabled = true;
if (::aos::robot_state->outputs_enabled) {
- last_pwm_sent_ = ::aos::robot_state->sent_time;
+ // If the driver's station reports being disabled, we're probably not
+ // actually going to send motor values regardless of what the FPGA
+ // reports.
+ if (::aos::joystick_state->enabled) {
+ last_pwm_sent_ = ::aos::robot_state->sent_time;
+ } else {
+ LOG(WARNING, "outputs enabled while disabled\n");
+ }
+ } else if (::aos::joystick_state->enabled) {
+ LOG(WARNING, "outputs disabled while enabled\n");
}
- } else if (::aos::joystick_state.IsNewerThanMS(kDSPacketTimeoutMs)) {
+ } else if (::aos::joystick_state.IsNewerThanMS(kDSPacketTimeoutMs) &&
+ ::aos::joystick_state->enabled) {
outputs_enabled = true;
} else {
if (::aos::joystick_state.get()) {