Skip to content

feat(canopen): configurable axis object-offset in Ds402Drive for multi-axis drives #757

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
finger563 merged 1 commit into from
Sep 2, 2026
Merged

feat(canopen): configurable axis object-offset in Ds402Drive for multi-axis drives #757

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
34 changes: 34 additions & 0 deletions components/canopen/include/detail/canopen_core.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -471,6 +471,40 @@ inline constexpr uint16_t OBJ_PROFILE_DECELERATION = 0x6084; ///< Profile
inline constexpr uint16_t OBJ_TARGET_VELOCITY = 0x60FF; ///< Target velocity (i32).
/// @}

/// @name Object-index bounds used for per-axis offsetting
/// @{
/// Inclusive low bound of the CiA 402 device-profile object range. Objects in
/// [OBJ_DEVICE_PROFILE_MIN, OBJ_DEVICE_PROFILE_MAX] are the per-axis objects on
/// a multi-axis drive.
inline constexpr uint16_t OBJ_DEVICE_PROFILE_MIN = 0x6000;
/// Inclusive high bound of the CiA 402 device-profile object range.
inline constexpr uint16_t OBJ_DEVICE_PROFILE_MAX = 0x6FFF;
/// Largest representable 16-bit CANopen object index.
inline constexpr uint16_t OBJ_INDEX_MAX = 0xFFFF;
/// Largest axis object-offset that keeps every device-profile index within the
/// 16-bit index space (OBJ_INDEX_MAX - OBJ_DEVICE_PROFILE_MAX).
inline constexpr uint16_t MAX_AXIS_OBJECT_OFFSET = OBJ_INDEX_MAX - OBJ_DEVICE_PROFILE_MAX;

/// \brief Apply a per-axis object offset to a CiA 402 device-profile index.
/// \details Only indices in the device-profile range
/// [OBJ_DEVICE_PROFILE_MIN, OBJ_DEVICE_PROFILE_MAX] are offset (those
/// are the per-axis objects). Any index outside that range, or an
/// offset that would push the result past OBJ_INDEX_MAX, is returned
/// unchanged. Callers that must not silently fall back on overflow
/// should reject an offset > MAX_AXIS_OBJECT_OFFSET up front (see
/// Ds402Drive's constructor).
/// \param index The object index.
/// \param offset The per-axis offset (0 for the first/only axis).
/// \return The offset index, or \p index unchanged when not applicable.
inline constexpr uint16_t apply_axis_offset(uint16_t index, uint16_t offset) {
if (index < OBJ_DEVICE_PROFILE_MIN || index > OBJ_DEVICE_PROFILE_MAX) {
return index;
}
const uint32_t offset_index = static_cast<uint32_t>(index) + offset;
return offset_index <= OBJ_INDEX_MAX ? static_cast<uint16_t>(offset_index) : index;
}
/// @}

