Skip to content
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

Jump to bottom

WIP: Sdo info support #93

Open
wants to merge 6 commits into
base: master
Choose a base branch
from
Open

WIP: Sdo info support #93

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
e61b862
Add sdo info messages
Apr 18, 2023
fb91e31
Add struct to parse SDO info responses
Apr 19, 2023
4c30691
WIP EntryDescription
Apr 20, 2023
cafb99b
get unit description
Apr 21, 2023
2b06c13
clean up and fix get od list display
Apr 24, 2023
efbe925
Fix fragmented
Apr 24, 2023
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
32 changes: 30 additions & 2 deletions examples/elmo_control/elmo_control.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -78,6 +78,9 @@ int main(int argc, char* argv[])
uint32_t tx_length = sizeof(pdo::tx_mapping);
bus.writeSDO(bus.slaves().at(0), 0x1C13, 0, Bus::Access::COMPLETE, const_cast<uint8_t*>(pdo::tx_mapping), tx_length); // 0x1C13 refers to CoE::SM_CHANNEL + 3, subindex 0

bus.writeSDO(bus.slaves().at(1), 0x1C12, 0, Bus::Access::COMPLETE, const_cast<uint8_t*>(pdo::rx_mapping), rx_length);
bus.writeSDO(bus.slaves().at(1), 0x1C13, 0, Bus::Access::COMPLETE, const_cast<uint8_t*>(pdo::tx_mapping), tx_length);

bus.createMapping(io_buffer);

bus.requestState(State::SAFE_OP);
Expand Down Expand Up @@ -124,7 +127,7 @@ int main(int argc, char* argv[])

link->setTimeout(500us);

Slave& elmo = bus.slaves().at(0);
Slave& elmo = bus.slaves().at(1);
pdo::Output* output_pdo = reinterpret_cast<pdo::Output*>(elmo.output.data);
pdo::Input* input_pdo = reinterpret_cast<pdo::Input*>(elmo.input.data);
CANOpenStateMachine elmo_state_machine;
Expand All @@ -133,12 +136,37 @@ int main(int argc, char* argv[])

// Setting a small torque
output_pdo->mode_of_operation = 4;
output_pdo->target_torque = 30;
output_pdo->target_torque = 3;
output_pdo->max_torque = 3990;
output_pdo->target_position = 0;
output_pdo->velocity_offset = 0;
output_pdo->digital_output = 0;

uint8_t buffer[4096];
uint32_t data_size = 4096;

// printf("Elmo mailbox size %u \n", elmo.mailbox.recv_size);
bus.getObjectDictionnaryList(elmo, CoE::SDO::information::ListType::AllObjects, buffer, &data_size);

CoE::SDO::information::ValueInfo value_info{7,1,1,1,1, 1};
try
{
bus.getEntryDescription(elmo, 0x607f, 0, value_info, buffer, &data_size);
}
catch (ErrorCode const& e)
{
std::cerr << e.what() << " " << e.code() << std::endl;
return 1;
}

CoE::UnitType unit_test;
unit_test.denominator = 0x48;
unit_test.numerator = 0x01;
unit_test.prefix = 0x03;
bus.getUnitDescription(elmo, unit_test);

exit(0);

constexpr int64_t LOOP_NUMBER = 12 * 3600 * 1000; // 12h
int64_t last_error = 0;
for (int64_t i = 0; i < LOOP_NUMBER; ++i)
Expand Down
30 changes: 29 additions & 1 deletion examples/ingenia_control/ingenia_control.cc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -101,6 +101,11 @@ int main(int argc, char *argv[])
// Map RXPDO
mapPDO(0, 0x1600, pdo::rx_mapping, pdo::rx_mapping_count, 0x1C12);

// Map TXPDO
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

copy/paste error?

mapPDO(1, 0x1A00, pdo::tx_mapping, pdo::tx_mapping_count, 0x1C13);
// Map RXPDO
mapPDO(1, 0x1600, pdo::rx_mapping, pdo::rx_mapping_count, 0x1C12);

bus.createMapping(io_buffer);

bus.requestState(State::SAFE_OP);
Expand Down Expand Up @@ -158,10 +163,33 @@ int main(int argc, char *argv[])

// Setting a small torque
output_pdo->mode_of_operation = 0x5;
output_pdo->target_torque = 3;
output_pdo->target_torque = 0.01;
output_pdo->max_current = 3990;
output_pdo->target_position = 0;


// uint32_t size = 4;
// uint32_t vendorID;
// bus.readSDO(ingenia, 0x1018, 0x01, Bus::Access::PARTIAL, &vendorID, &size);
// printf("Direct vendor id %x \n", vendorID);


uint8_t buffer[4096];
uint32_t data_size = 4096;

// bus.getObjectDictionnaryList(ingenia, CoE::SDO::information::ListType::AllObjects, buffer, &data_size);
// bus.getObjectDescription(ingenia, 0x2018, buffer, &data_size);

