| 1184 | } |
| 1185 | |
| 1186 | CO_ReturnError_t |
| 1187 | CO_CANopenInitPDO(CO_t* co, CO_EM_t* em, OD_t* od, uint8_t nodeId, uint32_t* errInfo) { |
| 1188 | if (co == NULL) { |
| 1189 | return CO_ERROR_ILLEGAL_ARGUMENT; |
| 1190 | } |
| 1191 | if ((nodeId < 1U) || (nodeId > 127U) || co->nodeIdUnconfigured) { |
| 1192 | return (co->nodeIdUnconfigured) ? CO_ERROR_NODE_ID_UNCONFIGURED_LSS : CO_ERROR_ILLEGAL_ARGUMENT; |
| 1193 | } |
| 1194 | |
| 1195 | #if ((CO_CONFIG_PDO)&CO_CONFIG_RPDO_ENABLE) != 0 |
| 1196 | if (CO_GET_CNT(RPDO) > 0U) { |
| 1197 | OD_entry_t* RPDOcomm = OD_GET(H1400, OD_H1400_RXPDO_1_PARAM); |
| 1198 | OD_entry_t* RPDOmap = OD_GET(H1600, OD_H1600_RXPDO_1_MAPPING); |
| 1199 | for (uint16_t i = 0; i < CO_GET_CNT(RPDO); i++) { |
| 1200 | CO_ReturnError_t err; |
| 1201 | uint16_t preDefinedCanId = 0; |
| 1202 | if (i < CO_RPDO_DEFAULT_CANID_COUNT) { |
| 1203 | #if CO_RPDO_DEFAULT_CANID_COUNT <= 4 |
| 1204 | preDefinedCanId = (uint16_t)((CO_CAN_ID_RPDO_1 + (i * 0x100U)) + nodeId); |
| 1205 | #else |
| 1206 | uint16_t pdoOffset = i % 4; |
| 1207 | uint16_t nodeIdOffset = i / 4; |
| 1208 | preDefinedCanId = (CO_CAN_ID_RPDO_1 + pdoOffset * 0x100) + nodeId + nodeIdOffset; |
| 1209 | #endif |
| 1210 | } |
| 1211 | err = CO_RPDO_init(&co->RPDO[i], od, em, |
| 1212 | #if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0 |
| 1213 | co->SYNC, |
| 1214 | #endif |
| 1215 | preDefinedCanId, RPDOcomm, RPDOmap, co->CANmodule, CO_GET_CO(RX_IDX_RPDO) + i, errInfo); |
| 1216 | if (err != CO_ERROR_NO) { |
| 1217 | return err; |
| 1218 | } |
| 1219 | RPDOcomm++; |
| 1220 | RPDOmap++; |
| 1221 | } |
| 1222 | } |
| 1223 | #endif |
| 1224 | |
| 1225 | #if ((CO_CONFIG_PDO)&CO_CONFIG_TPDO_ENABLE) != 0 |
| 1226 | if (CO_GET_CNT(TPDO) > 0U) { |
| 1227 | OD_entry_t* TPDOcomm = OD_GET(H1800, OD_H1800_TXPDO_1_PARAM); |
| 1228 | OD_entry_t* TPDOmap = OD_GET(H1A00, OD_H1A00_TXPDO_1_MAPPING); |
| 1229 | for (uint16_t i = 0; i < CO_GET_CNT(TPDO); i++) { |
| 1230 | CO_ReturnError_t err; |
| 1231 | uint16_t preDefinedCanId = 0; |
| 1232 | if (i < CO_TPDO_DEFAULT_CANID_COUNT) { |
| 1233 | #if CO_TPDO_DEFAULT_CANID_COUNT <= 4 |
| 1234 | preDefinedCanId = (uint16_t)((CO_CAN_ID_TPDO_1 + (i * 0x100U)) + nodeId); |
| 1235 | #else |
| 1236 | uint16_t pdoOffset = i % 4; |
| 1237 | uint16_t nodeIdOffset = i / 4; |
| 1238 | preDefinedCanId = (CO_CAN_ID_TPDO_1 + pdoOffset * 0x100) + nodeId + nodeIdOffset; |
| 1239 | #endif |
| 1240 | } |
| 1241 | err = CO_TPDO_init(&co->TPDO[i], od, em, |
| 1242 | #if ((CO_CONFIG_PDO)&CO_CONFIG_PDO_SYNC_ENABLE) != 0 |
| 1243 | co->SYNC, |