![](/media/cache/profiles/IMG_2753.JPG.50x50_q85.jpg)
Can_open_slavenode
framework/src/pdo.c@6:bc64031ac849, 2012-09-26 (annotated)
- Committer:
- sam_grove
- Date:
- Wed Sep 26 05:43:05 2012 +0000
- Revision:
- 6:bc64031ac849
- Parent:
- 0:6219434a0cb5
Change a typecast in can_mbed.cpp from unit8_t * to char * to fit the CANMessage constructor
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sam_grove |
0:6219434a0cb5 | 1 | /* |
sam_grove |
0:6219434a0cb5 | 2 | This file is part of CanFestival, a library implementing CanOpen Stack. |
sam_grove |
0:6219434a0cb5 | 3 | |
sam_grove |
0:6219434a0cb5 | 4 | Copyright (C): Edouard TISSERANT and Francis DUPIN |
sam_grove |
0:6219434a0cb5 | 5 | |
sam_grove |
0:6219434a0cb5 | 6 | See COPYING file for copyrights details. |
sam_grove |
0:6219434a0cb5 | 7 | |
sam_grove |
0:6219434a0cb5 | 8 | This library is free software; you can redistribute it and/or |
sam_grove |
0:6219434a0cb5 | 9 | modify it under the terms of the GNU Lesser General Public |
sam_grove |
0:6219434a0cb5 | 10 | License as published by the Free Software Foundation; either |
sam_grove |
0:6219434a0cb5 | 11 | version 2.1 of the License, or (at your option) any later version. |
sam_grove |
0:6219434a0cb5 | 12 | |
sam_grove |
0:6219434a0cb5 | 13 | This library is distributed in the hope that it will be useful, |
sam_grove |
0:6219434a0cb5 | 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of |
sam_grove |
0:6219434a0cb5 | 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU |
sam_grove |
0:6219434a0cb5 | 16 | Lesser General Public License for more details. |
sam_grove |
0:6219434a0cb5 | 17 | |
sam_grove |
0:6219434a0cb5 | 18 | You should have received a copy of the GNU Lesser General Public |
sam_grove |
0:6219434a0cb5 | 19 | License along with this library; if not, write to the Free Software |
sam_grove |
0:6219434a0cb5 | 20 | Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 |
sam_grove |
0:6219434a0cb5 | 21 | USA |
sam_grove |
0:6219434a0cb5 | 22 | */ |
sam_grove |
0:6219434a0cb5 | 23 | #include "pdo.h" |
sam_grove |
0:6219434a0cb5 | 24 | #include "objacces.h" |
sam_grove |
0:6219434a0cb5 | 25 | #include "canfestival.h" |
sam_grove |
0:6219434a0cb5 | 26 | #include "sysdep.h" |
sam_grove |
0:6219434a0cb5 | 27 | |
sam_grove |
0:6219434a0cb5 | 28 | /*! |
sam_grove |
0:6219434a0cb5 | 29 | ** @file pdo.c |
sam_grove |
0:6219434a0cb5 | 30 | ** @author Edouard TISSERANT and Francis DUPIN |
sam_grove |
0:6219434a0cb5 | 31 | ** @date Tue Jun 5 09:32:32 2007 |
sam_grove |
0:6219434a0cb5 | 32 | ** |
sam_grove |
0:6219434a0cb5 | 33 | ** @brief |
sam_grove |
0:6219434a0cb5 | 34 | ** |
sam_grove |
0:6219434a0cb5 | 35 | ** |
sam_grove |
0:6219434a0cb5 | 36 | */ |
sam_grove |
0:6219434a0cb5 | 37 | |
sam_grove |
0:6219434a0cb5 | 38 | /*! |
sam_grove |
0:6219434a0cb5 | 39 | ** |
sam_grove |
0:6219434a0cb5 | 40 | ** |
sam_grove |
0:6219434a0cb5 | 41 | ** @param d |
sam_grove |
0:6219434a0cb5 | 42 | ** @param TPDO_com TPDO communication parameters OD entry |
sam_grove |
0:6219434a0cb5 | 43 | ** @param TPDO_map TPDO mapping parameters OD entry |
sam_grove |
0:6219434a0cb5 | 44 | ** |
sam_grove |
0:6219434a0cb5 | 45 | ** @return |
sam_grove |
0:6219434a0cb5 | 46 | **/ |
sam_grove |
0:6219434a0cb5 | 47 | |
sam_grove |
0:6219434a0cb5 | 48 | UNS8 buildPDO (CO_Data * d, UNS8 numPdo, Message * pdo) |
sam_grove |
0:6219434a0cb5 | 49 | { |
sam_grove |
0:6219434a0cb5 | 50 | const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS + numPdo; |
sam_grove |
0:6219434a0cb5 | 51 | const indextable *TPDO_map = d->objdict + d->firstIndex->PDO_TRS_MAP + numPdo; |
sam_grove |
0:6219434a0cb5 | 52 | |
sam_grove |
0:6219434a0cb5 | 53 | UNS8 prp_j = 0x00; |
sam_grove |
0:6219434a0cb5 | 54 | UNS32 offset = 0x00000000; |
sam_grove |
0:6219434a0cb5 | 55 | const UNS8 *pMappingCount = (UNS8 *) TPDO_map->pSubindex[0].pObject; |
sam_grove |
0:6219434a0cb5 | 56 | |
sam_grove |
0:6219434a0cb5 | 57 | pdo->cob_id = (UNS16) UNS16_LE(*(UNS32*)TPDO_com->pSubindex[1].pObject & 0x7FF); |
sam_grove |
0:6219434a0cb5 | 58 | pdo->rtr = NOT_A_REQUEST; |
sam_grove |
0:6219434a0cb5 | 59 | |
sam_grove |
0:6219434a0cb5 | 60 | MSG_WAR (0x3009, " PDO CobId is : ", |
sam_grove |
0:6219434a0cb5 | 61 | *(UNS32 *) TPDO_com->pSubindex[1].pObject); |
sam_grove |
0:6219434a0cb5 | 62 | MSG_WAR (0x300D, " Number of objects mapped : ", *pMappingCount); |
sam_grove |
0:6219434a0cb5 | 63 | |
sam_grove |
0:6219434a0cb5 | 64 | do |
sam_grove |
0:6219434a0cb5 | 65 | { |
sam_grove |
0:6219434a0cb5 | 66 | UNS8 dataType; /* Unused */ |
sam_grove |
0:6219434a0cb5 | 67 | UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; /* temporary space to hold bits */ |
sam_grove |
0:6219434a0cb5 | 68 | |
sam_grove |
0:6219434a0cb5 | 69 | /* pointer fo the var which holds the mapping parameter of an mapping entry */ |
sam_grove |
0:6219434a0cb5 | 70 | UNS32 *pMappingParameter = |
sam_grove |
0:6219434a0cb5 | 71 | (UNS32 *) TPDO_map->pSubindex[prp_j + 1].pObject; |
sam_grove |
0:6219434a0cb5 | 72 | UNS16 index = (UNS16) ((*pMappingParameter) >> 16); |
sam_grove |
0:6219434a0cb5 | 73 | UNS32 Size = (UNS32) (*pMappingParameter & (UNS32) 0x000000FF); /* Size in bits */ |
sam_grove |
0:6219434a0cb5 | 74 | |
sam_grove |
0:6219434a0cb5 | 75 | /* get variable only if Size != 0 and Size is lower than remaining bits in the PDO */ |
sam_grove |
0:6219434a0cb5 | 76 | if (Size && ((offset + Size) <= 64)) |
sam_grove |
0:6219434a0cb5 | 77 | { |
sam_grove |
0:6219434a0cb5 | 78 | UNS32 ByteSize = 1 + ((Size - 1) >> 3); /*1->8 => 1 ; 9->16 => 2, ... */ |
sam_grove |
0:6219434a0cb5 | 79 | UNS8 subIndex = |
sam_grove |
0:6219434a0cb5 | 80 | (UNS8) (((*pMappingParameter) >> (UNS8) 8) & (UNS32) 0x000000FF); |
sam_grove |
0:6219434a0cb5 | 81 | |
sam_grove |
0:6219434a0cb5 | 82 | MSG_WAR (0x300F, " got mapping parameter : ", *pMappingParameter); |
sam_grove |
0:6219434a0cb5 | 83 | MSG_WAR (0x3050, " at index : ", TPDO_map->index); |
sam_grove |
0:6219434a0cb5 | 84 | MSG_WAR (0x3051, " sub-index : ", prp_j + 1); |
sam_grove |
0:6219434a0cb5 | 85 | |
sam_grove |
0:6219434a0cb5 | 86 | if (getODentry (d, index, subIndex, tmp, &ByteSize, &dataType, 0) != |
sam_grove |
0:6219434a0cb5 | 87 | OD_SUCCESSFUL) |
sam_grove |
0:6219434a0cb5 | 88 | { |
sam_grove |
0:6219434a0cb5 | 89 | MSG_ERR (0x1013, |
sam_grove |
0:6219434a0cb5 | 90 | " Couldn't find mapped variable at index-subindex-size : ", |
sam_grove |
0:6219434a0cb5 | 91 | (UNS16) (*pMappingParameter)); |
sam_grove |
0:6219434a0cb5 | 92 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 93 | } |
sam_grove |
0:6219434a0cb5 | 94 | /* copy bit per bit in little endian */ |
sam_grove |
0:6219434a0cb5 | 95 | CopyBits ((UNS8) Size, ((UNS8 *) tmp), 0, 0, |
sam_grove |
0:6219434a0cb5 | 96 | (UNS8 *) & pdo->data[offset >> 3], (UNS8)(offset % 8), 0); |
sam_grove |
0:6219434a0cb5 | 97 | |
sam_grove |
0:6219434a0cb5 | 98 | offset += Size; |
sam_grove |
0:6219434a0cb5 | 99 | } |
sam_grove |
0:6219434a0cb5 | 100 | prp_j++; |
sam_grove |
0:6219434a0cb5 | 101 | } |
sam_grove |
0:6219434a0cb5 | 102 | while (prp_j < *pMappingCount); |
sam_grove |
0:6219434a0cb5 | 103 | |
sam_grove |
0:6219434a0cb5 | 104 | pdo->len = (UNS8)(1 + ((offset - 1) >> 3)); |
sam_grove |
0:6219434a0cb5 | 105 | |
sam_grove |
0:6219434a0cb5 | 106 | MSG_WAR (0x3015, " End scan mapped variable", 0); |
sam_grove |
0:6219434a0cb5 | 107 | |
sam_grove |
0:6219434a0cb5 | 108 | return 0; |
sam_grove |
0:6219434a0cb5 | 109 | } |
sam_grove |
0:6219434a0cb5 | 110 | |
sam_grove |
0:6219434a0cb5 | 111 | /*! |
sam_grove |
0:6219434a0cb5 | 112 | ** |
sam_grove |
0:6219434a0cb5 | 113 | ** |
sam_grove |
0:6219434a0cb5 | 114 | ** @param d |
sam_grove |
0:6219434a0cb5 | 115 | ** @param cobId |
sam_grove |
0:6219434a0cb5 | 116 | ** |
sam_grove |
0:6219434a0cb5 | 117 | ** @return |
sam_grove |
0:6219434a0cb5 | 118 | **/ |
sam_grove |
0:6219434a0cb5 | 119 | UNS8 |
sam_grove |
0:6219434a0cb5 | 120 | sendPDOrequest (CO_Data * d, UNS16 RPDOIndex) |
sam_grove |
0:6219434a0cb5 | 121 | { |
sam_grove |
0:6219434a0cb5 | 122 | UNS32 *pwCobId; |
sam_grove |
0:6219434a0cb5 | 123 | UNS16 offset = d->firstIndex->PDO_RCV; |
sam_grove |
0:6219434a0cb5 | 124 | UNS16 lastIndex = d->lastIndex->PDO_RCV; |
sam_grove |
0:6219434a0cb5 | 125 | |
sam_grove |
0:6219434a0cb5 | 126 | if (!d->CurrentCommunicationState.csPDO) |
sam_grove |
0:6219434a0cb5 | 127 | { |
sam_grove |
0:6219434a0cb5 | 128 | return 0; |
sam_grove |
0:6219434a0cb5 | 129 | } |
sam_grove |
0:6219434a0cb5 | 130 | |
sam_grove |
0:6219434a0cb5 | 131 | /* Sending the request only if the cobid have been found on the PDO |
sam_grove |
0:6219434a0cb5 | 132 | receive */ |
sam_grove |
0:6219434a0cb5 | 133 | /* part dictionary */ |
sam_grove |
0:6219434a0cb5 | 134 | |
sam_grove |
0:6219434a0cb5 | 135 | MSG_WAR (0x3930, "sendPDOrequest RPDO Index : ", RPDOIndex); |
sam_grove |
0:6219434a0cb5 | 136 | |
sam_grove |
0:6219434a0cb5 | 137 | if (offset && RPDOIndex >= 0x1400) |
sam_grove |
0:6219434a0cb5 | 138 | { |
sam_grove |
0:6219434a0cb5 | 139 | offset += RPDOIndex - 0x1400; |
sam_grove |
0:6219434a0cb5 | 140 | if (offset <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 141 | { |
sam_grove |
0:6219434a0cb5 | 142 | /* get the CobId */ |
sam_grove |
0:6219434a0cb5 | 143 | // mbed error 513 - needs typecast |
sam_grove |
0:6219434a0cb5 | 144 | pwCobId = (unsigned int*)d->objdict[offset].pSubindex[1].pObject; |
sam_grove |
0:6219434a0cb5 | 145 | |
sam_grove |
0:6219434a0cb5 | 146 | MSG_WAR (0x3930, "sendPDOrequest cobId is : ", *pwCobId); |
sam_grove |
0:6219434a0cb5 | 147 | { |
sam_grove |
0:6219434a0cb5 | 148 | Message pdo; |
sam_grove |
0:6219434a0cb5 | 149 | pdo.cob_id = (UNS16)UNS16_LE(*pwCobId); |
sam_grove |
0:6219434a0cb5 | 150 | pdo.rtr = REQUEST; |
sam_grove |
0:6219434a0cb5 | 151 | pdo.len = 0; |
sam_grove |
0:6219434a0cb5 | 152 | return canSend (d->canHandle, &pdo); |
sam_grove |
0:6219434a0cb5 | 153 | } |
sam_grove |
0:6219434a0cb5 | 154 | } |
sam_grove |
0:6219434a0cb5 | 155 | } |
sam_grove |
0:6219434a0cb5 | 156 | MSG_ERR (0x1931, "sendPDOrequest : RPDO Index not found : ", RPDOIndex); |
sam_grove |
0:6219434a0cb5 | 157 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 158 | } |
sam_grove |
0:6219434a0cb5 | 159 | |
sam_grove |
0:6219434a0cb5 | 160 | |
sam_grove |
0:6219434a0cb5 | 161 | /*! |
sam_grove |
0:6219434a0cb5 | 162 | ** |
sam_grove |
0:6219434a0cb5 | 163 | ** |
sam_grove |
0:6219434a0cb5 | 164 | ** @param d |
sam_grove |
0:6219434a0cb5 | 165 | ** @param m |
sam_grove |
0:6219434a0cb5 | 166 | ** |
sam_grove |
0:6219434a0cb5 | 167 | ** @return |
sam_grove |
0:6219434a0cb5 | 168 | **/ |
sam_grove |
0:6219434a0cb5 | 169 | UNS8 |
sam_grove |
0:6219434a0cb5 | 170 | proceedPDO (CO_Data * d, Message * m) |
sam_grove |
0:6219434a0cb5 | 171 | { |
sam_grove |
0:6219434a0cb5 | 172 | UNS8 numPdo; |
sam_grove |
0:6219434a0cb5 | 173 | UNS8 numMap; /* Number of the mapped varable */ |
sam_grove |
0:6219434a0cb5 | 174 | UNS8 *pMappingCount = NULL; /* count of mapped objects... */ |
sam_grove |
0:6219434a0cb5 | 175 | /* pointer to the var which is mapped to a pdo... */ |
sam_grove |
0:6219434a0cb5 | 176 | /* void * pMappedAppObject = NULL; */ |
sam_grove |
0:6219434a0cb5 | 177 | /* pointer fo the var which holds the mapping parameter of an |
sam_grove |
0:6219434a0cb5 | 178 | mapping entry */ |
sam_grove |
0:6219434a0cb5 | 179 | UNS32 *pMappingParameter = NULL; |
sam_grove |
0:6219434a0cb5 | 180 | UNS8 *pTransmissionType = NULL; /* pointer to the transmission |
sam_grove |
0:6219434a0cb5 | 181 | type */ |
sam_grove |
0:6219434a0cb5 | 182 | UNS32 *pwCobId = NULL; |
sam_grove |
0:6219434a0cb5 | 183 | UNS8 Size; |
sam_grove |
0:6219434a0cb5 | 184 | UNS8 offset; |
sam_grove |
0:6219434a0cb5 | 185 | UNS8 status; |
sam_grove |
0:6219434a0cb5 | 186 | UNS32 objDict; |
sam_grove |
0:6219434a0cb5 | 187 | UNS16 offsetObjdict; |
sam_grove |
0:6219434a0cb5 | 188 | UNS16 lastIndex; |
sam_grove |
0:6219434a0cb5 | 189 | |
sam_grove |
0:6219434a0cb5 | 190 | status = state2; |
sam_grove |
0:6219434a0cb5 | 191 | |
sam_grove |
0:6219434a0cb5 | 192 | MSG_WAR (0x3935, "proceedPDO, cobID : ", (UNS16_LE(m->cob_id) & 0x7ff)); |
sam_grove |
0:6219434a0cb5 | 193 | offset = 0x00; |
sam_grove |
0:6219434a0cb5 | 194 | numPdo = 0; |
sam_grove |
0:6219434a0cb5 | 195 | numMap = 0; |
sam_grove |
0:6219434a0cb5 | 196 | if ((*m).rtr == NOT_A_REQUEST) |
sam_grove |
0:6219434a0cb5 | 197 | { /* The PDO received is not a |
sam_grove |
0:6219434a0cb5 | 198 | request. */ |
sam_grove |
0:6219434a0cb5 | 199 | |
sam_grove |
0:6219434a0cb5 | 200 | offsetObjdict = d->firstIndex->PDO_RCV; |
sam_grove |
0:6219434a0cb5 | 201 | lastIndex = d->lastIndex->PDO_RCV; |
sam_grove |
0:6219434a0cb5 | 202 | |
sam_grove |
0:6219434a0cb5 | 203 | /* study of all the PDO stored in the dictionary */ |
sam_grove |
0:6219434a0cb5 | 204 | if (offsetObjdict) |
sam_grove |
0:6219434a0cb5 | 205 | while (offsetObjdict <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 206 | { |
sam_grove |
0:6219434a0cb5 | 207 | |
sam_grove |
0:6219434a0cb5 | 208 | switch (status) |
sam_grove |
0:6219434a0cb5 | 209 | { |
sam_grove |
0:6219434a0cb5 | 210 | |
sam_grove |
0:6219434a0cb5 | 211 | case state2: |
sam_grove |
0:6219434a0cb5 | 212 | /* get CobId of the dictionary correspondant to the received |
sam_grove |
0:6219434a0cb5 | 213 | PDO */ |
sam_grove |
0:6219434a0cb5 | 214 | // mbed error 513 - needs typecast |
sam_grove |
0:6219434a0cb5 | 215 | pwCobId = (unsigned int*) |
sam_grove |
0:6219434a0cb5 | 216 | d->objdict[offsetObjdict].pSubindex[1].pObject; |
sam_grove |
0:6219434a0cb5 | 217 | /* check the CobId coherance */ |
sam_grove |
0:6219434a0cb5 | 218 | /*pwCobId is the cobId read in the dictionary at the state 3 |
sam_grove |
0:6219434a0cb5 | 219 | */ |
sam_grove |
0:6219434a0cb5 | 220 | if (*pwCobId == UNS16_LE(m->cob_id)) |
sam_grove |
0:6219434a0cb5 | 221 | { |
sam_grove |
0:6219434a0cb5 | 222 | /* The cobId is recognized */ |
sam_grove |
0:6219434a0cb5 | 223 | status = state4; |
sam_grove |
0:6219434a0cb5 | 224 | MSG_WAR (0x3936, "cobId found at index ", |
sam_grove |
0:6219434a0cb5 | 225 | 0x1400 + numPdo); |
sam_grove |
0:6219434a0cb5 | 226 | break; |
sam_grove |
0:6219434a0cb5 | 227 | } |
sam_grove |
0:6219434a0cb5 | 228 | else |
sam_grove |
0:6219434a0cb5 | 229 | { |
sam_grove |
0:6219434a0cb5 | 230 | /* cobId received does not match with those write in the |
sam_grove |
0:6219434a0cb5 | 231 | dictionnary */ |
sam_grove |
0:6219434a0cb5 | 232 | numPdo++; |
sam_grove |
0:6219434a0cb5 | 233 | offsetObjdict++; |
sam_grove |
0:6219434a0cb5 | 234 | status = state2; |
sam_grove |
0:6219434a0cb5 | 235 | break; |
sam_grove |
0:6219434a0cb5 | 236 | } |
sam_grove |
0:6219434a0cb5 | 237 | |
sam_grove |
0:6219434a0cb5 | 238 | case state4: /* Get Mapped Objects Number */ |
sam_grove |
0:6219434a0cb5 | 239 | /* The cobId of the message received has been found in the |
sam_grove |
0:6219434a0cb5 | 240 | dictionnary. */ |
sam_grove |
0:6219434a0cb5 | 241 | offsetObjdict = d->firstIndex->PDO_RCV_MAP; |
sam_grove |
0:6219434a0cb5 | 242 | lastIndex = d->lastIndex->PDO_RCV_MAP; |
sam_grove |
0:6219434a0cb5 | 243 | pMappingCount = |
sam_grove |
0:6219434a0cb5 | 244 | (UNS8 *) (d->objdict + offsetObjdict + |
sam_grove |
0:6219434a0cb5 | 245 | numPdo)->pSubindex[0].pObject; |
sam_grove |
0:6219434a0cb5 | 246 | numMap = 0; |
sam_grove |
0:6219434a0cb5 | 247 | while (numMap < *pMappingCount) |
sam_grove |
0:6219434a0cb5 | 248 | { |
sam_grove |
0:6219434a0cb5 | 249 | UNS8 tmp[] = { 0, 0, 0, 0, 0, 0, 0, 0 }; |
sam_grove |
0:6219434a0cb5 | 250 | UNS32 ByteSize; |
sam_grove |
0:6219434a0cb5 | 251 | pMappingParameter = |
sam_grove |
0:6219434a0cb5 | 252 | (UNS32 *) (d->objdict + offsetObjdict + |
sam_grove |
0:6219434a0cb5 | 253 | numPdo)->pSubindex[numMap + 1].pObject; |
sam_grove |
0:6219434a0cb5 | 254 | if (pMappingParameter == NULL) |
sam_grove |
0:6219434a0cb5 | 255 | { |
sam_grove |
0:6219434a0cb5 | 256 | MSG_ERR (0x1937, "Couldn't get mapping parameter : ", |
sam_grove |
0:6219434a0cb5 | 257 | numMap + 1); |
sam_grove |
0:6219434a0cb5 | 258 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 259 | } |
sam_grove |
0:6219434a0cb5 | 260 | /* Get the addresse of the mapped variable. */ |
sam_grove |
0:6219434a0cb5 | 261 | /* detail of *pMappingParameter : */ |
sam_grove |
0:6219434a0cb5 | 262 | /* The 16 hight bits contains the index, the medium 8 bits |
sam_grove |
0:6219434a0cb5 | 263 | contains the subindex, */ |
sam_grove |
0:6219434a0cb5 | 264 | /* and the lower 8 bits contains the size of the mapped |
sam_grove |
0:6219434a0cb5 | 265 | variable. */ |
sam_grove |
0:6219434a0cb5 | 266 | |
sam_grove |
0:6219434a0cb5 | 267 | Size = (UNS8) (*pMappingParameter & (UNS32) 0x000000FF); |
sam_grove |
0:6219434a0cb5 | 268 | |
sam_grove |
0:6219434a0cb5 | 269 | /* set variable only if Size != 0 and |
sam_grove |
0:6219434a0cb5 | 270 | * Size is lower than remaining bits in the PDO */ |
sam_grove |
0:6219434a0cb5 | 271 | if (Size && ((offset + Size) <= (m->len << 3))) |
sam_grove |
0:6219434a0cb5 | 272 | { |
sam_grove |
0:6219434a0cb5 | 273 | /* copy bit per bit in little endian */ |
sam_grove |
0:6219434a0cb5 | 274 | CopyBits (Size, (UNS8 *) & m->data[offset >> 3], |
sam_grove |
0:6219434a0cb5 | 275 | offset % 8, 0, ((UNS8 *) tmp), 0, 0); |
sam_grove |
0:6219434a0cb5 | 276 | /*1->8 => 1 ; 9->16 =>2, ... */ |
sam_grove |
0:6219434a0cb5 | 277 | ByteSize = (UNS32)(1 + ((Size - 1) >> 3)); |
sam_grove |
0:6219434a0cb5 | 278 | |
sam_grove |
0:6219434a0cb5 | 279 | objDict = |
sam_grove |
0:6219434a0cb5 | 280 | setODentry (d, (UNS16) ((*pMappingParameter) >> 16), |
sam_grove |
0:6219434a0cb5 | 281 | (UNS8) (((*pMappingParameter) >> 8) & |
sam_grove |
0:6219434a0cb5 | 282 | 0xFF), tmp, &ByteSize, 0); |
sam_grove |
0:6219434a0cb5 | 283 | |
sam_grove |
0:6219434a0cb5 | 284 | if (objDict != OD_SUCCESSFUL) |
sam_grove |
0:6219434a0cb5 | 285 | { |
sam_grove |
0:6219434a0cb5 | 286 | MSG_ERR (0x1938, |
sam_grove |
0:6219434a0cb5 | 287 | "error accessing to the mapped var : ", |
sam_grove |
0:6219434a0cb5 | 288 | numMap + 1); |
sam_grove |
0:6219434a0cb5 | 289 | MSG_WAR (0x2939, " Mapped at index : ", |
sam_grove |
0:6219434a0cb5 | 290 | (*pMappingParameter) >> 16); |
sam_grove |
0:6219434a0cb5 | 291 | MSG_WAR (0x2940, " subindex : ", |
sam_grove |
0:6219434a0cb5 | 292 | ((*pMappingParameter) >> 8) & 0xFF); |
sam_grove |
0:6219434a0cb5 | 293 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 294 | } |
sam_grove |
0:6219434a0cb5 | 295 | |
sam_grove |
0:6219434a0cb5 | 296 | MSG_WAR (0x3942, |
sam_grove |
0:6219434a0cb5 | 297 | "Variable updated by PDO cobid : ", |
sam_grove |
0:6219434a0cb5 | 298 | UNS16_LE(m->cob_id)); |
sam_grove |
0:6219434a0cb5 | 299 | MSG_WAR (0x3943, " Mapped at index : ", |
sam_grove |
0:6219434a0cb5 | 300 | (*pMappingParameter) >> 16); |
sam_grove |
0:6219434a0cb5 | 301 | MSG_WAR (0x3944, " subindex : ", |
sam_grove |
0:6219434a0cb5 | 302 | ((*pMappingParameter) >> 8) & 0xFF); |
sam_grove |
0:6219434a0cb5 | 303 | offset += Size; |
sam_grove |
0:6219434a0cb5 | 304 | } |
sam_grove |
0:6219434a0cb5 | 305 | numMap++; |
sam_grove |
0:6219434a0cb5 | 306 | } /* end loop while on mapped variables */ |
sam_grove |
0:6219434a0cb5 | 307 | |
sam_grove |
0:6219434a0cb5 | 308 | return 0; |
sam_grove |
0:6219434a0cb5 | 309 | |
sam_grove |
0:6219434a0cb5 | 310 | } /* end switch status */ |
sam_grove |
0:6219434a0cb5 | 311 | } /* end while */ |
sam_grove |
0:6219434a0cb5 | 312 | } /* end if Donnees */ |
sam_grove |
0:6219434a0cb5 | 313 | else if ((*m).rtr == REQUEST) |
sam_grove |
0:6219434a0cb5 | 314 | { |
sam_grove |
0:6219434a0cb5 | 315 | MSG_WAR (0x3946, "Receive a PDO request cobId : ", UNS16_LE(m->cob_id)); |
sam_grove |
0:6219434a0cb5 | 316 | status = state1; |
sam_grove |
0:6219434a0cb5 | 317 | offsetObjdict = d->firstIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 318 | lastIndex = d->lastIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 319 | if (offsetObjdict) |
sam_grove |
0:6219434a0cb5 | 320 | while (offsetObjdict <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 321 | { |
sam_grove |
0:6219434a0cb5 | 322 | /* study of all PDO stored in the objects dictionary */ |
sam_grove |
0:6219434a0cb5 | 323 | |
sam_grove |
0:6219434a0cb5 | 324 | switch (status) |
sam_grove |
0:6219434a0cb5 | 325 | { |
sam_grove |
0:6219434a0cb5 | 326 | |
sam_grove |
0:6219434a0cb5 | 327 | case state1: /* check the CobId */ |
sam_grove |
0:6219434a0cb5 | 328 | /* get CobId of the dictionary which match to the received PDO |
sam_grove |
0:6219434a0cb5 | 329 | */ |
sam_grove |
0:6219434a0cb5 | 330 | // mbed error 513 - needs typecast |
sam_grove |
0:6219434a0cb5 | 331 | pwCobId = (unsigned int*) |
sam_grove |
0:6219434a0cb5 | 332 | (d->objdict + |
sam_grove |
0:6219434a0cb5 | 333 | offsetObjdict)->pSubindex[1].pObject; |
sam_grove |
0:6219434a0cb5 | 334 | if (*pwCobId == UNS16_LE(m->cob_id)) |
sam_grove |
0:6219434a0cb5 | 335 | { |
sam_grove |
0:6219434a0cb5 | 336 | status = state4; |
sam_grove |
0:6219434a0cb5 | 337 | break; |
sam_grove |
0:6219434a0cb5 | 338 | } |
sam_grove |
0:6219434a0cb5 | 339 | else |
sam_grove |
0:6219434a0cb5 | 340 | { |
sam_grove |
0:6219434a0cb5 | 341 | numPdo++; |
sam_grove |
0:6219434a0cb5 | 342 | offsetObjdict++; |
sam_grove |
0:6219434a0cb5 | 343 | } |
sam_grove |
0:6219434a0cb5 | 344 | status = state1; |
sam_grove |
0:6219434a0cb5 | 345 | break; |
sam_grove |
0:6219434a0cb5 | 346 | |
sam_grove |
0:6219434a0cb5 | 347 | |
sam_grove |
0:6219434a0cb5 | 348 | case state4: /* check transmission type */ |
sam_grove |
0:6219434a0cb5 | 349 | pTransmissionType = |
sam_grove |
0:6219434a0cb5 | 350 | (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
sam_grove |
0:6219434a0cb5 | 351 | /* If PDO is to be sampled and send on RTR, do it */ |
sam_grove |
0:6219434a0cb5 | 352 | if ((*pTransmissionType == TRANS_RTR)) |
sam_grove |
0:6219434a0cb5 | 353 | { |
sam_grove |
0:6219434a0cb5 | 354 | status = state5; |
sam_grove |
0:6219434a0cb5 | 355 | break; |
sam_grove |
0:6219434a0cb5 | 356 | } |
sam_grove |
0:6219434a0cb5 | 357 | /* RTR_SYNC means data prepared at SYNC, transmitted on RTR */ |
sam_grove |
0:6219434a0cb5 | 358 | else if ((*pTransmissionType == TRANS_RTR_SYNC)) |
sam_grove |
0:6219434a0cb5 | 359 | { |
sam_grove |
0:6219434a0cb5 | 360 | if (d->PDO_status[numPdo]. |
sam_grove |
0:6219434a0cb5 | 361 | transmit_type_parameter & PDO_RTR_SYNC_READY) |
sam_grove |
0:6219434a0cb5 | 362 | { |
sam_grove |
0:6219434a0cb5 | 363 | /*Data ready, just send */ |
sam_grove |
0:6219434a0cb5 | 364 | canSend (d->canHandle, |
sam_grove |
0:6219434a0cb5 | 365 | &d->PDO_status[numPdo].last_message); |
sam_grove |
0:6219434a0cb5 | 366 | return 0; |
sam_grove |
0:6219434a0cb5 | 367 | } |
sam_grove |
0:6219434a0cb5 | 368 | else |
sam_grove |
0:6219434a0cb5 | 369 | { |
sam_grove |
0:6219434a0cb5 | 370 | /* if SYNC did never occur, transmit current data */ |
sam_grove |
0:6219434a0cb5 | 371 | /* DS301 do not tell what to do in such a case... */ |
sam_grove |
0:6219434a0cb5 | 372 | MSG_ERR (0x1947, |
sam_grove |
0:6219434a0cb5 | 373 | "Not ready RTR_SYNC TPDO send current data : ", |
sam_grove |
0:6219434a0cb5 | 374 | UNS16_LE(m->cob_id)); |
sam_grove |
0:6219434a0cb5 | 375 | status = state5; |
sam_grove |
0:6219434a0cb5 | 376 | } |
sam_grove |
0:6219434a0cb5 | 377 | break; |
sam_grove |
0:6219434a0cb5 | 378 | } |
sam_grove |
0:6219434a0cb5 | 379 | else if ((*pTransmissionType == TRANS_EVENT_PROFILE) || |
sam_grove |
0:6219434a0cb5 | 380 | (*pTransmissionType == TRANS_EVENT_SPECIFIC)) |
sam_grove |
0:6219434a0cb5 | 381 | { |
sam_grove |
0:6219434a0cb5 | 382 | /* Zap all timers and inhibit flag */ |
sam_grove |
0:6219434a0cb5 | 383 | d->PDO_status[numPdo].event_timer = |
sam_grove |
0:6219434a0cb5 | 384 | DelAlarm (d->PDO_status[numPdo].event_timer); |
sam_grove |
0:6219434a0cb5 | 385 | d->PDO_status[numPdo].inhibit_timer = |
sam_grove |
0:6219434a0cb5 | 386 | DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
sam_grove |
0:6219434a0cb5 | 387 | d->PDO_status[numPdo].transmit_type_parameter &= |
sam_grove |
0:6219434a0cb5 | 388 | ~PDO_INHIBITED; |
sam_grove |
0:6219434a0cb5 | 389 | /* Call PDOEventTimerAlarm for this TPDO, |
sam_grove |
0:6219434a0cb5 | 390 | * this will trigger emission et reset timers */ |
sam_grove |
0:6219434a0cb5 | 391 | PDOEventTimerAlarm (d, numPdo); |
sam_grove |
0:6219434a0cb5 | 392 | return 0; |
sam_grove |
0:6219434a0cb5 | 393 | } |
sam_grove |
0:6219434a0cb5 | 394 | else |
sam_grove |
0:6219434a0cb5 | 395 | { |
sam_grove |
0:6219434a0cb5 | 396 | /* The requested PDO is not to send on request. So, does |
sam_grove |
0:6219434a0cb5 | 397 | nothing. */ |
sam_grove |
0:6219434a0cb5 | 398 | MSG_WAR (0x2947, "PDO is not to send on request : ", |
sam_grove |
0:6219434a0cb5 | 399 | UNS16_LE(m->cob_id)); |
sam_grove |
0:6219434a0cb5 | 400 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 401 | } |
sam_grove |
0:6219434a0cb5 | 402 | |
sam_grove |
0:6219434a0cb5 | 403 | case state5: /* build and send requested PDO */ |
sam_grove |
0:6219434a0cb5 | 404 | { |
sam_grove |
0:6219434a0cb5 | 405 | Message pdo; |
sam_grove |
0:6219434a0cb5 | 406 | if (buildPDO (d, numPdo, &pdo)) |
sam_grove |
0:6219434a0cb5 | 407 | { |
sam_grove |
0:6219434a0cb5 | 408 | MSG_ERR (0x1948, " Couldn't build TPDO n�", numPdo); |
sam_grove |
0:6219434a0cb5 | 409 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 410 | } |
sam_grove |
0:6219434a0cb5 | 411 | canSend (d->canHandle, &pdo); |
sam_grove |
0:6219434a0cb5 | 412 | return 0; |
sam_grove |
0:6219434a0cb5 | 413 | } |
sam_grove |
0:6219434a0cb5 | 414 | } /* end switch status */ |
sam_grove |
0:6219434a0cb5 | 415 | } /* end while */ |
sam_grove |
0:6219434a0cb5 | 416 | } /* end if Requete */ |
sam_grove |
0:6219434a0cb5 | 417 | |
sam_grove |
0:6219434a0cb5 | 418 | return 0; |
sam_grove |
0:6219434a0cb5 | 419 | } |
sam_grove |
0:6219434a0cb5 | 420 | |
sam_grove |
0:6219434a0cb5 | 421 | /*! |
sam_grove |
0:6219434a0cb5 | 422 | ** |
sam_grove |
0:6219434a0cb5 | 423 | ** |
sam_grove |
0:6219434a0cb5 | 424 | ** @param NbBits |
sam_grove |
0:6219434a0cb5 | 425 | ** @param SrcByteIndex |
sam_grove |
0:6219434a0cb5 | 426 | ** @param SrcBitIndex |
sam_grove |
0:6219434a0cb5 | 427 | ** @param SrcBigEndian |
sam_grove |
0:6219434a0cb5 | 428 | ** @param DestByteIndex |
sam_grove |
0:6219434a0cb5 | 429 | ** @param DestBitIndex |
sam_grove |
0:6219434a0cb5 | 430 | ** @param DestBigEndian |
sam_grove |
0:6219434a0cb5 | 431 | **/ |
sam_grove |
0:6219434a0cb5 | 432 | void |
sam_grove |
0:6219434a0cb5 | 433 | CopyBits (UNS8 NbBits, UNS8 * SrcByteIndex, UNS8 SrcBitIndex, |
sam_grove |
0:6219434a0cb5 | 434 | UNS8 SrcBigEndian, UNS8 * DestByteIndex, UNS8 DestBitIndex, |
sam_grove |
0:6219434a0cb5 | 435 | UNS8 DestBigEndian) |
sam_grove |
0:6219434a0cb5 | 436 | { |
sam_grove |
0:6219434a0cb5 | 437 | /* This loop copy as many bits that it can each time, crossing */ |
sam_grove |
0:6219434a0cb5 | 438 | /* successively bytes */ |
sam_grove |
0:6219434a0cb5 | 439 | // boundaries from LSB to MSB. |
sam_grove |
0:6219434a0cb5 | 440 | while (NbBits > 0) |
sam_grove |
0:6219434a0cb5 | 441 | { |
sam_grove |
0:6219434a0cb5 | 442 | /* Bit missalignement between src and dest */ |
sam_grove |
0:6219434a0cb5 | 443 | INTEGER8 Vect = DestBitIndex - SrcBitIndex; |
sam_grove |
0:6219434a0cb5 | 444 | |
sam_grove |
0:6219434a0cb5 | 445 | /* We can now get src and align it to dest */ |
sam_grove |
0:6219434a0cb5 | 446 | UNS8 Aligned = |
sam_grove |
0:6219434a0cb5 | 447 | Vect > 0 ? *SrcByteIndex << Vect : *SrcByteIndex >> -Vect; |
sam_grove |
0:6219434a0cb5 | 448 | |
sam_grove |
0:6219434a0cb5 | 449 | /* Compute the nb of bit we will be able to copy */ |
sam_grove |
0:6219434a0cb5 | 450 | UNS8 BoudaryLimit = (Vect > 0 ? 8 - DestBitIndex : 8 - SrcBitIndex); |
sam_grove |
0:6219434a0cb5 | 451 | UNS8 BitsToCopy = BoudaryLimit > NbBits ? NbBits : BoudaryLimit; |
sam_grove |
0:6219434a0cb5 | 452 | |
sam_grove |
0:6219434a0cb5 | 453 | /* Create a mask that will serve in: */ |
sam_grove |
0:6219434a0cb5 | 454 | UNS8 Mask = |
sam_grove |
0:6219434a0cb5 | 455 | ((0xff << (DestBitIndex + BitsToCopy)) | |
sam_grove |
0:6219434a0cb5 | 456 | (0xff >> (8 - DestBitIndex))); |
sam_grove |
0:6219434a0cb5 | 457 | |
sam_grove |
0:6219434a0cb5 | 458 | /* - Filtering src */ |
sam_grove |
0:6219434a0cb5 | 459 | UNS8 Filtered = Aligned & ~Mask; |
sam_grove |
0:6219434a0cb5 | 460 | |
sam_grove |
0:6219434a0cb5 | 461 | /* - and erase bits where we write, preserve where we don't */ |
sam_grove |
0:6219434a0cb5 | 462 | *DestByteIndex &= Mask; |
sam_grove |
0:6219434a0cb5 | 463 | |
sam_grove |
0:6219434a0cb5 | 464 | /* Then write. */ |
sam_grove |
0:6219434a0cb5 | 465 | *DestByteIndex |= Filtered; |
sam_grove |
0:6219434a0cb5 | 466 | |
sam_grove |
0:6219434a0cb5 | 467 | /*Compute next time cursors for src */ |
sam_grove |
0:6219434a0cb5 | 468 | if ((SrcBitIndex += BitsToCopy) > 7) /* cross boundary ? */ |
sam_grove |
0:6219434a0cb5 | 469 | { |
sam_grove |
0:6219434a0cb5 | 470 | SrcBitIndex = 0; /* First bit */ |
sam_grove |
0:6219434a0cb5 | 471 | SrcByteIndex += (SrcBigEndian ? -1 : 1); /* Next byte */ |
sam_grove |
0:6219434a0cb5 | 472 | } |
sam_grove |
0:6219434a0cb5 | 473 | |
sam_grove |
0:6219434a0cb5 | 474 | |
sam_grove |
0:6219434a0cb5 | 475 | /*Compute next time cursors for dest */ |
sam_grove |
0:6219434a0cb5 | 476 | if ((DestBitIndex += BitsToCopy) > 7) |
sam_grove |
0:6219434a0cb5 | 477 | { |
sam_grove |
0:6219434a0cb5 | 478 | DestBitIndex = 0; /* First bit */ |
sam_grove |
0:6219434a0cb5 | 479 | DestByteIndex += (DestBigEndian ? -1 : 1); /* Next byte */ |
sam_grove |
0:6219434a0cb5 | 480 | } |
sam_grove |
0:6219434a0cb5 | 481 | |
sam_grove |
0:6219434a0cb5 | 482 | /*And decrement counter. */ |
sam_grove |
0:6219434a0cb5 | 483 | NbBits -= BitsToCopy; |
sam_grove |
0:6219434a0cb5 | 484 | } |
sam_grove |
0:6219434a0cb5 | 485 | |
sam_grove |
0:6219434a0cb5 | 486 | } |
sam_grove |
0:6219434a0cb5 | 487 | |
sam_grove |
0:6219434a0cb5 | 488 | static void sendPdo(CO_Data * d, UNS32 pdoNum, Message * pdo) |
sam_grove |
0:6219434a0cb5 | 489 | { |
sam_grove |
0:6219434a0cb5 | 490 | /*store_as_last_message */ |
sam_grove |
0:6219434a0cb5 | 491 | d->PDO_status[pdoNum].last_message = *pdo; |
sam_grove |
0:6219434a0cb5 | 492 | MSG_WAR (0x396D, "sendPDO cobId :", UNS16_LE(pdo.cob_id)); |
sam_grove |
0:6219434a0cb5 | 493 | MSG_WAR (0x396E, " Nb octets : ", pdo.len); |
sam_grove |
0:6219434a0cb5 | 494 | |
sam_grove |
0:6219434a0cb5 | 495 | canSend (d->canHandle, pdo); |
sam_grove |
0:6219434a0cb5 | 496 | } |
sam_grove |
0:6219434a0cb5 | 497 | |
sam_grove |
0:6219434a0cb5 | 498 | |
sam_grove |
0:6219434a0cb5 | 499 | /*! |
sam_grove |
0:6219434a0cb5 | 500 | ** |
sam_grove |
0:6219434a0cb5 | 501 | ** |
sam_grove |
0:6219434a0cb5 | 502 | ** @param d |
sam_grove |
0:6219434a0cb5 | 503 | ** |
sam_grove |
0:6219434a0cb5 | 504 | ** @return |
sam_grove |
0:6219434a0cb5 | 505 | **/ |
sam_grove |
0:6219434a0cb5 | 506 | |
sam_grove |
0:6219434a0cb5 | 507 | UNS8 |
sam_grove |
0:6219434a0cb5 | 508 | sendPDOevent (CO_Data * d) |
sam_grove |
0:6219434a0cb5 | 509 | { |
sam_grove |
0:6219434a0cb5 | 510 | /* Calls _sendPDOevent specifying it is not a sync event */ |
sam_grove |
0:6219434a0cb5 | 511 | return _sendPDOevent (d, 0); |
sam_grove |
0:6219434a0cb5 | 512 | } |
sam_grove |
0:6219434a0cb5 | 513 | |
sam_grove |
0:6219434a0cb5 | 514 | UNS8 |
sam_grove |
0:6219434a0cb5 | 515 | sendOnePDOevent (CO_Data * d, UNS8 pdoNum) |
sam_grove |
0:6219434a0cb5 | 516 | { |
sam_grove |
0:6219434a0cb5 | 517 | UNS16 offsetObjdict; |
sam_grove |
0:6219434a0cb5 | 518 | Message pdo; |
sam_grove |
0:6219434a0cb5 | 519 | if (!d->CurrentCommunicationState.csPDO || |
sam_grove |
0:6219434a0cb5 | 520 | (d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED)) |
sam_grove |
0:6219434a0cb5 | 521 | { |
sam_grove |
0:6219434a0cb5 | 522 | return 0; |
sam_grove |
0:6219434a0cb5 | 523 | } |
sam_grove |
0:6219434a0cb5 | 524 | |
sam_grove |
0:6219434a0cb5 | 525 | offsetObjdict = (UNS16) (d->firstIndex->PDO_TRS + pdoNum); |
sam_grove |
0:6219434a0cb5 | 526 | MSG_WAR (0x3968, " PDO is on EVENT. Trans type : ", |
sam_grove |
0:6219434a0cb5 | 527 | *pTransmissionType); |
sam_grove |
0:6219434a0cb5 | 528 | |
sam_grove |
0:6219434a0cb5 | 529 | memset(&pdo, 0, sizeof(pdo)); |
sam_grove |
0:6219434a0cb5 | 530 | if (buildPDO (d, pdoNum, &pdo)) |
sam_grove |
0:6219434a0cb5 | 531 | { |
sam_grove |
0:6219434a0cb5 | 532 | MSG_ERR (0x3907, " Couldn't build TPDO number : ", |
sam_grove |
0:6219434a0cb5 | 533 | pdoNum); |
sam_grove |
0:6219434a0cb5 | 534 | return 0; |
sam_grove |
0:6219434a0cb5 | 535 | } |
sam_grove |
0:6219434a0cb5 | 536 | |
sam_grove |
0:6219434a0cb5 | 537 | /*Compare new and old PDO */ |
sam_grove |
0:6219434a0cb5 | 538 | if (d->PDO_status[pdoNum].last_message.cob_id == pdo.cob_id |
sam_grove |
0:6219434a0cb5 | 539 | && d->PDO_status[pdoNum].last_message.len == pdo.len |
sam_grove |
0:6219434a0cb5 | 540 | && memcmp(d->PDO_status[pdoNum].last_message.data, |
sam_grove |
0:6219434a0cb5 | 541 | pdo.data, 8) == 0 |
sam_grove |
0:6219434a0cb5 | 542 | ) |
sam_grove |
0:6219434a0cb5 | 543 | { |
sam_grove |
0:6219434a0cb5 | 544 | /* No changes -> go to next pdo */ |
sam_grove |
0:6219434a0cb5 | 545 | return 0; |
sam_grove |
0:6219434a0cb5 | 546 | } |
sam_grove |
0:6219434a0cb5 | 547 | else |
sam_grove |
0:6219434a0cb5 | 548 | { |
sam_grove |
0:6219434a0cb5 | 549 | |
sam_grove |
0:6219434a0cb5 | 550 | TIMEVAL EventTimerDuration; |
sam_grove |
0:6219434a0cb5 | 551 | TIMEVAL InhibitTimerDuration; |
sam_grove |
0:6219434a0cb5 | 552 | |
sam_grove |
0:6219434a0cb5 | 553 | MSG_WAR (0x306A, "Changes TPDO number : ", pdoNum); |
sam_grove |
0:6219434a0cb5 | 554 | /* Changes detected -> transmit message */ |
sam_grove |
0:6219434a0cb5 | 555 | EventTimerDuration = |
sam_grove |
0:6219434a0cb5 | 556 | *(UNS16 *) d->objdict[offsetObjdict].pSubindex[5]. |
sam_grove |
0:6219434a0cb5 | 557 | pObject; |
sam_grove |
0:6219434a0cb5 | 558 | InhibitTimerDuration = |
sam_grove |
0:6219434a0cb5 | 559 | *(UNS16 *) d->objdict[offsetObjdict].pSubindex[3]. |
sam_grove |
0:6219434a0cb5 | 560 | pObject; |
sam_grove |
0:6219434a0cb5 | 561 | |
sam_grove |
0:6219434a0cb5 | 562 | /* Start both event_timer and inhibit_timer */ |
sam_grove |
0:6219434a0cb5 | 563 | if (EventTimerDuration) |
sam_grove |
0:6219434a0cb5 | 564 | { |
sam_grove |
0:6219434a0cb5 | 565 | DelAlarm (d->PDO_status[pdoNum].event_timer); |
sam_grove |
0:6219434a0cb5 | 566 | d->PDO_status[pdoNum].event_timer = |
sam_grove |
0:6219434a0cb5 | 567 | SetAlarm (d, pdoNum, &PDOEventTimerAlarm, |
sam_grove |
0:6219434a0cb5 | 568 | MS_TO_TIMEVAL (EventTimerDuration), 0); |
sam_grove |
0:6219434a0cb5 | 569 | } |
sam_grove |
0:6219434a0cb5 | 570 | |
sam_grove |
0:6219434a0cb5 | 571 | if (InhibitTimerDuration) |
sam_grove |
0:6219434a0cb5 | 572 | { |
sam_grove |
0:6219434a0cb5 | 573 | DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
sam_grove |
0:6219434a0cb5 | 574 | d->PDO_status[pdoNum].inhibit_timer = |
sam_grove |
0:6219434a0cb5 | 575 | SetAlarm (d, pdoNum, &PDOInhibitTimerAlarm, |
sam_grove |
0:6219434a0cb5 | 576 | US_TO_TIMEVAL (InhibitTimerDuration * |
sam_grove |
0:6219434a0cb5 | 577 | 100), 0); |
sam_grove |
0:6219434a0cb5 | 578 | /* and inhibit TPDO */ |
sam_grove |
0:6219434a0cb5 | 579 | d->PDO_status[pdoNum].transmit_type_parameter |= |
sam_grove |
0:6219434a0cb5 | 580 | PDO_INHIBITED; |
sam_grove |
0:6219434a0cb5 | 581 | } |
sam_grove |
0:6219434a0cb5 | 582 | |
sam_grove |
0:6219434a0cb5 | 583 | sendPdo(d, pdoNum, &pdo); |
sam_grove |
0:6219434a0cb5 | 584 | } |
sam_grove |
0:6219434a0cb5 | 585 | return 1; |
sam_grove |
0:6219434a0cb5 | 586 | } |
sam_grove |
0:6219434a0cb5 | 587 | |
sam_grove |
0:6219434a0cb5 | 588 | void |
sam_grove |
0:6219434a0cb5 | 589 | PDOEventTimerAlarm (CO_Data * d, UNS32 pdoNum) |
sam_grove |
0:6219434a0cb5 | 590 | { |
sam_grove |
0:6219434a0cb5 | 591 | /* This is needed to avoid deletion of re-attribuated timer */ |
sam_grove |
0:6219434a0cb5 | 592 | d->PDO_status[pdoNum].event_timer = TIMER_NONE; |
sam_grove |
0:6219434a0cb5 | 593 | /* force emission of PDO by artificially changing last emitted */ |
sam_grove |
0:6219434a0cb5 | 594 | d->PDO_status[pdoNum].last_message.cob_id = 0; |
sam_grove |
0:6219434a0cb5 | 595 | sendOnePDOevent (d, (UNS8) pdoNum); |
sam_grove |
0:6219434a0cb5 | 596 | } |
sam_grove |
0:6219434a0cb5 | 597 | |
sam_grove |
0:6219434a0cb5 | 598 | void |
sam_grove |
0:6219434a0cb5 | 599 | PDOInhibitTimerAlarm (CO_Data * d, UNS32 pdoNum) |
sam_grove |
0:6219434a0cb5 | 600 | { |
sam_grove |
0:6219434a0cb5 | 601 | /* This is needed to avoid deletion of re-attribuated timer */ |
sam_grove |
0:6219434a0cb5 | 602 | d->PDO_status[pdoNum].inhibit_timer = TIMER_NONE; |
sam_grove |
0:6219434a0cb5 | 603 | /* Remove inhibit flag */ |
sam_grove |
0:6219434a0cb5 | 604 | d->PDO_status[pdoNum].transmit_type_parameter &= ~PDO_INHIBITED; |
sam_grove |
0:6219434a0cb5 | 605 | sendOnePDOevent (d, (UNS8) pdoNum); |
sam_grove |
0:6219434a0cb5 | 606 | } |
sam_grove |
0:6219434a0cb5 | 607 | |
sam_grove |
0:6219434a0cb5 | 608 | /*! |
sam_grove |
0:6219434a0cb5 | 609 | ** |
sam_grove |
0:6219434a0cb5 | 610 | ** |
sam_grove |
0:6219434a0cb5 | 611 | ** @param d |
sam_grove |
0:6219434a0cb5 | 612 | ** @param isSyncEvent |
sam_grove |
0:6219434a0cb5 | 613 | ** |
sam_grove |
0:6219434a0cb5 | 614 | ** @return |
sam_grove |
0:6219434a0cb5 | 615 | **/ |
sam_grove |
0:6219434a0cb5 | 616 | |
sam_grove |
0:6219434a0cb5 | 617 | UNS8 |
sam_grove |
0:6219434a0cb5 | 618 | _sendPDOevent (CO_Data * d, UNS8 isSyncEvent) |
sam_grove |
0:6219434a0cb5 | 619 | { |
sam_grove |
0:6219434a0cb5 | 620 | UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
sam_grove |
0:6219434a0cb5 | 621 | UNS8 *pTransmissionType = NULL; |
sam_grove |
0:6219434a0cb5 | 622 | UNS8 status = state3; |
sam_grove |
0:6219434a0cb5 | 623 | UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 624 | UNS16 offsetObjdictMap = d->firstIndex->PDO_TRS_MAP; |
sam_grove |
0:6219434a0cb5 | 625 | UNS16 lastIndex = d->lastIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 626 | |
sam_grove |
0:6219434a0cb5 | 627 | if (!d->CurrentCommunicationState.csPDO) |
sam_grove |
0:6219434a0cb5 | 628 | { |
sam_grove |
0:6219434a0cb5 | 629 | return 0; |
sam_grove |
0:6219434a0cb5 | 630 | } |
sam_grove |
0:6219434a0cb5 | 631 | |
sam_grove |
0:6219434a0cb5 | 632 | |
sam_grove |
0:6219434a0cb5 | 633 | /* study all PDO stored in the objects dictionary */ |
sam_grove |
0:6219434a0cb5 | 634 | if (offsetObjdict) |
sam_grove |
0:6219434a0cb5 | 635 | { |
sam_grove |
0:6219434a0cb5 | 636 | Message pdo;/* = Message_Initializer;*/ |
sam_grove |
0:6219434a0cb5 | 637 | memset(&pdo, 0, sizeof(pdo)); |
sam_grove |
0:6219434a0cb5 | 638 | while (offsetObjdict <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 639 | { |
sam_grove |
0:6219434a0cb5 | 640 | switch (status) |
sam_grove |
0:6219434a0cb5 | 641 | { |
sam_grove |
0:6219434a0cb5 | 642 | case state3: |
sam_grove |
0:6219434a0cb5 | 643 | if ( /* bSubCount always 5 with objdictedit -> check disabled */ |
sam_grove |
0:6219434a0cb5 | 644 | /*d->objdict[offsetObjdict].bSubCount < 5 ||*/ |
sam_grove |
0:6219434a0cb5 | 645 | /* check if TPDO is not valid */ |
sam_grove |
0:6219434a0cb5 | 646 | *(UNS32 *) d->objdict[offsetObjdict].pSubindex[1]. |
sam_grove |
0:6219434a0cb5 | 647 | pObject & 0x80000000) |
sam_grove |
0:6219434a0cb5 | 648 | { |
sam_grove |
0:6219434a0cb5 | 649 | MSG_WAR (0x3960, "Not a valid PDO ", 0x1800 + pdoNum); |
sam_grove |
0:6219434a0cb5 | 650 | /*Go next TPDO */ |
sam_grove |
0:6219434a0cb5 | 651 | status = state11; |
sam_grove |
0:6219434a0cb5 | 652 | break; |
sam_grove |
0:6219434a0cb5 | 653 | } |
sam_grove |
0:6219434a0cb5 | 654 | /* get the PDO transmission type */ |
sam_grove |
0:6219434a0cb5 | 655 | pTransmissionType = |
sam_grove |
0:6219434a0cb5 | 656 | (UNS8 *) d->objdict[offsetObjdict].pSubindex[2].pObject; |
sam_grove |
0:6219434a0cb5 | 657 | MSG_WAR (0x3962, "Reading PDO at index : ", 0x1800 + pdoNum); |
sam_grove |
0:6219434a0cb5 | 658 | |
sam_grove |
0:6219434a0cb5 | 659 | /* check if transmission type is SYNCRONOUS */ |
sam_grove |
0:6219434a0cb5 | 660 | /* message transmited every n SYNC with n=TransmissionType */ |
sam_grove |
0:6219434a0cb5 | 661 | if (isSyncEvent && |
sam_grove |
0:6219434a0cb5 | 662 | (*pTransmissionType >= TRANS_SYNC_MIN) && |
sam_grove |
0:6219434a0cb5 | 663 | (*pTransmissionType <= TRANS_SYNC_MAX) && |
sam_grove |
0:6219434a0cb5 | 664 | (++d->PDO_status[pdoNum].transmit_type_parameter == |
sam_grove |
0:6219434a0cb5 | 665 | *pTransmissionType)) |
sam_grove |
0:6219434a0cb5 | 666 | { |
sam_grove |
0:6219434a0cb5 | 667 | /*Reset count of SYNC */ |
sam_grove |
0:6219434a0cb5 | 668 | d->PDO_status[pdoNum].transmit_type_parameter = 0; |
sam_grove |
0:6219434a0cb5 | 669 | MSG_WAR (0x3964, " PDO is on SYNCHRO. Trans type : ", |
sam_grove |
0:6219434a0cb5 | 670 | *pTransmissionType); |
sam_grove |
0:6219434a0cb5 | 671 | memset(&pdo, 0, sizeof(pdo)); |
sam_grove |
0:6219434a0cb5 | 672 | /*{ |
sam_grove |
0:6219434a0cb5 | 673 | Message msg_init = Message_Initializer; |
sam_grove |
0:6219434a0cb5 | 674 | pdo = msg_init; |
sam_grove |
0:6219434a0cb5 | 675 | }*/ |
sam_grove |
0:6219434a0cb5 | 676 | if (buildPDO (d, pdoNum, &pdo)) |
sam_grove |
0:6219434a0cb5 | 677 | { |
sam_grove |
0:6219434a0cb5 | 678 | MSG_ERR (0x1906, " Couldn't build TPDO number : ", |
sam_grove |
0:6219434a0cb5 | 679 | pdoNum); |
sam_grove |
0:6219434a0cb5 | 680 | status = state11; |
sam_grove |
0:6219434a0cb5 | 681 | break; |
sam_grove |
0:6219434a0cb5 | 682 | } |
sam_grove |
0:6219434a0cb5 | 683 | status = state5; |
sam_grove |
0:6219434a0cb5 | 684 | /* If transmission RTR, with data sampled on SYNC */ |
sam_grove |
0:6219434a0cb5 | 685 | } |
sam_grove |
0:6219434a0cb5 | 686 | else if (isSyncEvent && (*pTransmissionType == TRANS_RTR_SYNC)) |
sam_grove |
0:6219434a0cb5 | 687 | { |
sam_grove |
0:6219434a0cb5 | 688 | if (buildPDO |
sam_grove |
0:6219434a0cb5 | 689 | (d, pdoNum, &d->PDO_status[pdoNum].last_message)) |
sam_grove |
0:6219434a0cb5 | 690 | { |
sam_grove |
0:6219434a0cb5 | 691 | MSG_ERR (0x1966, " Couldn't build TPDO number : ", |
sam_grove |
0:6219434a0cb5 | 692 | pdoNum); |
sam_grove |
0:6219434a0cb5 | 693 | d->PDO_status[pdoNum].transmit_type_parameter &= |
sam_grove |
0:6219434a0cb5 | 694 | ~PDO_RTR_SYNC_READY; |
sam_grove |
0:6219434a0cb5 | 695 | } |
sam_grove |
0:6219434a0cb5 | 696 | else |
sam_grove |
0:6219434a0cb5 | 697 | { |
sam_grove |
0:6219434a0cb5 | 698 | d->PDO_status[pdoNum].transmit_type_parameter |= |
sam_grove |
0:6219434a0cb5 | 699 | PDO_RTR_SYNC_READY; |
sam_grove |
0:6219434a0cb5 | 700 | } |
sam_grove |
0:6219434a0cb5 | 701 | status = state11; |
sam_grove |
0:6219434a0cb5 | 702 | break; |
sam_grove |
0:6219434a0cb5 | 703 | /* If transmission on Event and not inhibited, check for changes */ |
sam_grove |
0:6219434a0cb5 | 704 | } |
sam_grove |
0:6219434a0cb5 | 705 | else |
sam_grove |
0:6219434a0cb5 | 706 | if ( (isSyncEvent && (*pTransmissionType == TRANS_SYNC_ACYCLIC)) |
sam_grove |
0:6219434a0cb5 | 707 | || |
sam_grove |
0:6219434a0cb5 | 708 | (!isSyncEvent && (*pTransmissionType == TRANS_EVENT_PROFILE || *pTransmissionType == TRANS_EVENT_SPECIFIC) |
sam_grove |
0:6219434a0cb5 | 709 | && !(d->PDO_status[pdoNum].transmit_type_parameter & PDO_INHIBITED))) |
sam_grove |
0:6219434a0cb5 | 710 | { |
sam_grove |
0:6219434a0cb5 | 711 | sendOnePDOevent(d, pdoNum); |
sam_grove |
0:6219434a0cb5 | 712 | status = state11; |
sam_grove |
0:6219434a0cb5 | 713 | } |
sam_grove |
0:6219434a0cb5 | 714 | else |
sam_grove |
0:6219434a0cb5 | 715 | { |
sam_grove |
0:6219434a0cb5 | 716 | MSG_WAR (0x306C, |
sam_grove |
0:6219434a0cb5 | 717 | " PDO is not on EVENT or synchro or not at this SYNC. Trans type : ", |
sam_grove |
0:6219434a0cb5 | 718 | *pTransmissionType); |
sam_grove |
0:6219434a0cb5 | 719 | status = state11; |
sam_grove |
0:6219434a0cb5 | 720 | } |
sam_grove |
0:6219434a0cb5 | 721 | break; |
sam_grove |
0:6219434a0cb5 | 722 | case state5: /*Send the pdo */ |
sam_grove |
0:6219434a0cb5 | 723 | sendPdo(d, pdoNum, &pdo); |
sam_grove |
0:6219434a0cb5 | 724 | status = state11; |
sam_grove |
0:6219434a0cb5 | 725 | break; |
sam_grove |
0:6219434a0cb5 | 726 | case state11: /*Go to next TPDO */ |
sam_grove |
0:6219434a0cb5 | 727 | pdoNum++; |
sam_grove |
0:6219434a0cb5 | 728 | offsetObjdict++; |
sam_grove |
0:6219434a0cb5 | 729 | offsetObjdictMap++; |
sam_grove |
0:6219434a0cb5 | 730 | MSG_WAR (0x3970, "next pdo index : ", pdoNum); |
sam_grove |
0:6219434a0cb5 | 731 | status = state3; |
sam_grove |
0:6219434a0cb5 | 732 | break; |
sam_grove |
0:6219434a0cb5 | 733 | |
sam_grove |
0:6219434a0cb5 | 734 | default: |
sam_grove |
0:6219434a0cb5 | 735 | MSG_ERR (0x1972, "Unknown state has been reached : %d", status); |
sam_grove |
0:6219434a0cb5 | 736 | return 0xFF; |
sam_grove |
0:6219434a0cb5 | 737 | } /* end switch case */ |
sam_grove |
0:6219434a0cb5 | 738 | |
sam_grove |
0:6219434a0cb5 | 739 | } /* end while */ |
sam_grove |
0:6219434a0cb5 | 740 | } |
sam_grove |
0:6219434a0cb5 | 741 | return 0; |
sam_grove |
0:6219434a0cb5 | 742 | } |
sam_grove |
0:6219434a0cb5 | 743 | |
sam_grove |
0:6219434a0cb5 | 744 | /*! |
sam_grove |
0:6219434a0cb5 | 745 | ** |
sam_grove |
0:6219434a0cb5 | 746 | ** |
sam_grove |
0:6219434a0cb5 | 747 | ** @param d |
sam_grove |
0:6219434a0cb5 | 748 | ** @param OD_entry |
sam_grove |
0:6219434a0cb5 | 749 | ** @param bSubindex |
sam_grove |
0:6219434a0cb5 | 750 | ** @return always 0 |
sam_grove |
0:6219434a0cb5 | 751 | **/ |
sam_grove |
0:6219434a0cb5 | 752 | |
sam_grove |
0:6219434a0cb5 | 753 | UNS32 |
sam_grove |
0:6219434a0cb5 | 754 | TPDO_Communication_Parameter_Callback (CO_Data * d, |
sam_grove |
0:6219434a0cb5 | 755 | const indextable * OD_entry, |
sam_grove |
0:6219434a0cb5 | 756 | UNS8 bSubindex) |
sam_grove |
0:6219434a0cb5 | 757 | { |
sam_grove |
0:6219434a0cb5 | 758 | /* If PDO are actives */ |
sam_grove |
0:6219434a0cb5 | 759 | if (d->CurrentCommunicationState.csPDO) |
sam_grove |
0:6219434a0cb5 | 760 | switch (bSubindex) |
sam_grove |
0:6219434a0cb5 | 761 | { |
sam_grove |
0:6219434a0cb5 | 762 | case 2: /* Changed transmition type */ |
sam_grove |
0:6219434a0cb5 | 763 | case 3: /* Changed inhibit time */ |
sam_grove |
0:6219434a0cb5 | 764 | case 5: /* Changed event time */ |
sam_grove |
0:6219434a0cb5 | 765 | { |
sam_grove |
0:6219434a0cb5 | 766 | const indextable *TPDO_com = d->objdict + d->firstIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 767 | UNS8 numPdo = (UNS8) (OD_entry - TPDO_com); /* number of the actual processed pdo-nr. */ |
sam_grove |
0:6219434a0cb5 | 768 | |
sam_grove |
0:6219434a0cb5 | 769 | /* Zap all timers and inhibit flag */ |
sam_grove |
0:6219434a0cb5 | 770 | d->PDO_status[numPdo].event_timer = |
sam_grove |
0:6219434a0cb5 | 771 | DelAlarm (d->PDO_status[numPdo].event_timer); |
sam_grove |
0:6219434a0cb5 | 772 | d->PDO_status[numPdo].inhibit_timer = |
sam_grove |
0:6219434a0cb5 | 773 | DelAlarm (d->PDO_status[numPdo].inhibit_timer); |
sam_grove |
0:6219434a0cb5 | 774 | d->PDO_status[numPdo].transmit_type_parameter = 0; |
sam_grove |
0:6219434a0cb5 | 775 | /* Call PDOEventTimerAlarm for this TPDO, this will trigger emission et reset timers */ |
sam_grove |
0:6219434a0cb5 | 776 | PDOEventTimerAlarm (d, numPdo); |
sam_grove |
0:6219434a0cb5 | 777 | return 0; |
sam_grove |
0:6219434a0cb5 | 778 | } |
sam_grove |
0:6219434a0cb5 | 779 | |
sam_grove |
0:6219434a0cb5 | 780 | default: /* other subindex are ignored */ |
sam_grove |
0:6219434a0cb5 | 781 | break; |
sam_grove |
0:6219434a0cb5 | 782 | } |
sam_grove |
0:6219434a0cb5 | 783 | return 0; |
sam_grove |
0:6219434a0cb5 | 784 | } |
sam_grove |
0:6219434a0cb5 | 785 | |
sam_grove |
0:6219434a0cb5 | 786 | void |
sam_grove |
0:6219434a0cb5 | 787 | PDOInit (CO_Data * d) |
sam_grove |
0:6219434a0cb5 | 788 | { |
sam_grove |
0:6219434a0cb5 | 789 | /* For each TPDO mapping parameters */ |
sam_grove |
0:6219434a0cb5 | 790 | UNS16 pdoIndex = 0x1800; /* OD index of TDPO */ |
sam_grove |
0:6219434a0cb5 | 791 | |
sam_grove |
0:6219434a0cb5 | 792 | UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 793 | UNS16 lastIndex = d->lastIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 794 | if (offsetObjdict) |
sam_grove |
0:6219434a0cb5 | 795 | while (offsetObjdict <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 796 | { |
sam_grove |
0:6219434a0cb5 | 797 | /* Assign callbacks to sensible TPDO mapping subindexes */ |
sam_grove |
0:6219434a0cb5 | 798 | UNS32 errorCode; |
sam_grove |
0:6219434a0cb5 | 799 | ODCallback_t *CallbackList; |
sam_grove |
0:6219434a0cb5 | 800 | /* Find callback list */ |
sam_grove |
0:6219434a0cb5 | 801 | scanIndexOD (d, pdoIndex, &errorCode, &CallbackList); |
sam_grove |
0:6219434a0cb5 | 802 | if (errorCode == OD_SUCCESSFUL && CallbackList) |
sam_grove |
0:6219434a0cb5 | 803 | { |
sam_grove |
0:6219434a0cb5 | 804 | /*Assign callbacks to corresponding subindex */ |
sam_grove |
0:6219434a0cb5 | 805 | /* Transmission type */ |
sam_grove |
0:6219434a0cb5 | 806 | CallbackList[2] = &TPDO_Communication_Parameter_Callback; |
sam_grove |
0:6219434a0cb5 | 807 | /* Inhibit time */ |
sam_grove |
0:6219434a0cb5 | 808 | CallbackList[3] = &TPDO_Communication_Parameter_Callback; |
sam_grove |
0:6219434a0cb5 | 809 | /* Event timer */ |
sam_grove |
0:6219434a0cb5 | 810 | CallbackList[5] = &TPDO_Communication_Parameter_Callback; |
sam_grove |
0:6219434a0cb5 | 811 | } |
sam_grove |
0:6219434a0cb5 | 812 | pdoIndex++; |
sam_grove |
0:6219434a0cb5 | 813 | offsetObjdict++; |
sam_grove |
0:6219434a0cb5 | 814 | } |
sam_grove |
0:6219434a0cb5 | 815 | |
sam_grove |
0:6219434a0cb5 | 816 | /* Trigger a non-sync event */ |
sam_grove |
0:6219434a0cb5 | 817 | _sendPDOevent (d, 0); |
sam_grove |
0:6219434a0cb5 | 818 | } |
sam_grove |
0:6219434a0cb5 | 819 | |
sam_grove |
0:6219434a0cb5 | 820 | void |
sam_grove |
0:6219434a0cb5 | 821 | PDOStop (CO_Data * d) |
sam_grove |
0:6219434a0cb5 | 822 | { |
sam_grove |
0:6219434a0cb5 | 823 | /* For each TPDO mapping parameters */ |
sam_grove |
0:6219434a0cb5 | 824 | UNS8 pdoNum = 0x00; /* number of the actual processed pdo-nr. */ |
sam_grove |
0:6219434a0cb5 | 825 | UNS16 offsetObjdict = d->firstIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 826 | UNS16 lastIndex = d->lastIndex->PDO_TRS; |
sam_grove |
0:6219434a0cb5 | 827 | if (offsetObjdict) |
sam_grove |
0:6219434a0cb5 | 828 | while (offsetObjdict <= lastIndex) |
sam_grove |
0:6219434a0cb5 | 829 | { |
sam_grove |
0:6219434a0cb5 | 830 | /* Delete TPDO timers */ |
sam_grove |
0:6219434a0cb5 | 831 | d->PDO_status[pdoNum].event_timer = |
sam_grove |
0:6219434a0cb5 | 832 | DelAlarm (d->PDO_status[pdoNum].event_timer); |
sam_grove |
0:6219434a0cb5 | 833 | d->PDO_status[pdoNum].inhibit_timer = |
sam_grove |
0:6219434a0cb5 | 834 | DelAlarm (d->PDO_status[pdoNum].inhibit_timer); |
sam_grove |
0:6219434a0cb5 | 835 | /* Reset transmit type parameter */ |
sam_grove |
0:6219434a0cb5 | 836 | d->PDO_status[pdoNum].transmit_type_parameter = 0; |
sam_grove |
0:6219434a0cb5 | 837 | d->PDO_status[pdoNum].last_message.cob_id = 0; |
sam_grove |
0:6219434a0cb5 | 838 | pdoNum++; |
sam_grove |
0:6219434a0cb5 | 839 | offsetObjdict++; |
sam_grove |
0:6219434a0cb5 | 840 | } |
sam_grove |
0:6219434a0cb5 | 841 | } |