Adds support for E-Stop query to trains. (#483)

- Makes the DCC train objects remember that they were set to estop speed.
- Correctly implements get_emergencystop() on them.
- Makes the Traction Service fill in the is_estop bit in the Get Speed Response payload.
- Updates the API that parses the Get Speed Response payload to be able to produce the is_estop bit.
- Adds support to the TractionThrottle for querying estop. Verifies that estop state is correctly queried during load and updated when listener commands arrive.
- Fixes estop handling bug in TractionTestTrain.

===

* Adds support for E-Stop query to trains.

- Makes the DCC train objects remember that they were set to estop speed.
- Correctly implements get_emergencystop() on them.
- Makes the Traction Service fill in the is_estop bit in the Get Speed Response payload.
- Updates the API that parses the Get Speed Response payload to be able to produce the is_estop bit.

* Adds support to the TractionThrottle for querying estop.
Verifies that estop state is correctly queried during load.
Fixes estop handling bug in TractionTestTrain.

* Adds test to verify that speed replies from the listener protocol correctly
clear the estopActive_ in the throttle client cache.

src/dcc/Loco.hxx

@@ -117,6 +117,7 @@ public:
             return;
         }
         p.lastSetSpeed_ = new_speed;
+        p.isEstop_ = false;
         unsigned previous_light = get_effective_f0();
         if (speed.direction() != p.direction_)
         {
@@ -170,6 +171,7 @@ public:
     void set_emergencystop() OVERRIDE
     {
         p.speed_ = 0;
+        p.isEstop_ = true;
         SpeedType dir0;
         dir0.set_direction(p.direction_);
         p.lastSetSpeed_ = dir0.get_wire();
@@ -182,7 +184,7 @@ public:
     /// Gets the train's ESTOP state.
     bool get_emergencystop() OVERRIDE
     {
-        return false;
+        return p.isEstop_;
     }
     /// Sets a function to a given value. @param address is the function number
     /// (0..28), @param value is 0 for funciton OFF, 1 for function ON.
@@ -368,6 +370,8 @@ struct Dcc28Payload
     unsigned nextRefresh_ : 3;
     /// Speed step we last set.
     unsigned speed_ : 5;
+    /// 1 if the last speed set was estop.
+    unsigned isEstop_ : 1;
     /// Whether the direction change packet still needs to go out.
     unsigned directionChanged_ : 1;
     /// 1 if the F0 function should be set/get in a directional way.
@@ -473,6 +477,8 @@ struct Dcc128Payload
     /// Whether the direction change packet still needs to go out.
     unsigned directionChanged_ : 1;
 
+    /// 1 if the last speed set was estop.
+    unsigned isEstop_ : 1;
     /// 1 if the F0 function should be set/get in a directional way.
     unsigned f0SetDirectional_ : 1;
     /// 1 if directional f0 is used and f0 is on for F.
@@ -548,6 +554,8 @@ struct MMOldPayload
     unsigned directionChanged_ : 1;
     /// Speed step we last set.
     unsigned speed_ : 4;
+    /// 1 if the last speed set was estop.
+    unsigned isEstop_ : 1;
 
     /// 1 if the F0 function should be set/get in a directional way.
     unsigned f0SetDirectional_ : 1;
@@ -625,6 +633,8 @@ struct MMNewPayload
     unsigned speed_ : 4;
     /// internal refresh cycle state machine
     unsigned nextRefresh_ : 3;
+    /// 1 if the last speed set was estop.
+    unsigned isEstop_ : 1;
 
     /// 1 if the F0 function should be set/get in a directional way.
     unsigned f0SetDirectional_ : 1;

src/dcc/Packet.cxxtest

@@ -355,12 +355,15 @@ protected:
         EXPECT_CALL(loop_, unregister_source(&train_));
     }
 
+    /// Requests a refresh packet from the train.
     void do_refresh()
     {
         new (&pkt_) Packet();
         train_.get_next_packet(0, &pkt_);
     }
 
+    /// Requests the packet from the train that the last code was generated
+    /// for.
     void do_callback()
     {
         Mock::VerifyAndClear(&loop_);
@@ -455,6 +458,45 @@ TEST_F(Train28Test, ZeroSpeed)
     EXPECT_THAT(get_packet(), ElementsAre(55, 0b01100000, _));
 }
 
+/// Verifies that the train correctly remembers that it was commanded to estop.
+TEST_F(Train28Test, EstopSaved)
+{
+    SpeedType s;
+    s.set_mph(128);
+    code_ = 0;
+    EXPECT_CALL(loop_, send_update(&train_, _))
+        .WillRepeatedly(SaveArg<1>(&code_));
+
+    // First make the train move.
+    train_.set_speed(s);
+    EXPECT_EQ(DccTrainUpdateCode::SPEED, code_);
+    do_callback();
+    EXPECT_THAT(get_packet(), ElementsAre(55, 0b01111111, _));
+    EXPECT_FALSE(train_.get_emergencystop());
+    EXPECT_NEAR(128, train_.get_speed().mph(), 0.1);
+
+    // Then estop the train.
+    EXPECT_FALSE(train_.get_emergencystop());
+    train_.set_emergencystop();
+    EXPECT_EQ(DccTrainUpdateCode::ESTOP, code_);
+    EXPECT_TRUE(train_.get_emergencystop());
+    do_callback();
+    EXPECT_THAT(get_packet(), ElementsAre(55, 0b01100001, _));
+    // Checks that the train knows it's estopped and the speed is reported as
+    // zero.
+    EXPECT_TRUE(train_.get_emergencystop());
+    EXPECT_NEAR(0, train_.get_speed().mph(), 0.1);
+
+    // Move the train again.
+    s = 37.5;
+    train_.set_speed(s);
+    EXPECT_EQ(DccTrainUpdateCode::SPEED, code_);
+    do_callback();
+    EXPECT_THAT(get_packet(), ElementsAre(55, 0b01101011, _));
+    EXPECT_FALSE(train_.get_emergencystop());
+    EXPECT_NEAR(37.5, train_.get_speed().speed(), 0.1);
+}
+
 TEST_F(Train28Test, RefreshLoop)
 {
     EXPECT_CALL(loop_, send_update(&train_, _)).Times(AtLeast(1));

src/openlcb/TractionDefs.hxx

@@ -134,6 +134,9 @@ struct TractionDefs {
         RESP_CONSIST_CONFIG = REQ_CONSIST_CONFIG,
         RESP_TRACTION_MGMT = REQ_TRACTION_MGMT,
 
+        // Status byte of the Speed Query response
+        SPEEDRESP_STATUS_IS_ESTOP = 1,
+
         // Byte 1 of Controller Configuration response
         CTRLRESP_ASSIGN_CONTROLLER = CTRLREQ_ASSIGN_CONTROLLER,
         CTRLRESP_QUERY_CONTROLLER = CTRLREQ_QUERY_CONTROLLER,
@@ -356,8 +359,12 @@ struct TractionDefs {
     /** Parses the response payload of a GET_SPEED packet.
      * @returns true if the last_set_speed value was present and non-NaN.
      * @param p is the response payload.
-     * @param v is the velocity that will be set to the speed value. */
-    static bool speed_get_parse_last(const Payload &p, Velocity *v)
+     * @param v is the velocity that will be set to the speed value.
+     * @param is_estop if non-null, will be set to true if the train was last
+     * set to estop instead of a speed.
+     */
+    static bool speed_get_parse_last(
+        const Payload &p, Velocity *v, bool *is_estop = nullptr)
     {
         if (p.size() < 3)
         {
@@ -368,6 +375,14 @@ struct TractionDefs {
         {
             return false;
         }
+        if (is_estop)
+        {
+            if (p.size() < 4)
+            {
+                return false;
+            }
+            *is_estop = (p[3] & SPEEDRESP_STATUS_IS_ESTOP) != 0;
+        }
         return true;
     }
 

src/openlcb/TractionTestTrain.cxx

@@ -86,7 +86,8 @@ void LoggingTrain::set_emergencystop()
         TractionDefs::train_node_name_from_legacy(
             legacyAddressType_, legacyAddress_)
             .c_str());
-    estopActive_ = 0;
+    currentSpeed_.set_mph(0); // keeps sign
+    estopActive_ = true;
 }
 
 bool LoggingTrain::get_emergencystop()

src/openlcb/TractionThrottle.cxxtest

@@ -225,6 +225,7 @@ TEST_F(ThrottleTest, SendQuery)
     EXPECT_EQ(0, flow.response()->resultCode);
 
     EXPECT_NEAR(13.2, flow.throttle_.get_speed().mph(), 0.1);
+    EXPECT_FALSE(flow.throttle_.get_emergencystop());
     EXPECT_EQ(0, flow.throttle_.get_fn(0));
     EXPECT_EQ(1, flow.throttle_.get_fn(1));
     EXPECT_EQ(0, flow.throttle_.get_fn(2));
@@ -233,6 +234,15 @@ TEST_F(ThrottleTest, SendQuery)
     EXPECT_EQ(0, flow.throttle_.get_fn(5));
     EXPECT_EQ(0, flow.throttle_.get_fn(6));
     EXPECT_EQ(1, flow.throttle_.get_fn(7));
+
+    // Checks emergency stop load.
+    trainImpl_.set_emergencystop();
+    flow.load();
+    n_.wait_for_notification();
+    wait();
+    EXPECT_EQ(0, flow.response()->resultCode);
+    EXPECT_NEAR(0, flow.throttle_.get_speed().mph(), 0.1);
+    EXPECT_TRUE(flow.throttle_.get_emergencystop());
 }
 
 class ThrottleClientTest : public ThrottleTest {
@@ -523,6 +533,23 @@ TEST_F(ThrottleClientTest, ListenerCallback)
     // send another speed command to verify that E-Stop gets cleared
     throttle_.set_speed(v);
     EXPECT_FALSE(throttle_.get_emergencystop());
+
+    // Go back to estop.
+    EXPECT_CALL(l, update(-1));
+    EXPECT_FALSE(throttle_.get_emergencystop());
+    send_packet(":X195EB330N077102;"); // E-Stop
+    wait();
+    Mock::VerifyAndClear(&l);
+    EXPECT_TRUE(throttle_.get_emergencystop());
+
+    // Speed replies will also clear estop.
+    EXPECT_CALL(l, update(-1));
+    send_packet(":X195EB330N07710045D0;"); // speed 13 mph
+    wait();
+    Mock::VerifyAndClear(&l);
+    EXPECT_NEAR(13, throttle_.get_speed().mph(), 0.1);
+    EXPECT_EQ(Velocity::FORWARD, throttle_.get_speed().direction());
+    EXPECT_FALSE(throttle_.get_emergencystop());
 }
 
 } // namespace openlcb

src/openlcb/TractionThrottle.hxx

@@ -619,15 +619,15 @@ private:
             {
                 bool expected = pending_reply_arrived();
                 Velocity v;
-                if (TractionDefs::speed_get_parse_last(p, &v))
+                bool is_estop;
+                if (TractionDefs::speed_get_parse_last(p, &v, &is_estop))
                 {
                     lastSetSpeed_ = v;
+                    estopActive_ = is_estop;
                     if (updateCallback_ && !expected)
                     {
                         updateCallback_(-1);
                     }
-                    /// @todo (Stuart.Baker): Do we need to do anything with
-                    /// estopActive_?
                 }
                 return;
             }

src/openlcb/TractionTrain.cxx

@@ -280,7 +280,12 @@ struct TrainService::Impl
                     uint8_t *d = reinterpret_cast<uint8_t *>(&(*p)[0]);
                     d[0] = TractionDefs::RESP_QUERY_SPEED;
                     speed_to_fp16(train_node()->train()->get_speed(), d + 1);
-                    d[3] = 0; // status byte: reserved.
+                    uint8_t status = 0;
+                    if (train_node()->train()->get_emergencystop())
+                    {
+                        status |= TractionDefs::SPEEDRESP_STATUS_IS_ESTOP;
+                    }
+                    d[3] = status;
                     speed_to_fp16(train_node()->train()->get_commanded_speed(),
                                   d + 4);
                     speed_to_fp16(train_node()->train()->get_actual_speed(),

src/openlcb/TractionTrain.cxxtest

@@ -128,6 +128,7 @@ TEST_F(TractionSingleMockTest, SetSpeed)
 TEST_F(TractionSingleMockTest, GetSpeed)
 {
     EXPECT_CALL(m1_, get_speed()).WillOnce(Return(37.5));
+    EXPECT_CALL(m1_, get_emergencystop()).WillOnce(Return(false));
     EXPECT_CALL(m1_, get_commanded_speed()).WillOnce(Return(nan_to_speed()));
     EXPECT_CALL(m1_, get_actual_speed()).WillOnce(Return(nan_to_speed()));
     expect_packet(":X191E933AN15511050B000FFFF;");
@@ -138,6 +139,7 @@ TEST_F(TractionSingleMockTest, GetSpeed)
 TEST_F(TractionSingleMockTest, GetSpeedTestWithCommandedSpeed)
 {
     EXPECT_CALL(m1_, get_speed()).WillOnce(Return(37.5));
+    EXPECT_CALL(m1_, get_emergencystop()).WillOnce(Return(false));
     EXPECT_CALL(m1_, get_commanded_speed()).WillOnce(Return(37.0));
     EXPECT_CALL(m1_, get_actual_speed()).WillOnce(Return(nan_to_speed()));
     expect_packet(":X191E933AN15511050B00050A0;");
@@ -148,13 +150,25 @@ TEST_F(TractionSingleMockTest, GetSpeedTestWithCommandedSpeed)
 TEST_F(TractionSingleMockTest, GetSpeedTestWithActualSpeed)
 {
     EXPECT_CALL(m1_, get_speed()).WillOnce(Return(37.5));
+    EXPECT_CALL(m1_, get_emergencystop()).WillOnce(Return(false));
     EXPECT_CALL(m1_, get_commanded_speed()).WillOnce(Return(37.0));
     EXPECT_CALL(m1_, get_actual_speed()).WillOnce(Return(38.0));
     expect_packet(":X191E933AN15511050B00050A0;");
     expect_packet(":X191E933AN255150C0;");
     send_packet(":X195EB551N033A10;");
 }
 
+TEST_F(TractionSingleMockTest, GetSpeedTestWithEstop)
+{
+    EXPECT_CALL(m1_, get_speed()).WillOnce(Return(-0.0));
+    EXPECT_CALL(m1_, get_emergencystop()).WillOnce(Return(true));
+    EXPECT_CALL(m1_, get_commanded_speed()).WillOnce(Return(-0.0));
+    EXPECT_CALL(m1_, get_actual_speed()).WillOnce(Return(-0.0));
+    expect_packet(":X191E933AN1551108000018000;");
+    expect_packet(":X191E933AN25518000;");
+    send_packet(":X195EB551N033A10;");
+}
+
 TEST_F(TractionSingleMockTest, SetFn)
 {
     EXPECT_CALL(m1_, set_fn(0x112233, 0x4384));