cia402.cpp

Go to the documentation of this file.

// Copyright (C) 2026 Douglas Quigg (dstroy0) <dquigg123@gmail.com>
// SPDX-License-Identifier: AGPL-3.0-or-later
 
/**
 * @file cia402.cpp
 * @brief CiA 402 drive profile: state machine + Controlword/Statusword + CANopen object access.
 */
 
#include "services/cia402/cia402.h"
 
#if DETWS_ENABLE_CIA402
 
#include <string.h>
 
// CANopen object values are little-endian.
static size_t put16le(uint8_t *p, uint16_t v)
{
    p[0] = (uint8_t)(v & 0xFF);
    p[1] = (uint8_t)(v >> 8);
    return 2;
}
 
static size_t put32le(uint8_t *p, uint32_t v)
{
    p[0] = (uint8_t)(v & 0xFF);
    p[1] = (uint8_t)((v >> 8) & 0xFF);
    p[2] = (uint8_t)((v >> 16) & 0xFF);
    p[3] = (uint8_t)((v >> 24) & 0xFF);
    return 4;
}
 
static uint16_t get16le(const uint8_t *p)
{
    return (uint16_t)(p[0] | ((uint16_t)p[1] << 8));
}
 
static uint32_t get32le(const uint8_t *p)
{
    return (uint32_t)p[0] | ((uint32_t)p[1] << 8) | ((uint32_t)p[2] << 16) | ((uint32_t)p[3] << 24);
}
 
Cia402State cia402_state(uint16_t sw)
{
    // Mask/value table from IEC 61800-7-201 (CiA 402). Order matters where masks differ.
    if ((sw & 0x4F) == 0x00)
        return Cia402State::not_ready_to_switch_on;
    if ((sw & 0x4F) == 0x40)
        return Cia402State::switch_on_disabled;
    if ((sw & 0x6F) == 0x21)
        return Cia402State::ready_to_switch_on;
    if ((sw & 0x6F) == 0x23)
        return Cia402State::switched_on;
    if ((sw & 0x6F) == 0x27)
        return Cia402State::operation_enabled;
    if ((sw & 0x6F) == 0x07)
        return Cia402State::quick_stop_active;
    if ((sw & 0x4F) == 0x0F)
        return Cia402State::fault_reaction_active;
    if ((sw & 0x4F) == 0x08)
        return Cia402State::fault;
    return Cia402State::unknown;
}
 
uint16_t cia402_controlword(Cia402Command cmd)
{
    switch (cmd)
    {
    case Cia402Command::shutdown:
        return 0x0006; // enable voltage + quick stop, switch-on 0
    case Cia402Command::switch_on:
    case Cia402Command::disable_operation:
        return 0x0007; // switch-on + enable voltage + quick stop
    case Cia402Command::enable_operation:
        return 0x000F; // + enable operation
    case Cia402Command::disable_voltage:
        return 0x0000;
    case Cia402Command::quick_stop:
        return 0x0002; // enable voltage, quick-stop bit cleared (active)
    case Cia402Command::fault_reset:
        return 0x0080; // bit 7 rising edge
    }
    return 0x0000;
}
 
uint16_t cia402_enable_sequence(Cia402State state)
{
    switch (state)
    {
    case Cia402State::fault:
    case Cia402State::fault_reaction_active:
        return cia402_controlword(Cia402Command::fault_reset);
    case Cia402State::switch_on_disabled:
        return cia402_controlword(Cia402Command::shutdown);
    case Cia402State::ready_to_switch_on:
        return cia402_controlword(Cia402Command::switch_on);
    case Cia402State::switched_on:
    case Cia402State::quick_stop_active:
    case Cia402State::operation_enabled:
        return cia402_controlword(Cia402Command::enable_operation);
    default: // not_ready_to_switch_on / unknown: wait, hold voltage off
        return cia402_controlword(Cia402Command::disable_voltage);
    }
}
 
bool cia402_sdo_set_controlword(CanFrame *out, uint8_t node, uint16_t controlword)
{
    uint8_t d[2];
    put16le(d, controlword);
    return canopen_build_sdo_write(out, node, CIA402_OD_CONTROLWORD, 0, d, 2);
}
 
bool cia402_sdo_set_mode(CanFrame *out, uint8_t node, Cia402Mode mode)
{
    uint8_t d = (uint8_t)(int8_t)mode; // wire byte
    return canopen_build_sdo_write(out, node, CIA402_OD_MODES_OF_OPERATION, 0, &d, 1);
}
 
bool cia402_sdo_set_target_position(CanFrame *out, uint8_t node, int32_t position)
{
    uint8_t d[4];
    put32le(d, (uint32_t)position);
    return canopen_build_sdo_write(out, node, CIA402_OD_TARGET_POSITION, 0, d, 4);
}
 
bool cia402_sdo_set_target_velocity(CanFrame *out, uint8_t node, int32_t velocity)
{
    uint8_t d[4];
    put32le(d, (uint32_t)velocity);
    return canopen_build_sdo_write(out, node, CIA402_OD_TARGET_VELOCITY, 0, d, 4);
}
 
bool cia402_sdo_set_target_torque(CanFrame *out, uint8_t node, int16_t torque)
{
    uint8_t d[2];
    put16le(d, (uint16_t)torque);
    return canopen_build_sdo_write(out, node, CIA402_OD_TARGET_TORQUE, 0, d, 2);
}
 
bool cia402_sdo_read(CanFrame *out, uint8_t node, uint16_t index, uint8_t sub)
{
    return canopen_build_sdo_read(out, node, index, sub);
}
 
// Validate an expedited SDO upload response and copy its inline payload into @p out (>= need
// octets). No shared state - the parsed response lives on this call's stack.
static bool sdo_upload_bytes(const CanFrame *f, uint16_t want_index, uint8_t need, uint8_t *out)
{
    CanopenSdoResponse resp;
    if (!canopen_parse_sdo_response(f, &resp))
        return false;
    if (resp.is_abort || !resp.is_upload || !resp.expedited || resp.len < need)
        return false;
    if (want_index != 0 && resp.index != want_index)
        return false;
    memcpy(out, resp.data, need);
    return true;
}
 
bool cia402_sdo_get_u16(const CanFrame *f, uint16_t want_index, uint16_t *value)
{
    uint8_t d[2];
    if (!value || !sdo_upload_bytes(f, want_index, 2, d))
        return false;
    *value = get16le(d);
    return true;
}
 
bool cia402_sdo_get_i32(const CanFrame *f, uint16_t want_index, int32_t *value)
{
    uint8_t d[4];
    if (!value || !sdo_upload_bytes(f, want_index, 4, d))
        return false;
    *value = (int32_t)get32le(d);
    return true;
}
 
size_t cia402_pack_command(uint8_t *buf, size_t cap, uint16_t controlword, int32_t target)
{
    if (!buf || cap < 6)
        return 0;
    size_t p = put16le(buf, controlword);
    p += put32le(buf + p, (uint32_t)target);
    return p; // 6
}
 
bool cia402_unpack_status(const uint8_t *buf, size_t len, uint16_t *statusword, int32_t *actual)
{
    if (!buf || !statusword || !actual || len < 6)
        return false;
    *statusword = get16le(buf);
    *actual = (int32_t)get32le(buf + 2);
    return true;
}
 
#endif // DETWS_ENABLE_CIA402