diff --git a/include/co_api.h b/include/co_api.h index 4c5f773..d171fce 100644 --- a/include/co_api.h +++ b/include/co_api.h @@ -540,6 +540,40 @@ CO_EXPORT int co_error_clear (co_client_t * client, uint8_t mask); */ CO_EXPORT int co_error_get (co_client_t * client, uint8_t * error); +/** + * Read local dictionary object + * + * @param client client handle + * @param index index + * @param subindex subindex + * @param value result + * + * @return 0 on success, CO_STATUS error code otherwise + */ +CO_EXPORT int co_od_local_read ( + co_client_t * client, + uint16_t index, + uint8_t subindex, + uint64_t * value); + +/** + * Write local directory object + * + * @param client client handle + * @param index index + * @param subindex subindex + * @param value value + * @param pdo_event trigger PDOs that map this object + * + * @return 0 on success, CO_STATUS error code otherwise + */ +CO_EXPORT int co_od_local_write ( + co_client_t * client, + uint16_t index, + uint8_t subindex, + uint64_t value, + bool pdo_event); + #ifdef __cplusplus } #endif diff --git a/src/co_main.c b/src/co_main.c index b1d51ab..a10e691 100644 --- a/src/co_main.c +++ b/src/co_main.c @@ -33,6 +33,7 @@ #include "co_pdo.h" #include "co_sync.h" #include "co_emcy.h" +#include "co_od.h" #include "co_heartbeat.h" #include "co_node_guard.h" #include "co_lss.h" @@ -143,6 +144,10 @@ void co_main (void * arg) case CO_JOB_ERROR_GET: co_emcy_job (net, job); break; + case CO_JOB_LOCAL_READ: + case CO_JOB_LOCAL_WRITE: + co_od_job (net, job); + break; case CO_JOB_EXIT: running = false; break; @@ -378,6 +383,56 @@ int co_error_get (co_client_t * client, uint8_t * error) return job->result; } +int co_od_local_read ( + co_client_t * client, + uint16_t index, + uint8_t subindex, + uint64_t * value) +{ + co_net_t * net = client->net; + co_job_t * job = &client->job; + + job->client = client; + job->od.index = index; + job->od.subindex = subindex; + job->callback = co_job_callback; + job->type = CO_JOB_LOCAL_READ; + + os_mbox_post (net->mbox, job, OS_WAIT_FOREVER); + os_sem_wait (client->sem, OS_WAIT_FOREVER); + + if (job->result == 0) + { + *value = job->od.value; + } + + return job->result; +} + +int co_od_local_write ( + co_client_t * client, + uint16_t index, + uint8_t subindex, + uint64_t value, + bool pdo_event) +{ + co_net_t * net = client->net; + co_job_t * job = &client->job; + + job->client = client; + job->od.index = index; + job->od.subindex = subindex; + job->od.value = value; + job->od.pdo_event = pdo_event; + job->callback = co_job_callback; + job->type = CO_JOB_LOCAL_WRITE; + + os_mbox_post (net->mbox, job, OS_WAIT_FOREVER); + os_sem_wait (client->sem, OS_WAIT_FOREVER); + + return job->result; +} + co_client_t * co_client_init (co_net_t * net) { co_client_t * client; diff --git a/src/co_main.h b/src/co_main.h index d677260..a9c9757 100644 --- a/src/co_main.h +++ b/src/co_main.h @@ -103,6 +103,8 @@ typedef enum co_job_type CO_JOB_ERROR_SET, CO_JOB_ERROR_CLEAR, CO_JOB_ERROR_GET, + CO_JOB_LOCAL_READ, + CO_JOB_LOCAL_WRITE, CO_JOB_EXIT, } co_job_type_t; @@ -139,6 +141,17 @@ typedef struct co_pdo_job uint8_t subindex; } co_pdo_job_t; +typedef struct co_od_job +{ + uint16_t index; + uint8_t subindex; + uint64_t value; + struct + { + bool pdo_event : 1; + }; +} co_od_job_t; + /** Generic job */ typedef struct co_job { @@ -148,6 +161,7 @@ typedef struct co_job co_sdo_job_t sdo; co_emcy_job_t emcy; co_pdo_job_t pdo; + co_od_job_t od; }; uint32_t timestamp; struct co_client * client; diff --git a/src/co_od.c b/src/co_od.c index 3eb18da..5949914 100644 --- a/src/co_od.c +++ b/src/co_od.c @@ -18,6 +18,7 @@ #include "co_od.h" #include "co_sdo.h" +#include "co_pdo.h" #include "co_util.h" #include @@ -266,6 +267,78 @@ uint32_t co_od_set_value ( return 0; } +static uint32_t co_od_set_by_index ( + co_net_t * net, + uint16_t index, + uint8_t subindex, + uint64_t value) +{ + const co_obj_t * obj; + const co_entry_t * entry; + + obj = co_obj_find (net, index); + if (obj == NULL) + return CO_SDO_ABORT_BAD_INDEX; + + entry = co_entry_find (net, obj, subindex); + if (entry == NULL) + return CO_SDO_ABORT_BAD_SUBINDEX; + + if (CO_BYTELENGTH (entry->bitlength) > sizeof (value)) + return CO_SDO_ABORT_LENGTH; + + return co_od_set_value (net, obj, entry, subindex, value); +} + +static uint32_t co_od_get_by_index ( + co_net_t * net, + uint16_t index, + uint8_t subindex, + uint64_t * value) +{ + const co_obj_t * obj; + const co_entry_t * entry; + + obj = co_obj_find (net, index); + if (obj == NULL) + return CO_SDO_ABORT_BAD_INDEX; + + entry = co_entry_find (net, obj, subindex); + if (entry == NULL) + return CO_SDO_ABORT_BAD_SUBINDEX; + + if (CO_BYTELENGTH (entry->bitlength) > sizeof (* value)) + return CO_SDO_ABORT_LENGTH; + + return co_od_get_value (net, obj, entry, subindex, value); +} + +void co_od_job (co_net_t * net, co_job_t * job) +{ + uint32_t abort = 0; + + switch (job->type) + { + case CO_JOB_LOCAL_READ: + abort = co_od_get_by_index (net, job->od.index, job->od.subindex, &job->od.value); + break; + case CO_JOB_LOCAL_WRITE: + abort = co_od_set_by_index (net, job->od.index, job->od.subindex, job->od.value); + + if (abort == 0 && job->od.pdo_event) + { + co_pdo_trigger_with_obj(net, job->od.index, job->od.subindex); + } + break; + default: + CC_ASSERT (0); + } + + job->result = abort == 0 ? 0 : CO_STATUS_ERROR; + if (job->callback) + job->callback (job); +} + void co_od_set_defaults (co_net_t * net, uint16_t min, uint16_t max) { const co_default_t * item = net->defaults; diff --git a/src/co_od.h b/src/co_od.h index f5e0803..695a95a 100644 --- a/src/co_od.h +++ b/src/co_od.h @@ -271,6 +271,14 @@ uint32_t co_od_set_value ( uint8_t subindex, uint64_t value); +/** + * Perform local OD job + * + * @param net network handle + * @param job emcy job + */ +void co_od_job (co_net_t * net, co_job_t * job); + #ifdef __cplusplus } #endif