CoE::SDO::information::ValueInfo value_info{7,1,1,1,1, 1};
bus.getEntryDescription(ingenia, 0x2018, 0, value_info, buffer, &data_size);

// CoE::UnitType unit_test;
// unit_test.denominator = 0x48;
// unit_test.numerator = 0x01;
// unit_test.prefix = 0x03;
// bus.getUnitDescription(ingenia, unit_test);

std::abort();
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

such violence 🤣

constexpr int64_t LOOP_NUMBER = 12 * 3600 * 1000; // 12h
int64_t last_error = 0;
for (int64_t i = 0; i < LOOP_NUMBER; ++i)
Expand Down
9 changes: 9 additions & 0 deletions include/kickcat/Bus.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -98,6 +98,15 @@ namespace kickcat
void readSDO (Slave& slave, uint16_t index, uint8_t subindex, Access CA, void* data, uint32_t* data_size, nanoseconds timeout = 1s);
void writeSDO(Slave& slave, uint16_t index, uint8_t subindex, bool CA, void* data, uint32_t data_size, nanoseconds timeout = 1s);

void getObjectDictionnaryList(Slave& slave, CoE::SDO::information::ListType type, void* data, uint32_t* data_size, nanoseconds timeout = 1s);
void getObjectDescription(Slave& slave, uint16_t index, void* data, uint32_t* data_size, nanoseconds timeout = 1s);
void getEntryDescription(Slave& slave, uint16_t index, uint8_t subindex, CoE::SDO::information::ValueInfo value_info,
void* data, uint32_t* data_size, nanoseconds timeout = 1s);

// TODO move to helpers.
void getUnitDescription(Slave& slave, CoE::UnitType unit);


/// \brief Add a gateway message to the bus
/// \param raw_message A raw EtherCAT mailbox message
/// \param raw_message_size Size of the mailbox message (shall be less or equal of the actual storage size)
Expand Down
28 changes: 28 additions & 0 deletions include/kickcat/Mailbox.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,14 @@ namespace kickcat
std::shared_ptr<AbstractMessage> createSDO(uint16_t index, uint8_t subindex, bool CA, uint8_t request, void* data, uint32_t* data_size, nanoseconds timeout = 20ms);
std::shared_ptr<GatewayMessage> createGatewayMessage(uint8_t const* raw_message, int32_t raw_message_size, uint16_t gateway_index, nanoseconds timeout = 20ms);

std::shared_ptr<AbstractMessage> createSDOInfoGetODList(CoE::SDO::information::ListType type, void* data,
uint32_t* data_size, nanoseconds timeout = 20ms);
std::shared_ptr<AbstractMessage> createSDOInfoGetOD(uint16_t index, void* data, uint32_t* data_size,
nanoseconds timeout = 20ms);
std::shared_ptr<AbstractMessage> createSDOInfoGetED(uint16_t index, uint8_t subindex,
CoE::SDO::information::ValueInfo value_info,
void* data, uint32_t* data_size, nanoseconds timeout = 20ms);

// helper to get next message to send and transfer it to reception callbacks if required
std::shared_ptr<AbstractMessage> send();

Expand Down Expand Up @@ -142,6 +150,26 @@ namespace kickcat
uint32_t* client_data_size_;
};

class SDOInfoMessage : public AbstractMessage
{
public:
Copy link
Owner

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

indentation

SDOInfoMessage(uint16_t mailbox_size, uint8_t request, void* data, uint32_t* data_size, uint32_t request_payload_size, nanoseconds timeout);
virtual ~SDOInfoMessage() = default;

ProcessingResult process(uint8_t const* received) override;

protected:
ProcessingResult processSDOInfoResponse(mailbox::Header const* header, CoE::ServiceDataInfo const* sdo,
uint8_t const* payload, uint8_t expected_opcode);

CoE::Header* coe_;
CoE::ServiceDataInfo* sdo_;
uint8_t* payload_;
uint8_t* client_data_;
uint32_t* client_data_size_;
uint32_t already_received_size_{0};
};

class EmergencyMessage : public AbstractMessage
{
public:
Expand Down
178 changes: 173 additions & 5 deletions include/kickcat/protocol.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -453,9 +453,9 @@ namespace kickcat
struct ServiceDataInfo // ETG1000.6 chapter 5.6.3 SDO Information
{
uint16_t opcode : 7,
incomplete : 1,
reserved : 8;
uint16_t index;
incomplete : 1,
reserved : 8;
uint16_t fragments_left;
} __attribute__((__packed__));

struct Emergency // ETG1000.6 chapter 5.6.4 Emergency
Expand All @@ -477,6 +477,102 @@ namespace kickcat
SDO_INFORMATION = 0x08
};