/// @name Controlword command values (CiA 402 §8.2.1)
/// @{
inline constexpr uint16_t CW_SHUTDOWN = 0x0006; ///< Shutdown -> Ready to switch on.
Expand Down
64 changes: 49 additions & 15 deletions components/canopen/include/ds402.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,7 +38,13 @@ class Ds402Drive : public BaseComponent {
struct Config {
std::chrono::milliseconds state_timeout{
1000}; ///< Timeout for each state transition / mode change to take effect.
std::chrono::milliseconds poll_period{10}; ///< Statusword polling period.
std::chrono::milliseconds poll_period{10}; ///< Statusword polling period.
uint16_t object_offset{0}; ///< Added to device-profile object indices (0x6000-0x6FFF) to
///< select an axis on a multi-axis drive: 0 for the first/only
///< axis, 0x800 for a second axis (e.g. Basicmicro MCP266 M2,
///< whose objects mirror M1 at +0x800). Communication and
///< identity objects (< 0x6000) are shared by the device and
///< never offset.
Comment thread
finger563 marked this conversation as resolved.
Logger::Verbosity log_level{Logger::Verbosity::WARN}; ///< Logger verbosity.
};

Expand All @@ -49,7 +55,20 @@ class Ds402Drive : public BaseComponent {
: BaseComponent("Ds402Drive", config.log_level)
, client_(client)
, state_timeout_(config.state_timeout)
, poll_period_(config.poll_period) {}
, poll_period_(config.poll_period)
, object_offset_(config.object_offset) {
// Validate the axis offset once, up front, rather than silently falling
// back to the un-offset (axis 1) index later: an offset large enough to
// push a device-profile object past the 16-bit index space is a
// misconfiguration, so reject it loudly and disable offsetting.
if (object_offset_ > detail::ds402::MAX_AXIS_OBJECT_OFFSET) {
logger_.error("object_offset 0x{:04X} exceeds the maximum 0x{:04X} (it would push a "
"device-profile index past 0x{:04X}); ignoring it and using axis 1",
object_offset_, detail::ds402::MAX_AXIS_OBJECT_OFFSET,
detail::ds402::OBJ_INDEX_MAX);
object_offset_ = 0;
}
}

/// \brief Create a DS402 drive helper with the default configuration.
/// \param client The CANopen client for the drive's node. Must outlive this object.
Expand Down Expand Up @@ -97,36 +116,38 @@ class Ds402Drive : public BaseComponent {
/// \param ec Set on failure. \return True on success.
bool set_controlword(uint16_t controlword, std::error_code &ec) {
logger_.debug("controlword <- 0x{:04X}", controlword);
return client_.write_u16(detail::ds402::OBJ_CONTROLWORD, 0, controlword, ec);
return client_.write_u16(axis_object(detail::ds402::OBJ_CONTROLWORD), 0, controlword, ec);
}
/// \brief Read the statusword (object 0x6041). \param ec Set on failure. \return The value.
uint16_t get_statusword(std::error_code &ec) {
return client_.read_u16(detail::ds402::OBJ_STATUSWORD, 0, ec);
return client_.read_u16(axis_object(detail::ds402::OBJ_STATUSWORD), 0, ec);
}
/// \brief Read the velocity actual value (object 0x606C). \param ec Set on failure.
/// \return The value.
int32_t get_velocity_actual(std::error_code &ec) {
return client_.read_i32(detail::ds402::OBJ_VELOCITY_ACTUAL, 0, ec);
return client_.read_i32(axis_object(detail::ds402::OBJ_VELOCITY_ACTUAL), 0, ec);
}
/// \brief Read the position actual value (object 0x6064). \param ec Set on failure.
/// \return The value.
int32_t get_position_actual(std::error_code &ec) {
return client_.read_i32(detail::ds402::OBJ_POSITION_ACTUAL, 0, ec);
return client_.read_i32(axis_object(detail::ds402::OBJ_POSITION_ACTUAL), 0, ec);
}
/// \brief Write the profile velocity (object 0x6081). \param velocity Value to write.
/// \param ec Set on failure. \return True on success.
bool set_profile_velocity(uint32_t velocity, std::error_code &ec) {
return client_.write_u32(detail::ds402::OBJ_PROFILE_VELOCITY, 0, velocity, ec);
return client_.write_u32(axis_object(detail::ds402::OBJ_PROFILE_VELOCITY), 0, velocity, ec);
}
/// \brief Write the profile acceleration (object 0x6083). \param acceleration Value to write.
/// \param ec Set on failure. \return True on success.
bool set_profile_acceleration(uint32_t acceleration, std::error_code &ec) {
return client_.write_u32(detail::ds402::OBJ_PROFILE_ACCELERATION, 0, acceleration, ec);
return client_.write_u32(axis_object(detail::ds402::OBJ_PROFILE_ACCELERATION), 0, acceleration,
ec);
}
/// \brief Write the profile deceleration (object 0x6084). \param deceleration Value to write.
/// \param ec Set on failure. \return True on success.
bool set_profile_deceleration(uint32_t deceleration, std::error_code &ec) {
return client_.write_u32(detail::ds402::OBJ_PROFILE_DECELERATION, 0, deceleration, ec);
return client_.write_u32(axis_object(detail::ds402::OBJ_PROFILE_DECELERATION), 0, deceleration,
ec);
}

/// @}
Expand Down Expand Up @@ -225,13 +246,14 @@ class Ds402Drive : public BaseComponent {
/// mode within the state timeout.
/// \return True once modes-of-operation-display matches.
bool set_mode(OperatingMode mode, std::error_code &ec) {
if (!client_.write_i8(detail::ds402::OBJ_MODES_OF_OPERATION, 0, static_cast<int8_t>(mode),
ec)) {
if (!client_.write_i8(axis_object(detail::ds402::OBJ_MODES_OF_OPERATION), 0,
static_cast<int8_t>(mode), ec)) {
return false;
}
const auto deadline = std::chrono::steady_clock::now() + state_timeout_;
do {
const auto display = client_.read_i8(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY, 0, ec);
const auto display =
client_.read_i8(axis_object(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY), 0, ec);
if (ec) {
return false;
}
Expand All @@ -249,7 +271,7 @@ class Ds402Drive : public BaseComponent {
/// \brief Read the mode of operation display (object 0x6061).
/// \param ec Set on failure. \return The reported mode.
int8_t get_mode_display(std::error_code &ec) {
return client_.read_i8(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY, 0, ec);
return client_.read_i8(axis_object(detail::ds402::OBJ_MODES_OF_OPERATION_DISPLAY), 0, ec);
}

/// @}
Expand All @@ -261,7 +283,7 @@ class Ds402Drive : public BaseComponent {
/// \param velocity Target velocity in device units.
/// \param ec Set on failure. \return True on success.
bool set_target_velocity(int32_t velocity, std::error_code &ec) {
return client_.write_i32(detail::ds402::OBJ_TARGET_VELOCITY, 0, velocity, ec);
return client_.write_i32(axis_object(detail::ds402::OBJ_TARGET_VELOCITY), 0, velocity, ec);
}

/// \brief Command a profile-position move (object 0x607A + new-set-point handshake).
Expand All @@ -277,7 +299,7 @@ class Ds402Drive : public BaseComponent {
/// \return True once the set-point was acknowledged and bit 4 released.
bool set_target_position(int32_t position, std::error_code &ec, bool immediate = true,
bool relative = false) {
if (!client_.write_i32(detail::ds402::OBJ_TARGET_POSITION, 0, position, ec)) {
if (!client_.write_i32(axis_object(detail::ds402::OBJ_TARGET_POSITION), 0, position, ec)) {
return false;
}
uint16_t controlword = detail::ds402::CW_ENABLE_OPERATION;
Expand Down Expand Up @@ -376,9 +398,21 @@ class Ds402Drive : public BaseComponent {
}
}

/// Apply the configured axis offset to a CiA 402 device-profile object index.
/// Only objects the helper knows to be axis-relative -- the standard
/// device-profile range (OBJ_DEVICE_PROFILE_MIN..OBJ_DEVICE_PROFILE_MAX) --
/// are offset; every other index (communication / identity objects below the
/// range, and anything above it) is not offset and is returned unchanged.
/// object_offset_ is validated in the constructor, so the addition never
/// overflows the 16-bit index space here.
uint16_t axis_object(uint16_t index) const {
return detail::ds402::apply_axis_offset(index, object_offset_);
}
Comment thread
finger563 marked this conversation as resolved.

CanopenClient &client_;
std::chrono::milliseconds state_timeout_;
std::chrono::milliseconds poll_period_;
uint16_t object_offset_;
};

} // namespace espp
39 changes: 39 additions & 0 deletions components/canopen/test/canopen_host_test.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -338,6 +338,44 @@ static void test_sdo_segmented_size_cap() {
CHECK(!b.consume(seg)); // 14 > 10 -> refused
}

// Ds402Drive's per-axis object offsetting (apply_axis_offset): device-profile
// objects (0x6000-0x6FFF) shift by the axis offset; everything else is
// returned unchanged; an offset that would overflow the 16-bit index space is
// not applied.
static void test_axis_offset() {
std::printf("test_axis_offset\n");
// named bounds are the expected CiA 402 device-profile range
CHECK(ds::OBJ_DEVICE_PROFILE_MIN == 0x6000);
CHECK(ds::OBJ_DEVICE_PROFILE_MAX == 0x6FFF);
CHECK(ds::OBJ_INDEX_MAX == 0xFFFF);
CHECK(ds::MAX_AXIS_OBJECT_OFFSET == 0x9000);

// offset 0 is the identity for every index
CHECK(ds::apply_axis_offset(ds::OBJ_CONTROLWORD, 0) == 0x6040);
CHECK(ds::apply_axis_offset(ds::OBJ_TARGET_POSITION, 0) == 0x607A);

// (1) device-profile objects are offset (M2 mirrors M1 at +0x800)
CHECK(ds::apply_axis_offset(ds::OBJ_CONTROLWORD, 0x800) == 0x6840);
CHECK(ds::apply_axis_offset(ds::OBJ_STATUSWORD, 0x800) == 0x6841);
CHECK(ds::apply_axis_offset(ds::OBJ_TARGET_POSITION, 0x800) == 0x687A);
CHECK(ds::apply_axis_offset(ds::OBJ_TARGET_VELOCITY, 0x800) == 0x68FF);
// the inclusive range boundaries
CHECK(ds::apply_axis_offset(0x6000, 0x800) == 0x6800);
CHECK(ds::apply_axis_offset(0x6FFF, 0x800) == 0x77FF);

// (2) objects outside the device-profile range are never offset
CHECK(ds::apply_axis_offset(ds::OBJ_DEVICE_TYPE, 0x800) == 0x1000); // 0x1000
CHECK(ds::apply_axis_offset(ds::OBJ_IDENTITY, 0x800) == 0x1018); // 0x1018
CHECK(ds::apply_axis_offset(0x5FFF, 0x800) == 0x5FFF); // just below range
CHECK(ds::apply_axis_offset(0x7000, 0x800) == 0x7000); // just above range

// (3) an offset that would push a device-profile index past 0xFFFF is not
// applied (returns the index unchanged rather than wrapping)
CHECK(ds::apply_axis_offset(0x6FFF, 0x9001) == 0x6FFF); // 0x6FFF + 0x9001 = 0x10000
CHECK(ds::apply_axis_offset(0x6FFF, 0x9000) == 0xFFFF); // exactly 0xFFFF still fits
CHECK(ds::apply_axis_offset(0x6000, 0xA000) == 0x6000); // overflow for this index
}

int main() {
test_nmt();
test_sync_and_pdo();
Expand All @@ -349,6 +387,7 @@ int main() {
test_sdo_segmented_size_cap();
test_sdo_segmented_upload();
test_le_helpers();
test_axis_offset();
test_ds402_decode();

if (g_failures == 0) {
Expand Down
Loading