diff --git a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h b/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h index 8d0daa6..240e8cc 100644 --- a/piper_cpp/include/piper_cpp/interface/piper_interface_v2.h +++ b/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 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 getArmAllMotorMaxAccLimit() const { return StateSnapshot(arm_all_motor_max_acc_limit_); diff --git a/piper_cpp/include/piper_cpp/types/feedback/motors.h b/piper_cpp/include/piper_cpp/types/feedback/motors.h index 7d56657..2d0b77a 100644 --- a/piper_cpp/include/piper_cpp/types/feedback/motors.h +++ b/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()`. diff --git a/piper_cpp/src/interface/piper_interface_v2.cpp b/piper_cpp/src/interface/piper_interface_v2.cpp index a6e4760..538ba8c 100644 --- a/piper_cpp/src/interface/piper_interface_v2.cpp +++ b/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,11 +288,9 @@ 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: " @@ -300,27 +298,37 @@ void PiperInterfaceV2::parseCANFrame(const struct can_frame &frame, double times } 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 data(tx.data, tx.data + PiperMessage::raw_data_len); - - auto can_id = static_cast(msgTypeToCanId(msg.type)); - - if (!arm_can_->sendCanMessage(can_id, data)) + std::vector 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; diff --git a/piper_cpp/src/protocol/piper_parser_v2.cpp b/piper_cpp/src/protocol/piper_parser_v2.cpp index 8ec6d11..a3709dd 100644 --- a/piper_cpp/src/protocol/piper_parser_v2.cpp +++ b/piper_cpp/src/protocol/piper_parser_v2.cpp @@ -27,6 +27,30 @@ static int8_t bytesToInt8(const uint8_t *data) { return static_cast(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(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(msgTypeToCanId(msg.type)); + tx_frame.can_dlc = 8; + } return ret; } diff --git a/piper_cpp/test/CMakeLists.txt b/piper_cpp/test/CMakeLists.txt index 185c926..3786ecf 100644 --- a/piper_cpp/test/CMakeLists.txt +++ b/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) diff --git a/piper_cpp/test/piper_interface_v2_test.cpp b/piper_cpp/test/piper_interface_v2_test.cpp index a87a2d5..a60aa66 100644 --- a/piper_cpp/test/piper_interface_v2_test.cpp +++ b/piper_cpp/test/piper_interface_v2_test.cpp @@ -5,8 +5,10 @@ #include +#include #include #include +#include #include #include #include @@ -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 bytes) +{ + can_frame f{}; + f.can_id = id; + f.can_dlc = static_cast(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(max_angle >> 8); + const auto lo = static_cast(max_angle); + return frame(0x473, {motor, hi, lo, 0, 0, 0, 0}); +} + +template 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); + } +} diff --git a/piper_cpp/test/piper_parser_v2_test.cpp b/piper_cpp/test/piper_parser_v2_test.cpp new file mode 100644 index 0000000..1b59440 --- /dev/null +++ b/piper_cpp/test/piper_parser_v2_test.cpp @@ -0,0 +1,123 @@ +// Golden-frame tests for the V2 wire protocol: what the decoder accepts, how it interprets +// signed fields, and that the encoder produces complete frames on its own. + +#include "piper_cpp/protocol/piper_parser_v2.h" + +#include + +#include +#include +#include + +using piper_cpp::ArmMsgType; +using piper_cpp::PiperMessage; +using piper_cpp::PiperParserV2; + +namespace +{ + +can_frame frame(uint32_t id, std::initializer_list bytes) +{ + can_frame f{}; + f.can_id = id; + f.can_dlc = static_cast(bytes.size()); + std::copy(bytes.begin(), bytes.end(), f.data); + return f; +} + +} // namespace + +TEST(PiperParserV2, RejectsFramesThatAreNotStandardData) +{ + PiperParserV2 parser; + PiperMessage msg; + const auto joints = frame(0x2A5, {0, 0, 0, 1, 0, 0, 0, 2}); + ASSERT_TRUE(parser.decodeMessage(joints, 0.0, msg)); + + // Flags, and a reserved bit between the 11-bit ID and the flags, which masking would hide. + for (canid_t bit : {CAN_RTR_FLAG, CAN_EFF_FLAG, CAN_ERR_FLAG, canid_t{0x800}}) + { + can_frame f = joints; + f.can_id |= bit; + EXPECT_FALSE(parser.decodeMessage(f, 0.0, msg)) << "bit 0x" << std::hex << bit; + EXPECT_EQ(msg.type, ArmMsgType::Unknown); + } +} + +TEST(PiperParserV2, RejectsDlcAboveClassicFrameSize) +{ + PiperParserV2 parser; + PiperMessage msg; + // Firmware chunks are copied by DLC, so an oversize DLC would read past the buffer. + auto f = frame(0x4AF, {'S', '-', 'V', '1', '.', '5', '-', '2'}); + f.can_dlc = 9; + EXPECT_FALSE(parser.decodeMessage(f, 0.0, msg)); +} + +TEST(PiperParserV2, RejectsPayloadShorterThanTheFieldsRead) +{ + PiperParserV2 parser; + PiperMessage msg; + + // Joint feedback reads all eight bytes. + EXPECT_FALSE(parser.decodeMessage(frame(0x2A5, {}), 0.0, msg)); + EXPECT_FALSE(parser.decodeMessage(frame(0x2A5, {0, 0, 0, 1, 0, 0, 0}), 0.0, msg)); + EXPECT_TRUE(parser.decodeMessage(frame(0x2A5, {0, 0, 0, 1, 0, 0, 0, 2}), 0.0, msg)); + + // Gripper feedback reads seven, so a seven-byte frame is complete and six is not. + EXPECT_FALSE(parser.decodeMessage(frame(0x2A8, {0, 0, 0, 1, 0, 0}), 0.0, msg)); + EXPECT_TRUE(parser.decodeMessage(frame(0x2A8, {0, 0, 0, 1, 0, 0, 0}), 0.0, msg)); + + // Firmware text arrives in chunks of any length; only the bytes on the bus are kept, in + // both the typed field and the raw copy. + msg.raw_data[3] = 0xAA; // stale byte from a previous decode of the same message object + ASSERT_TRUE(parser.decodeMessage(frame(0x4AF, {'S', '-', 'V'}), 0.0, msg)); + EXPECT_EQ(msg.firmware_data, (std::vector{'S', '-', 'V'})); + EXPECT_EQ(msg.raw_data[2], 'V'); + EXPECT_EQ(msg.raw_data[3], 0); +} + +TEST(PiperParserV2, MotorCurrentIsSigned) +{ + PiperParserV2 parser; + PiperMessage msg; + // Joint 1 high-speed feedback: speed 0, current -100 mA (0xFF9C), position 0. + ASSERT_TRUE(parser.decodeMessage(frame(0x251, {0, 0, 0xFF, 0x9C, 0, 0, 0, 0}), 0.0, msg)); + EXPECT_EQ(msg.high_spd_feedbacks[0].current, -100); + EXPECT_LT(msg.high_spd_feedbacks[0].effort, 0.0f) << "derived torque must keep the sign of the current"; +} + +TEST(PiperParserV2, EncoderDerivesIdAndDlcFromType) +{ + PiperParserV2 parser; + PiperMessage msg; + msg.type = ArmMsgType::JointCtrl12; + msg.arm_joint_ctrl.joint_1 = 1000; + + can_frame tx{}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(tx.can_id, 0x155u); + EXPECT_EQ(tx.can_dlc, 8); + EXPECT_EQ(tx.data[0], 0x00); // 1000 = 0x000003E8, big-endian + EXPECT_EQ(tx.data[1], 0x00); + EXPECT_EQ(tx.data[2], 0x03); + EXPECT_EQ(tx.data[3], 0xE8); +} + +TEST(PiperParserV2, MotionCtrl2RoundTripsInstallationPos) +{ + PiperParserV2 parser; + PiperMessage msg; + msg.type = ArmMsgType::MotionCtrl2; + msg.arm_motion_ctrl_2.move_spd_rate_ctrl = 30; + msg.arm_motion_ctrl_2.installation_pos = 3; + + can_frame tx{}; + ASSERT_TRUE(parser.encodeMessage(msg, tx)); + EXPECT_EQ(tx.can_id, 0x151u); + + PiperMessage back; + ASSERT_TRUE(parser.decodeMessage(tx, 0.0, back)); + EXPECT_EQ(back.arm_motion_ctrl_2.move_spd_rate_ctrl, 30); + EXPECT_EQ(back.arm_motion_ctrl_2.installation_pos, 3); +}