// ETG 1000.5 chapter 5 Data type ASE (Application Service Element) and ETG 1020 Base Data Types
enum DataType: uint16_t
{
Boolean = 0x0001,

Byte = 0x001E,
Word = 0x001F,
Dword = 0x0020,

BIT2 = 0x0031,
BIT3 = 0x0032,
BIT4 = 0x0033,
BIT5 = 0x0034,
BIT6 = 0x0035,
BIT7 = 0x0036,
BIT8 = 0x0037,
BIT9 = 0x0038,
BIT10 = 0x0039,
BIT11 = 0x003A,
BIT12 = 0x003B,
BIT13 = 0x003C,
BIT14 = 0x003D,
BIT15 = 0x003E,
BIT16 = 0x003F,

BITARR8 = 0x002D,
BITARR16 = 0x002E,
BITARR32 = 0x002F,

TimeOfDay = 0x000C,
TimeDifference = 0x000D,

Float32 = 0x0008,
Float64 = 0x0011,

Integer8 = 0x0002,
Integer16 = 0x0003,
Integer24 = 0x0010,
Integer32 = 0x0004,
Integer40 = 0x0012,
Integer48 = 0x0013,
Integer56 = 0x0014,
Integer64 = 0x0015,

Unsigned8 = 0x0005,
Unsigned16 = 0x0006,
Unsigned24 = 0x0016,
Unsigned32 = 0x0007,
Unsigned40 = 0x0018,
Unsigned48 = 0x0019,
Unsigned56 = 0x001A,
Unsigned64 = 0x001B,

VisibleString = 0x0009,
OctetString = 0x000A,
UnicodeString = 0x000B,
GUID = 0x001D,

ArrayOfInt = 0x0260,
ArrayOfSInt = 0x0261,
ArrayOfDInt = 0x0262,
ArrayOfUDInt = 0x0263,

PDOMapping = 0x0021,
Identity = 0x0023,
CommandPar = 0x0025,
PDOParameter = 0x0027,
Enum = 0x0028,
SMSynchronisation = 0x0029,
Record = 0x002A,
BackupParameter = 0x002B,
ModularDeviceProfile = 0x002C,
ErrorSetting = 0x0281,
DiagnosisHistory = 0x0282,
ExternalSyncStatus = 0x0283,
ExternalSyncSettings = 0x0284,
DefTypeFSOEFrame = 0x0285,
DefTypeFSOECommPar = 0x0286
};
std::string toString(DataType data_type);


/// ETG1004 extension unit_specification
/// For the textual description of the unit entry, the entry description shall be a definition in the object area
/// 0x0400 … 0x04FF. The entry description itself is defined as a record of the UNIT type (0x2A) which is
/// read-only in the object dictionary.

constexpr uint32_t UNIT_OFFSET = 0x0400;
struct UnitType
{
uint8_t reserved;
uint8_t denominator;
uint8_t numerator;
int8_t prefix;
};

namespace SDO
{
// Command specifiers depending on SDO request type
Expand All @@ -503,9 +599,81 @@ namespace kickcat
constexpr uint8_t GET_OD_LIST_RESP = 0x02;
constexpr uint8_t GET_OD_REQ = 0x03;
constexpr uint8_t GET_OD_RESP = 0x04;
constexpr uint8_t GET_ED_LIST_REQ = 0x05;
constexpr uint8_t GET_ED_LIST_RESP = 0x06;
constexpr uint8_t GET_ED_REQ = 0x05;
constexpr uint8_t GET_ED_RESP = 0x06;
constexpr uint8_t SDO_INFO_ERROR_REQ = 0x07;

enum ListType : uint16_t
{
NumberOfObjects = 0x00,
AllObjects = 0x01,
RxPDO = 0x02,
TxPDO = 0x03,
DeviceReplacement = 0x04,
StartupParameters = 0x05
};

enum ObjectCode : uint8_t
{
Variable = 7,
Array = 8,
Record = 9
};
std::string toString(ObjectCode object_code);

struct ObjectAccess
{
uint16_t read_pre_operational: 1,
read_safe_operational: 1,
read_operational: 1,
write_pre_operational: 1,
write_safe_operational: 1,
write_operational: 1,
mappable_RxPDO: 1,
mappable_TxPDO: 1,
backup: 1,
settings: 1,
reserved: 6;
} __attribute__((__packed__));
std::string toString(ObjectAccess object_access);

struct ValueInfo
{
uint8_t reserved : 3,
unit_type : 1,
default_value : 1,
minimum_value: 1,
maximum_value: 1,
unused : 1;
} __attribute__((__packed__));
std::string toString(ValueInfo const& value_description);

struct EntryDescriptionRequest
{
uint16_t index;
uint8_t subindex;
ValueInfo value_info;
} __attribute__((__packed__));

struct ObjectDescription
{
uint16_t index;
DataType data_type;
uint8_t max_subindex;
CoE::SDO::information::ObjectCode object_code;
} __attribute__((__packed__));
std::string toString(ObjectDescription const& object_description, std::string const& name = "");

struct EntryDescription
{
uint16_t index;
uint8_t subindex;
ValueInfo value_info;
DataType data_type;
uint16_t bit_length;
ObjectAccess object_access;
} __attribute__((__packed__));
std::string toString(EntryDescription const& entry_description, uint8_t* data, uint32_t data_size);
}

char const* abort_to_str(uint32_t abort_code);
Expand Down
Loading