// this is called by the eeprom init task
bool eeprom_init_done(semaphore_t *worker_task)
{
switch(init_state)
{
case config_can :
if(can_config(memid_nodeid, worker_task))
init_state = config_serial;
break;
case config_serial :
// TODO: read other config values that are set....
uart_config.rate = get_parameter_type_1(memid_baudrate)->value.SHORT;
init_deque(&uart_config.rx_queue, RX_BUFFER_LENGTH, rx_queue, NUM_RX_BUFFERS);
init_uart(&uart_config, rx_stack, numelements(rx_stack), tx_stack, numelements(tx_stack));
signal(worker_task);
init_state = config_complete;
break;
default :
return true; // init is done
}
return false; // more work
}
LONG cop_request(LONG cob_id, SHORT *length, BYTE *data)
{
WORD timeout[9] = {2,2,2,2,2,2,5,10,20};
BYTE dlc = (BYTE)*length;
// 1. Configure receive message object with remote request
if((cop_error = can_config(CANBUF_RX, cob_id, (WORD)CAN_REQUEST(dlc))) != CANERR_NOERROR) {
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Start timer (increased time-out for RTR-frames)
can_start_timer((WORD)(timeout[cop_baudrate] * CANRTR_FACTOR));
// 3. Wait until message is received
do {
// Return if a message is received, or an error occurred
if((cop_error = can_receive(CANBUF_RX, length, data)) != COPERR_RX_EMPTY) {
can_delete(CANBUF_RX);
return cop_error;
}
} while(!can_is_timeout());
// 4. A time-out occurred!
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
LONG cop_transmit(LONG cob_id, SHORT length, BYTE *data)
{
// 1. Configure transmit message object
if((cop_error = can_config(CANBUF_TX, cob_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Transmit the message
if((cop_error = can_transmit(CANBUF_TX, length, data)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// Return if the message is transmitted
can_delete(CANBUF_TX);
return cop_error;
}
static LONG sdo_receive(BYTE node_id, WORD index, BYTE subindex, SHORT *length, BYTE *data, SHORT max)
{
short n; // data length code
short rc; // return value
short t = 0; // toggle bit
// --- Initiate SDO Upload ---
cop_buffer[0] = 0x40; // client command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = 0x00; // reserved: set to 00h
cop_buffer[5] = 0x00; // -"-
cop_buffer[6] = 0x00; // -"-
cop_buffer[7] = 0x00; // -"-
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
*length = 0; // no data received!
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x40) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x02) == 0x02) { // expedited transfer?
if((cop_buffer[0] & 0x01) == 0x01)
n = 4 - (short)((cop_buffer[0] & 0x0C) >> 2);
else
n = 4;
memcpy(data, &cop_buffer[4], n < max? n : max);
*length = n < max? n : max; // data received!!!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
break;
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR); // segmented transfer:
// --- Upload SDO Segment ---
for(*length = 0;;)
{
cop_buffer[0] = 0x60 | t; // client command specifier
cop_buffer[1] = 0x00; // reserved: set to 00h
cop_buffer[2] = 0x00; // -"-
cop_buffer[3] = 0x00; // -"-
cop_buffer[4] = 0x00; // -"-
cop_buffer[5] = 0x00; // -"-
cop_buffer[6] = 0x00; // -"-
cop_buffer[7] = 0x00; // -"-
n = 8; // 8 bytes to transmit!
// 5. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 6. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 7. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[0] & 0xE0) == 0x80) {// SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
*length = 0; // no data received!
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x00) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered?
cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x0E) != 0x00) // number of segment data bytes
n = 7 - (int)((cop_buffer[0] & 0x0E) >> 1);
else
n = 7;
if(max - *length > 0) // copy segment data if space
memcpy(&data[*length], &cop_buffer[1], *length + n < max? n : max - *length);
*length += n;
if((cop_buffer[0] & 0x01) == 0x01) { // no more segments?
if(*length > max)
*length = max; // truncate to buffer size!
if(*length < max)
data[*length] = '\0'; // for zero-closed strings!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
break;
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
*length = 0; // no data received!
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
t = t? 0x00 : 0x10; // alternate toggle bit!
}
}
static LONG sdo_segmented(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data)
{
short n, i; // data length code
short rc; // return value
short t = 0; // toggle bit
// --- Initiate SDO Download ---
cop_buffer[0] = (BYTE)0x21; // client command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOBYTE(length); // number of data bytes (LSB)
cop_buffer[5] = HIBYTE(length); // -"-
cop_buffer[6] = (BYTE)0x00; // -"-
cop_buffer[7] = (BYTE)0x00; // number of data bytes (MSB)
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
// --- Download SDO Segment ---
for(i = 0;;)
{
if(length <= 7) // bytes that does not contain data
n = 7 - length;
else // no segment size indicated
n = 0;
cop_buffer[0] = (BYTE)(n << 1); // client command specifier
memset(&cop_buffer[1], 0x00, 7);// clear data buffer
memcpy(&cop_buffer[1], &data[i], 7 - n);// copy segment data
length -= 7 - n; // remaining number of bytes
i += 7 - n; // index to remainung bytes
cop_buffer[0]|= length? 0x00 : 0x01;// last segment to transmit
cop_buffer[0]|= t; // toggle bit
n = 8; // 8 bytes to transmit!
// 5. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 6. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 7. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[0] & 0xE0) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xE0) != 0x20) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if((cop_buffer[0] & 0x10) != t) { // toggle bit not altered?
cop_error = SDOERR_WRONG_TOGGLEBIT; // abort: toggle bit not altered
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
if(length == 0) { // all data tranmitted?
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(rc != CANERR_NOERROR);
t = t? 0x00 : 0x10; // alternate toggle bit!
}
}
static LONG sdo_expedited(BYTE node_id, WORD index, BYTE subindex, SHORT length, BYTE *data)
{
short n; // data length code
short rc; // return value
// --- Expedited SDO Download ---
cop_buffer[0] = (BYTE)0x23; // client command specifier
cop_buffer[0] |= (BYTE)((4 - length) << 2);
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
memset(&cop_buffer[4],0x00,4); // clear data buffer
memcpy(&cop_buffer[4],data,length); // copy data bytes
n = 8; // 8 bytes to transmit!
// 1. Configure transmit message object for client SDO
if((cop_error = can_config(CANBUF_TX, SDO_CLIENT + node_id, CANMSG_TRANSMIT)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
return cop_error;
}
// 2. Configure receive message object for server SDO
if((cop_error = can_config(CANBUF_RX, SDO_SERVER + node_id, CANMSG_RECEIVE)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 2. Transmit the client SDO message
if((cop_error = can_transmit(CANBUF_TX, n, cop_buffer)) != CANERR_NOERROR) {
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
// 3. Start timer for SDO time-out
can_start_timer(cop_timeout);
// 4. Wait until server message is received
do {
switch((rc = can_receive(CANBUF_RX, &n, cop_buffer)))
{
case CANERR_NOERROR: // confirmation:
if(n != 8) { // 8 bytes received?
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_LENGTH;
}
if((cop_buffer[1] != LOBYTE(index)) || // multiplexor? index (LSB)
(cop_buffer[2] != HIBYTE(index)) || // index (MSB)
(cop_buffer[3] != (BYTE)(subindex))) { // subindex
rc = COPERR_FORMAT;
break;
}
if((cop_buffer[0] & 0xFF) == 0x80) { // SDO abort received?
LOLOBYTE(cop_error) = cop_buffer[4]; // abort code (LSB)
LOHIBYTE(cop_error) = cop_buffer[5]; // -"-
HILOBYTE(cop_error) = cop_buffer[6]; // -"-
HIHIBYTE(cop_error) = cop_buffer[7]; // abort code (MSB)
// Return value is abort code!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error;
}
if((cop_buffer[0] & 0xFF) != 0x60) { // unknown command specifier?
cop_error = SDOERR_UNKNOWN_SPECIFIER; // abort: unknown command specifier
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_FORMAT;
}
else { // success: data written!
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_NOERROR;
}
case CANERR_RX_EMPTY: // receiver empty:
if(can_is_timeout()) { // time-out occurred?
cop_error = SDOERR_PROTOCOL_TIMEOUT; // abort: time-out
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = COPERR_TIMEOUT;
}
break;
default: // other errors:
cop_error = SDOERR_GENERAL_ERROR; // abort: general error
cop_buffer[0] = 0x80; // command specifier
cop_buffer[1] = LOBYTE(index); // multiplexor: index (LSB)
cop_buffer[2] = HIBYTE(index); // index (MSB)
cop_buffer[3] = (BYTE)(subindex); // subindex
cop_buffer[4] = LOLOBYTE(cop_error); // abort code (LSB)
cop_buffer[5] = LOHIBYTE(cop_error); // -"-
cop_buffer[6] = HILOBYTE(cop_error); // -"-
cop_buffer[7] = HIHIBYTE(cop_error); // abort code (MSB)
// Transmit SDO abort and return
can_transmit(CANBUF_TX, 8, cop_buffer);
can_delete(CANBUF_TX);
can_delete(CANBUF_RX);
return cop_error = rc;
}
} while(1); // "the torture never stops!"
}
