From 16eb593b95d091f1833d0d712c52fbf9d92d358d Mon Sep 17 00:00:00 2001 From: Balazs Racz Date: Fri, 4 Dec 2020 13:18:39 +0100 Subject: [PATCH] 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 | 12 +++++++- src/dcc/Packet.cxxtest | 42 ++++++++++++++++++++++++++++ src/openlcb/TractionDefs.hxx | 19 +++++++++++-- src/openlcb/TractionTestTrain.cxx | 3 +- src/openlcb/TractionThrottle.cxxtest | 27 ++++++++++++++++++ src/openlcb/TractionThrottle.hxx | 6 ++-- src/openlcb/TractionTrain.cxx | 7 ++++- src/openlcb/TractionTrain.cxxtest | 14 ++++++++++ 8 files changed, 122 insertions(+), 8 deletions(-) diff --git a/src/dcc/Loco.hxx b/src/dcc/Loco.hxx index caed8643b..55e2d9b71 100644 --- a/src/dcc/Loco.hxx +++ b/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; diff --git a/src/dcc/Packet.cxxtest b/src/dcc/Packet.cxxtest index f6392c2ba..0f9bf934a 100644 --- a/src/dcc/Packet.cxxtest +++ b/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)); diff --git a/src/openlcb/TractionDefs.hxx b/src/openlcb/TractionDefs.hxx index 389a984f4..ed05f939d 100644 --- a/src/openlcb/TractionDefs.hxx +++ b/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; } diff --git a/src/openlcb/TractionTestTrain.cxx b/src/openlcb/TractionTestTrain.cxx index 8cd587e4c..d847ca29b 100644 --- a/src/openlcb/TractionTestTrain.cxx +++ b/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() diff --git a/src/openlcb/TractionThrottle.cxxtest b/src/openlcb/TractionThrottle.cxxtest index 8f4f66de4..3f8298281 100644 --- a/src/openlcb/TractionThrottle.cxxtest +++ b/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 diff --git a/src/openlcb/TractionThrottle.hxx b/src/openlcb/TractionThrottle.hxx index c06c44b98..9951b1c9e 100644 --- a/src/openlcb/TractionThrottle.hxx +++ b/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; } diff --git a/src/openlcb/TractionTrain.cxx b/src/openlcb/TractionTrain.cxx index c2214dadd..2e2b81407 100644 --- a/src/openlcb/TractionTrain.cxx +++ b/src/openlcb/TractionTrain.cxx @@ -280,7 +280,12 @@ struct TrainService::Impl uint8_t *d = reinterpret_cast(&(*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(), diff --git a/src/openlcb/TractionTrain.cxxtest b/src/openlcb/TractionTrain.cxxtest index 9c8c386c7..96eaf5e02 100644 --- a/src/openlcb/TractionTrain.cxxtest +++ b/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,6 +150,7 @@ 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;"); @@ -155,6 +158,17 @@ TEST_F(TractionSingleMockTest, GetSpeedTestWithActualSpeed) 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));