diff --git a/components/basicmicro/include/basicmicro.hpp b/components/basicmicro/include/basicmicro.hpp index ebf96893e..72d14448e 100644 --- a/components/basicmicro/include/basicmicro.hpp +++ b/components/basicmicro/include/basicmicro.hpp @@ -818,6 +818,9 @@ class Basicmicro : public BaseComponent { std::error_code ec32; if (read_command(Command::ReadStatus, data, ec32)) { status = detail::read_u32_be(data, 0); + // The 32-bit attempt used a local ec32; honor the "true => ec cleared" + // contract so a caller reusing ec sees success. + ec.clear(); return true; } } @@ -992,9 +995,12 @@ class Basicmicro : public BaseComponent { bool set_velocity_pid(Command cmd, float p, float i, float d, uint32_t qpps, std::error_code &ec) { std::vector payload; - detail::append_u32_be(payload, static_cast(d * detail::kBasicmicroPidScale)); - detail::append_u32_be(payload, static_cast(p * detail::kBasicmicroPidScale)); - detail::append_u32_be(payload, static_cast(i * detail::kBasicmicroPidScale)); + // Route through scale_pid_gain (rounds; guards negative -> uint32 wrap and + // NaN/inf -> UB in std::llround) just like the position path — a raw + // static_cast of the float product bypassed those guards. + detail::append_u32_be(payload, detail::scale_pid_gain(d, detail::kBasicmicroPidScale)); + detail::append_u32_be(payload, detail::scale_pid_gain(p, detail::kBasicmicroPidScale)); + detail::append_u32_be(payload, detail::scale_pid_gain(i, detail::kBasicmicroPidScale)); detail::append_u32_be(payload, qpps); return write_command(cmd, payload, ec); } diff --git a/components/canopen/include/canopen_client.hpp b/components/canopen/include/canopen_client.hpp index 5c6b4d711..f85c0ac37 100644 --- a/components/canopen/include/canopen_client.hpp +++ b/components/canopen/include/canopen_client.hpp @@ -78,7 +78,15 @@ class CanopenClient : public BaseComponent { , node_id_(config.node_id) , send_(config.send) , sdo_timeout_(config.sdo_timeout) - , on_heartbeat_(config.on_heartbeat) {} + , on_heartbeat_(config.on_heartbeat) { + // A CANopen node id is 1-127; 0 is the broadcast/unconfigured value and would + // make SDO addressing (0x580/0x600 + id) and heartbeat matching wrong. + if (node_id_ < 1 || node_id_ > 127) { + logger_.error("node_id {} is out of range (1-127); clamping to 1 — set a valid node id", + static_cast(node_id_)); + node_id_ = 1; + } + } /// \brief The configured server node id. uint8_t node_id() const { return node_id_; } @@ -276,9 +284,16 @@ class CanopenClient : public BaseComponent { /// \param index Object dictionary index. /// \param subindex Object dictionary subindex. /// \param out Destination for the object data (little-endian). - /// \param ec Set on transmit failure, timeout, SDO abort, or if the object is - /// larger than \p out (use read_string() for segmented transfers). + /// \param ec Set on transmit failure, timeout, SDO abort, or a size mismatch + /// (use read_string() for segmented transfers). /// \return Number of bytes read (> 0), or 0 on error. + /// \note Size handling depends on whether the server indicated the object size + /// in its response. When it did, an object larger than \p out is rejected + /// as a width mismatch (ec = protocol_error). When it did NOT (CiA 301 + /// allows this for expedited transfers, where all four data bytes are + /// valid), the low \p out.size() bytes are returned and any remaining + /// high bytes are truncated -- so a caller must size \p out to the width + /// it expects for such objects. size_t sdo_upload(uint16_t index, uint8_t subindex, std::span out, std::error_code &ec) { std::lock_guard lock(sdo_mutex_); detail::canopen::SdoResponse response; @@ -286,15 +301,37 @@ class CanopenClient : public BaseComponent { index, subindex, ec)) { return 0; } - if (response.type != detail::canopen::SdoResponse::Type::ExpeditedUpload || - response.len > out.size()) { - logger_.error("SDO upload 0x{:04X}:{:02X}: not an expedited response of <= {} bytes", index, - subindex, out.size()); + if (response.type != detail::canopen::SdoResponse::Type::ExpeditedUpload) { + logger_.error("SDO upload 0x{:04X}:{:02X}: not an expedited response", index, subindex); + ec = std::make_error_code(std::errc::protocol_error); + return 0; + } + // When the server INDICATED a size, the object must fit the caller's buffer + // (a larger object is a real width mismatch -> error below). When it did NOT + // indicate a size, CiA 301 says all four expedited data bytes are valid and + // the caller's requested width governs, so take the low out.size() bytes — + // otherwise a conformant u8/u16 read against a server that leaves the size + // bit clear (where the core reports len == 4) would spuriously fail. + const size_t n = + (response.size_indicated || response.len <= out.size()) ? response.len : out.size(); + if (n > out.size()) { + logger_.error( + "SDO upload 0x{:04X}:{:02X}: object is {} bytes, larger than the {}-byte buffer", index, + subindex, response.len, out.size()); + ec = std::make_error_code(std::errc::protocol_error); + return 0; + } + // Defensive: never read past the fixed-size expedited data buffer even if a + // malformed frame or parser bug reported a length the parser should have + // capped at 4 (guards the copy_n source, not just the out destination). + if (n > response.data.size()) { + logger_.error("SDO upload 0x{:04X}:{:02X}: reported length {} exceeds the {}-byte payload", + index, subindex, response.len, response.data.size()); ec = std::make_error_code(std::errc::protocol_error); return 0; } - std::copy_n(response.data.begin(), response.len, out.begin()); - return response.len; + std::copy_n(response.data.begin(), n, out.begin()); + return n; } /// \brief Read a string object via SDO segmented (or expedited) upload. @@ -449,6 +486,9 @@ class CanopenClient : public BaseComponent { { std::lock_guard lock(response_mutex_); awaiting_response_ = true; + // Clear any abort code cached by a previous transaction so last_abort_code() + // never reports a stale code from an earlier, unrelated failure. + last_abort_code_ = 0; // Record what the in-flight request is for, so process_frame() can // reject stale/unrelated responses instead of completing the wrong // transaction (segment responses carry no index/subindex and are