Fix frame validation and decode errors in the V2 parser

piper_cpp/include/piper_cpp/interface/piper_interface_v2.h

@@ -166,12 +166,13 @@ class PiperInterfaceV2
 
     /// Per-motor angle/speed limits for all six motors. The arm fills this in after the search
     /// queries that `connectPort()` fires automatically; ready within a second of opening.
+    /// `is_valid` only once every motor has replied.
     StateSnapshot<ArmMsgAllCurrentMotorAngleLimitMaxSpd> getArmAllMotorAngleLimitMaxSpd() const
     {
         return StateSnapshot(arm_all_motor_angle_limit_max_spd_);
     }
 
-    /// Per-motor max-acceleration limits for all six motors. Same lifecycle as above.
+    /// Per-motor max-acceleration limits for all six motors. Same lifecycle and validity as above.
     StateSnapshot<ArmMsgFeedbackAllCurrentMotorMaxAccLimit> getArmAllMotorMaxAccLimit() const
     {
         return StateSnapshot(arm_all_motor_max_acc_limit_);

piper_cpp/include/piper_cpp/types/feedback/motors.h

@@ -50,7 +50,7 @@ struct ArmMsgFeedbackHighSpd
 {
     uint32_t can_id{0};     ///< Source CAN ID 0x251..0x256, identifying which joint this is.
     int16_t motor_speed{0}; ///< Motor speed, 0.001 rad/s.
-    uint16_t current{0};    ///< Motor current, 0.001 A.
+    int16_t current{0};     ///< Motor current, 0.001 A, signed.
     int32_t pos{0};         ///< Raw motor position (encoder units; convert via DH if needed).
     float effort{0.0f};     ///< Derived torque, 0.001 N*m. Populated by `calcEffort()`.
 

piper_cpp/src/interface/piper_interface_v2.cpp

@@ -33,8 +33,8 @@ PiperInterfaceV2::PiperInterfaceV2(
     arm_gripper_teaching_pendant_fb_.setNumCallers(1);
     arm_current_motor_angle_limit_max_spd_.setNumCallers(1);
     arm_current_motor_max_acc_limit_.setNumCallers(1);
-    arm_all_motor_angle_limit_max_spd_.setNumCallers(1);
-    arm_all_motor_max_acc_limit_.setNumCallers(1);
+    arm_all_motor_angle_limit_max_spd_.setNumCallers(6); // one reply per motor
+    arm_all_motor_max_acc_limit_.setNumCallers(6);       // one reply per motor
     arm_joint_ctrl_msgs_.setNumCallers(3); // joint control messages are updated in 3 parts
     arm_gripper_ctrl_.setNumCallers(1);    // gripper control messages
     arm_motion_ctrl_code_151_.setNumCallers(1);
@@ -288,39 +288,47 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times
         }
         case ArmMsgType::FeedbackCurrentMotorAngleLimitMaxSpd:
         {
-            arm_current_motor_angle_limit_max_spd_.set(
-                msg.arm_feedback_current_motor_angle_limit_max_spd, msg.timestamp
-            );
             // all_motor_angle_limit_max_spd.motor[];
-            if (msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num > 6)
+            if (msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num < 1 ||
+                msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num > 6)
             {
                 // Handle unexpected motor number
                 std::cerr << "Error: Invalid motor number for current motor angle limit max speed: "
                           << msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num << "\n";
             }
             else
             {
+                arm_current_motor_angle_limit_max_spd_.set(
+                    msg.arm_feedback_current_motor_angle_limit_max_spd, msg.timestamp
+                );
                 auto all_motor_angle_limit_max_spd = arm_all_motor_angle_limit_max_spd_.getValue();
                 all_motor_angle_limit_max_spd.motor[msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num - 1] =
                     msg.arm_feedback_current_motor_angle_limit_max_spd;
-                arm_all_motor_angle_limit_max_spd_.set(all_motor_angle_limit_max_spd, msg.timestamp);
+                arm_all_motor_angle_limit_max_spd_.set(
+                    all_motor_angle_limit_max_spd, msg.timestamp,
+                    msg.arm_feedback_current_motor_angle_limit_max_spd.motor_num - 1
+                );
             }
             break;
         }
         case ArmMsgType::FeedbackCurrentMotorMaxAccLimit:
         {
-            arm_current_motor_max_acc_limit_.set(msg.arm_feedback_current_motor_max_acc_limit, msg.timestamp);
-            if (msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num > 6)
+            if (msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num < 1 ||
+                msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num > 6)
             {
                 std::cerr << "Error: Invalid motor number for current motor max acceleration limit: "
                           << msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num << "\n";
             }
             else
             {
+                arm_current_motor_max_acc_limit_.set(msg.arm_feedback_current_motor_max_acc_limit, msg.timestamp);
                 auto all_motor_max_acc_limit = arm_all_motor_max_acc_limit_.getValue();
                 all_motor_max_acc_limit.motor[msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num - 1] =
                     msg.arm_feedback_current_motor_max_acc_limit;
-                arm_all_motor_max_acc_limit_.set(all_motor_max_acc_limit, msg.timestamp);
+                arm_all_motor_max_acc_limit_.set(
+                    all_motor_max_acc_limit, msg.timestamp,
+                    msg.arm_feedback_current_motor_max_acc_limit.joint_motor_num - 1
+                );
             }
             break;
         }
@@ -459,11 +467,8 @@ bool PiperInterfaceV2::sendPiperMsg(const PiperMessage &msg)
         std::cerr << "Error: Failed to encode Piper message.\n";
         return false;
     }
-    std::vector<uint8_t> data(tx.data, tx.data + PiperMessage::raw_data_len);
-
-    auto can_id = static_cast<uint16_t>(msgTypeToCanId(msg.type));
-
-    if (!arm_can_->sendCanMessage(can_id, data))
+    std::vector<uint8_t> data(tx.data, tx.data + tx.can_dlc);
+    if (!arm_can_->sendCanMessage(tx.can_id, data))
     {
         std::cerr << "Error: Failed to send CAN message.\n";
         return false;

piper_cpp/src/protocol/piper_parser_v2.cpp

@@ -27,6 +27,30 @@ static int8_t bytesToInt8(const uint8_t *data) { return static_cast<int8_t>(data
 // For unsigned 8-bit
 static uint8_t bytesToUint8(const uint8_t *data) { return data[0]; }
 
+// Smallest payload each decoded ID needs: one past the last byte the decoder reads. A shorter
+// frame would otherwise be decoded from bytes that were never on the bus.
+static uint8_t requiredDlc(CanIDPiper id)
+{
+    switch (id)
+    {
+    case CanIDPiper::ARM_GRIPPER_FEEDBACK:
+    case CanIDPiper::ARM_FEEDBACK_CURRENT_MOTOR_ANGLE_LIMIT_MAX_SPD:
+        return 7;
+    case CanIDPiper::ARM_CRASH_PROTECTION_RATING_FEEDBACK:
+    case CanIDPiper::ARM_MOTION_CTRL_2:
+        return 6;
+    case CanIDPiper::ARM_GRIPPER_TEACHING_PENDANT_PARAM_FEEDBACK:
+    case CanIDPiper::ARM_FEEDBACK_CURRENT_MOTOR_MAX_ACC_LIMIT:
+        return 3;
+    case CanIDPiper::ARM_INSTRUCTION_RESPONSE_CONFIG:
+        return 2;
+    case CanIDPiper::ARM_FIRMWARE_READ:
+        return 0; // variable-length text chunk, copied by DLC
+    default:
+        return 8;
+    }
+}
+
 // Split a value into unsigned bytes (big-endian)
 inline void int16_to_bytes(uint16_t val, uint8_t *out)
 {
@@ -60,7 +84,14 @@ inline void int8_to_bytes(uint8_t val, uint8_t *out) { out[0] = val & 0xFF; }
 
 bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double timestamp, PiperMessage &msg)
 {
-    uint32_t can_id = rx_frame.can_id & CAN_EFF_MASK;
+    // The Piper protocol uses standard 11-bit data frames only. Anything above those bits is
+    // a remote-request, error or extended flag, or a reserved bit no valid frame sets.
+    if ((rx_frame.can_id & ~CAN_SFF_MASK) || rx_frame.can_dlc > CAN_MAX_DLEN)
+    {
+        msg.type = ArmMsgType::Unknown;
+        return false;
+    }
+    uint32_t can_id = rx_frame.can_id;
     const uint8_t *data = rx_frame.data;
     auto can_enum_id = static_cast<CanIDPiper>(can_id);
     msg.can_id = can_id;
@@ -73,7 +104,14 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times
         msg.type = ArmMsgType::Unknown;
         return false;
     }
-    std::copy(std::begin(rx_frame.data), std::end(rx_frame.data), std::begin(msg.raw_data));
+    if (rx_frame.can_dlc < requiredDlc(can_enum_id))
+    {
+        msg.type = ArmMsgType::Unknown;
+        return false;
+    }
+    // Only the received bytes are real; the rest of the buffer is whatever the frame carried.
+    std::copy(data, data + rx_frame.can_dlc, std::begin(msg.raw_data));
+    std::fill(std::begin(msg.raw_data) + rx_frame.can_dlc, std::end(msg.raw_data), 0);
     msg.timestamp = timestamp;
 
     // ----------------- Main switch -----------------
@@ -234,6 +272,7 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times
         msg.arm_motion_ctrl_2.move_spd_rate_ctrl = data[2];
         msg.arm_motion_ctrl_2.mit_mode = data[3];
         msg.arm_motion_ctrl_2.residence_time = data[4];
+        msg.arm_motion_ctrl_2.installation_pos = data[5];
         break;
     }
 
@@ -280,7 +319,7 @@ bool PiperParserV2::decodeMessage(const struct can_frame &rx_frame, double times
     {
         msg.type = ArmMsgType::FirmwareRead;
         // Firmware data is just raw; store in vector for easy use
-        msg.firmware_data.assign(data, data + PiperMessage::raw_data_len);
+        msg.firmware_data.assign(data, data + rx_frame.can_dlc);
         break;
     }
 
@@ -306,9 +345,6 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_
     bool ret = true;
     memset(tx_frame.data, 0, 8);
 
-    // Always set CAN ID based on type for robustness
-    tx_frame.can_id = msg.can_id;
-
     switch (msg.type)
     {
     // --- Motion Ctrl 1 ---
@@ -498,6 +534,13 @@ bool PiperParserV2::encodeMessage(const PiperMessage &msg, struct can_frame &tx_
         break;
     }
 
+    if (ret)
+    {
+        // Every Piper command is an 8-byte standard frame whose ID follows from the type, so
+        // callers only fill in the payload fields.
+        tx_frame.can_id = static_cast<canid_t>(msgTypeToCanId(msg.type));
+        tx_frame.can_dlc = 8;
+    }
     return ret;
 }
 

piper_cpp/test/CMakeLists.txt

@@ -12,3 +12,4 @@ endfunction()
 
 add_piper_test(std_can_interface_test)
 add_piper_test(piper_interface_v2_test)
+add_piper_test(piper_parser_v2_test)

piper_cpp/test/piper_interface_v2_test.cpp

@@ -5,8 +5,10 @@
 
 #include <gtest/gtest.h>
 
+#include <algorithm>
 #include <chrono>
 #include <future>
+#include <initializer_list>
 #include <memory>
 #include <sys/socket.h>
 #include <thread>
@@ -50,6 +52,35 @@ can_frame jointFeedbackFrame(uint32_t id, int32_t a, int32_t b)
     return f;
 }
 
+can_frame frame(uint32_t id, std::initializer_list<uint8_t> bytes)
+{
+    can_frame f{};
+    f.can_id = id;
+    f.can_dlc = static_cast<uint8_t>(bytes.size());
+    std::copy(bytes.begin(), bytes.end(), f.data);
+    return f;
+}
+
+// 0x473 reply: motor index, then max angle, min angle and max speed as big-endian int16.
+can_frame angleLimitReply(uint8_t motor, int16_t max_angle)
+{
+    const auto hi = static_cast<uint8_t>(max_angle >> 8);
+    const auto lo = static_cast<uint8_t>(max_angle);
+    return frame(0x473, {motor, hi, lo, 0, 0, 0, 0});
+}
+
+template <typename Pred> bool waitFor(Pred pred, std::chrono::milliseconds timeout = 2s)
+{
+    const auto deadline = steady_clock::now() + timeout;
+    while (!pred())
+    {
+        if (steady_clock::now() > deadline)
+            return false;
+        std::this_thread::sleep_for(1ms);
+    }
+    return true;
+}
+
 void sendFrame(int fd, const can_frame& f, steady_clock::time_point deadline)
 {
     while (::send(fd, &f, sizeof(f), MSG_DONTWAIT) < 0 && errno == EAGAIN && steady_clock::now() < deadline)
@@ -104,3 +135,54 @@ TEST(PiperInterfaceV2, ReadLoopKeepsUpWithBurst)
     EXPECT_EQ(js.value.joint_5, 3 * kSets);
     EXPECT_EQ(js.value.joint_6, -3 * kSets);
 }
+
+TEST(PiperInterfaceV2, AllMotorAngleLimitsValidOnlyAfterEveryMotorReplies)
+{
+    auto bus = makeBus();
+    ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true));
+    const auto deadline = steady_clock::now() + 5s;
+
+    // Out-of-range motor indices must be ignored by both states (index 0 used to write motor[-1]).
+    sendFrame(bus.peer, angleLimitReply(0, 999), deadline);
+    sendFrame(bus.peer, angleLimitReply(7, 999), deadline);
+    std::this_thread::sleep_for(50ms);
+    EXPECT_FALSE(bus.piper->getArmCurrentMotorAngleLimitMaxSpd().is_valid);
+    for (uint8_t motor = 1; motor <= 5; ++motor)
+        sendFrame(bus.peer, angleLimitReply(motor, motor * 100), deadline);
+    ASSERT_TRUE(waitFor([&] { return bus.piper->getArmCurrentMotorAngleLimitMaxSpd().value.motor_num == 5; }));
+    EXPECT_FALSE(bus.piper->getArmAllMotorAngleLimitMaxSpd().is_valid) << "five of six replies is not a complete set";
+
+    sendFrame(bus.peer, angleLimitReply(6, 600), deadline);
+    ASSERT_TRUE(waitFor([&] { return bus.piper->getArmAllMotorAngleLimitMaxSpd().is_valid; }));
+    const auto all = bus.piper->getArmAllMotorAngleLimitMaxSpd().value;
+    for (int i = 0; i < 6; ++i)
+    {
+        EXPECT_EQ(all.motor[i].motor_num, i + 1);
+        EXPECT_EQ(all.motor[i].max_angle_limit, (i + 1) * 100);
+    }
+}
+
+TEST(PiperInterfaceV2, AllMotorAccLimitsValidOnlyAfterEveryMotorReplies)
+{
+    auto bus = makeBus();
+    ASSERT_TRUE(bus.piper->connectPort(/*can_init=*/false, /*piper_init=*/false, /*start_threads=*/true));
+    const auto deadline = steady_clock::now() + 5s;
+
+    sendFrame(bus.peer, frame(0x47C, {0, 0, 9}), deadline);
+    sendFrame(bus.peer, frame(0x47C, {7, 0, 9}), deadline);
+    std::this_thread::sleep_for(50ms);
+    EXPECT_FALSE(bus.piper->getArmCurrentMotorMaxAccLimit().is_valid);
+    for (uint8_t motor = 1; motor <= 5; ++motor)
+        sendFrame(bus.peer, frame(0x47C, {motor, 0, motor}), deadline);
+    ASSERT_TRUE(waitFor([&] { return bus.piper->getArmCurrentMotorMaxAccLimit().value.joint_motor_num == 5; }));
+    EXPECT_FALSE(bus.piper->getArmAllMotorMaxAccLimit().is_valid) << "five of six replies is not a complete set";
+
+    sendFrame(bus.peer, frame(0x47C, {6, 0, 6}), deadline);
+    ASSERT_TRUE(waitFor([&] { return bus.piper->getArmAllMotorMaxAccLimit().is_valid; }));
+    const auto all = bus.piper->getArmAllMotorMaxAccLimit().value;
+    for (int i = 0; i < 6; ++i)
+    {
+        EXPECT_EQ(all.motor[i].joint_motor_num, i + 1);
+        EXPECT_EQ(all.motor[i].max_joint_acc, i + 1);
+    }
+}