Sam Grove / Mbed 2 deprecated canopen_slavenode

CANfestival - an open source CANopen framework

Dependencies:   mbed

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?

UserRevisionLine numberNew 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&#65533;", 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 }