Luis Silvio Cordova Rivadeneira / Mbed 2 deprecated Can_open_masternode

Can_open_masternode

Dependencies:   mbed

framework/src/pdo.c@7:537bae5a6fc6, 2012-09-26 (annotated)

Committer:
sam_grove
Date:
Wed Sep 26 05:48:14 2012 +0000
Revision:
7:537bae5a6fc6
Parent:
0:9dd7c6129683 
Pushing the project into the new repo

Who changed what in which revision?

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